Robot position calculation - using simplified Filter-Smoother

#1

There is an existing thread on Robot position calculation started by PixelToast that keeps many of us playing a guessing game of what his, yet unreleased code, will look like.

From other threads on this subject you could find out how this type of problems are solved in the “real” world:

If I try to follow the links and learn about Kalman Filter or Estimation Theory I see a lot of math which goes over the top my head if I was an average middle schooler.

So, the goal of this thread is to explain estimation theory as applicable to the NbN robot navigation without using any advanced math. For this purpose I will pretend that my math skills are of the 8th grade level (they really are). If we have to learn something more advanced we will try, but otherwise we will make it as simple as possible.

When you learn geometry everything is perfect. However, in the real life there are noise and errors. Since we are dealing with estimation theory don’t be surprised if there will be initial errors in this thread. With enough eyes on the problem we will try to filter out the noise, correct errors, and uncover the truth…

I don’t have the ready answer just yet, the solution will evolve as we go. So ask questions, make corrections, and stay tuned…

2 Likes
V5 Motor Position, Velocity, and Acceleration functions
#2

Filter

The main purpose of a filter is to maintain object state (such as position, orientation and velocity), accept valid measurements (such as encoder and gyro readings) and reject measurements if they are erroneous. For example, if gyro was reporting 1 deg change every measurement interval and then suddenly reports 50 deg change, then there must be some sort of abnormal condition and filter will reject it.

Once filter rejects a measurement you may wish to do various things, depending on your experience with the specific sensor. For example, if you observe that every time your robot bumps into a pile of balls the gyro will jump and then continue tracking rotation rate correctly, you may want to reset gyro angle and start tracking its change after a few measurement cycles. However, if you notice that, after you collide with another robot or a wall, the reading out of gyro are total garbage you may want to start ignoring its readings until gyro goes through a 1.5 sec hardware reset, while the robot is not moving.

Filter will use accepted measurements to update the object state. As was already mentioned in another thread, filter uses weighted coefficients. Essentially, the filter keeps measurement statistics about each sensor to know how much it should be trusted. If one sensor says robot moved to the left and another says it moved to the right then filter will move internal state proportional to the trustworthiness of each sensor.

If you have a group of good sensors and one that keeps reporting erroneous results all the time, the filter will quickly figure out whom to trust and will start ignoring the outliers. However, in the real life it is more complicated. All sensors will have some noise and errors in their measurements, with some being less accurate than the others. As a result, the state out of the filter will be “jumpy”. To prevent that you need to “smooth” the measurements over several intervals and try to model physics more precisely.

For example, you know that your position cannot suddenly jump over the long distance if you are applying small motor power, unless you are being bumped by another robot or one of your wheels loses traction with the ground. In addition to filtering and smoothing of the observations good systems will model a lot of physics and many of the abnormal events. We will try to model, at least, the “normal” conditions and try to detect abnormal events. Then you could flash an indicator light to let operator know that we lost a lock on a good solutions and need some sort of a reset.

Smoother

Had it been senior college level project, I would say that everybody needs to learn math behind optimal Kalman Filter and implement Variable or Fixed Lag Smoother. But we are only in the middle school and this picture from the linked chapter should be enough:

Essentially, it illustrates that smoother defers judgment about trustworthiness of each single observation for several measurement intervals. While filter provides instant preliminary state, the smoother will have more refined state that lags in time, but provides more accurate estimates despite considerable noise in the input measurements.

Let’s get back to our problem:

Our state vector will have: position (x,y), orientation (heading angle), linear velocity (Vx,Vy) and rotational velocity (w) at any given moment of time. State vector contains parameters that we want to actively estimate. In addition to those six variables we will need to keep some information about their error bounds or how much we trust each of those numbers. In the formal filter theory we would need to keep 6x6 (or larger if we have more state variables) covariance matrix, but we will see if we could get away with something smaller and still get useful results.

Our sensors will include four quad encoders - one for each wheel, and a gyro. Potentially, you can include an accelerometer to detect abnormal events, line followers, to detect line crossings, or even additional gyros if you think it will somehow decrease your errors. But I think the latter is overkill - you should be able to do much better with relatively simple gyro error modelling.

For each sensor we will keep some measurement statistics, which lets us estimate how much we trust that particular sensor, how much error the sensor has accumulated so far, how much it drifts over time, and anything else we might need to model that sensor. If those numbers change over time we add them to our state and actively estimate, otherwise we collect calibration parameters for each sensor and then keep them as constants in our code. However, you may need to re-calibrate those sensors once in while.

Finally, in order to model your system you will need to input requested motor power, current battery voltage and approximate robot mass and inertia of the drive train components. Read down below and you will understand why.

Prediction Step

The way Filter-Smoother pair works is that at a given moment of time you know your current state and try to predict your next state at the next measurement time. For instance, if you know what power level was applied to each motor, you will know approximate force produced by each motor and, after considering drivetrain inertia and robot mass, you will know direction and magnitude of the acceleration vector. Acceleration is the rate of change of your velocity. Knowing current velocity and acceleration you can predict your velocity at the end of prediction time interval. You apply similar logic to both linear and rotational velocities and at the end could predict the new position and rotation angle (around center of mass) at the final time. Once you know that you can predict what reading each of the quad encoders and gyro will have at the next meas time.

If you have line follower sensors you can predict when line crossing event will occur and by the time difference of when it actually happens you can correct your state estimates.

Residuals

The difference between predicted and observer sensor value is a residual. If all residuals are zeros we know that we have perfectly modeled state. If residuals are non-zero we need to correct our state. We use our sensor trustworthiness estimation to know which sensors “pull more weight”.

Finally, after the state is corrected, we look back to see how accurate was each of the sensors and recompute their trustworthiness for the next step.

State Vector Update

The trick is to know how much each state parameter should change given that specific sensor produced certain residual. For example, how much (x) and (y) need to be corrected if quad encoder on the left-front wheel missed it’s predicted value by 3 ticks?

Those who’ve been to high school or college could sense that we are talking about Partial derivatives. For the rest of us, I just want to say that there will be relatively simple formulas for each drive train configuration, and knowing basic trigonometry should be sufficient to code them up.

#3

Null Test Case

Null test case or Null hypothesis is always the very first integration test or a series of tests you perform on the system. It is performed with no or minimal external influences and errors.

If you use simulated measurements - you generate them without simulating any errors. If you test an electrical system you first feed it no signal and then signal with no distortions. And if you test a mechanical system you first subject it to no additional loads and see how it behaves.

Contrary to initial thought, null test case is not only an extremely valuable tool for debugging your system, but could produce a large amount of valuable data. We could later use that data to calibrate our system and fine tune performance of the filter.

For our robot, lets fix it on a stand such that wheels do not touch the ground and could freely rotate without shaking the robot.

Null Test 0

You turn on the power. If the lights turn on and there is no smell of the the burnt wire insulation - congratulation you didn’t short any of wires!!!

Null Test 1

Do not touch any controls - just observe values out of your sensors and battery voltage. The expectation is that quad encoders and voltage remains steady while gyro may slightly drift. If you log the results of the gyro change rate for several minutes you will be able to use to initialize your error bounds later.

One would expect that unfiltered VEX gyroscope measurements would be dominated by temperature and voltage drifts followed by random walk. However, system libraries apply initial filtering for us based on 1.5 sec sampling interval for constant drift. So, I would say, we are only interested in establishing max error bounds over 5-10 min interval at this point.

Null Test 2

Run the motors in random directions, observe the gyro readings. Expectation is that it will establish larger gyro error bounds under heavy, but still normal load.

Null Test 3

Run all motors with 50% and then in another test with 100% power. Log quad encoder readings. Initial ramp up time will depend on the inertia of the drive train components. Speed and distance readings during steady operation may indicate that motors run differently even without load. You may need to clean motors, lubricate the gears, or replace motor controllers. For the competing robot you want to have matched motor characteristics, unless your strategy is sitting in the home base all the time.

Can anybody think of other null test cases?

#4

Wow, really helpful. Thanks :slight_smile:

#5

this is awesome and thanks for sharing, knowing the theory and reason behind all the math really helps make sense of it, I hope you will continue this tutorial for us

just one question, how would one log, in code all the errors established from the null cases, would these be logged once in a test, or would this be test you run every time, maybe in the pre-auton code?

where would you store this info and how?

#6

The easiest way is to write the log to console output or to the debug stream. The latest version of Robot C conveniently allows to save debug stream into a file, which could later be loaded into another application.

To trigger debugging I use orange jumper clip. You can check if it is inserted into one of the input ports and either start dedicated debugging function or write debugging info on every cycle. This way you don’t have to re-download the program when you need to do debugging.

However, you would rarely be interested in raw measurements data. It is only useful if you are starting to develop your system or need to debug some nasty problem.

Most of the time all you need to know is statistical summary like median, mean, and standard deviation. I would usually accumulate running total of sample count, sum, and sum of squares and output them on regular intervals, like once every few seconds. This lets you compute everything you need without clogging your debug stream.

An important note is that this trick is going to work well for measurements that are expected to be hovering around some fixed value or slowly drift. For example, in the above null test cases we expect gyro reading to be around zero or very slowly drifting.

You will quickly realize that in Null Test 3 the distance reported by quad encoders will keep growing. In the ideal case, measurements out of all four quads will be the proportional to motor speed as a function of commanded motor power multiplied by the time you let the motors run.

In fact, if you make a function or a table that will return expected motor speed as a function of commanded power and, maybe, battery voltage then:

  [INDENT]f(commanded power) * time - actual quad measurement => residual[/INDENT]

If both your modeling function is accurate and actual motors are in good shape then your residuals will statistically look very good: mean = ~0 and standard deviation will be small.

Now you will think: OMG! that table needs to be humongous and/or function needs to be complex in order to take care of both motor curves, unknown loads, and dependency on battery power.

The good news is that we don’t have to do all that. The filter will gladly estimate for us any slowly changing parameter as long as we do it right.

For example, let say you add an estimated parameter (K) to your state vector that is being used to compute magnitude of the robot acceleration as the function of commanded motor power §:

          [INDENT]a = K * P.[/INDENT]

Notice that K takes into account initial robot mass and motor force coefficient as the function of battery voltage. If a ball gets stuck in a robot or battery voltage drops, filter will automatically correct the value of K for us in a few measurement cycles. After several trial runs we will even know good initial value for K.

There are two important caveats however. First, I blindly assumed that relationship between a, K, and P is linear. It could be higher degree polynomial a = k1 * P^2 + k2 * P with two coefficients or have some other tricky formula a = f(P,k1,k2,…). In order for the filter to work we really need to get the formula right either by analysis or by experimentation.

Second, the above example is only applicable to a single motor pulling entire robot of a certain mass. If we have multiple motors we need to look how load divides between them and how power divides between linear and angular accelerations. The beauty of the filter is once we get those relationships right it will help us estimate all unknown coefficients.

So, here is the homework: build a simple test robot that could drive forward.

  1. Run motors with no load (wheels up in the air) at different commanded power levels (only compensating motor friction and heat loss).

  2. Run the robot on the flat surface (initial acceleration and then friction).

  3. Repeat the test running robot up to a 10, 20, and 30 deg slopes to simulate different loads.

Let see if we get linear relationships between commanded power, speed, and acceleration.

#7

I will totally do this, I will have to come up with a way to log acceleration (without an accelerometer cause we know those don’t work well) speed should not be too bad, should I take average acceleration over the entire period of acceleration, and the average of speed over a large time frame(call it 10 seconds) or should I make a really large table of values taken every 20/th of a second, the fist option would be far easier for me to make/

#8

Extremely nice write-up. Estimation theory and kalman filters are definitely difficult subjects to understand and this describes them perfectly without getting lost in the math. Its hard to find such a well written description, even at a collegiate level. Well done!

There is one thing I noticed, but it may have been out of the scope of this piece. The problem of tracking the absolute positition a robot via relative measurements is inherently non-linear. In this case, the absolute x and y velocities are a function of the relative velocity multiplied with a trig function of the heading. Kalman filters require that the equation of motion be linear. This could be resolved by taking a linear estimate of the trig functions, but this probably isn’t good enough for our purpose. Normally you resolve this with an extended kalman filter (EKF). It isn’t much different than a normal kalman filter and requires just a few easy partial derivatives (assuming a discrete time implimentation). I’m not sure by how much the results would differ if this is just ignored vs. linear estimate vs. using an EKF.

#9

Yes, that would be correct, we don’t want to use accelerometers. We need to compute acceleration as the rate of increase of the speed as measured by quad encoders. There might be some wheel slippage at high power level, but we just ignore it for now.

I would try to take quad measurements every 10 msec, but that may be subject to round-off errors. If you could get a moving average over several intervals that would be great. For example, compute speed every 10 msec by dividing (quad value now - quad value 50 msec earlier) by 50 msec. Similarly you could measure acceleration by the changes in the value of speed.

Another important consideration is to time-stamp each measurement with nSysTime instead of counting wait delays between measurement cycles. This will reduce round-off errors as well.

#10

You are right. If we were building a system to provide submillimeter accuracy over hundred of meters of travel distance then, yes, we would need to worry about non-linear state transitions and implement a full blown EKF. However, I would like to avoid matrix multiplications and inversions that EKF requires. We will try do some sort of partial derivatives for folding back state corrections, as long as it doesn’t become too complex. So even if we get something working it could not be called a Kalman Filter anyways, since we are going to cut many corners.

My hope is that we have enough error budget to spare so that even approximate implementation will give us enough accuracy for hitting high goal after 20-30 feet of travel distance.

Consider the following example:

https://vexforum.com/attachment.php?attachmentid=9608&d=1443823646

A two-wheel robot is commanded to make a turn with P1 and P2 applied to its motors. After a period of time we will read quad encoders and will try to figure out where it is.

If you only consider position and heading you will realize that it is not trivial to derive an analytic formula that will compute new position based on quad readings (see dotted line). In addition to (x,y) and heading angle you would need to use at least first derivative (velocity) and second derivative (acceleration) to describe the accurate motion of the robot.

Thankfully, concepts of velocity and acceleration are easy to grasp and are taught in the high school. Also, predicting final position is much simpler task, than answering the reverse question of where we are given the final quad readings.

In the picture above the commanded motor power P1 and P2 translates into force and torque applied to robot’s center of gravity. That results in forward acceleration (a) and angular acceleration (alpha). Our algorithm predicted that robot travels to PREDICTED position while it actually traveled to position denoted as TRUTH. Our algorithm is, clearly, too optimistic. It had predicted robot traveling further and making sharper turn, probably, underestimating energy losses due to friction and sliding of the wheels.

The greenish line denotes actual readings of the quad encoders mapped onto the modeled path. The blue line represents residuals corresponding to the distance that our prediction overshoot the actual quad readings. After we correct the state the red lines denote the error between true state and our final state. Those errors are unobservable to our estimator.

If we had a gyro or an extra quad encoder mounted perpendicular to those we had we could correct for the most of the red error and produce a much better final state. The reason for that is that our robot has three degrees of freedom (x,y) and heading angle and we need three geometrically independent measurement sources to account for them.

Another observation is that we don’t assume lineriarity when we make path prediction but we do assume it when we apply state corrections back to the velocity (v), angular velocity (w), and any other estimated parameters. For simplicity you can think about it like this: we don’t model our path as a straight line, but we assume corrected (blue) portion of the path to be a straight line. This assumption will introduce some errors but we can live with them. Those errors are the price we are willing to pay for the much simpler and less computationally intensive filter implementation.
filter1.jpg

#11

Here is a problem to think about:

https://vexforum.com/attachment.php?attachmentid=9614&d=1444006833

Let say you have a turtlebot with two wheels. Each wheel has its own independent motor and a quad encoder.

Can you say where the robot is after a unit of time has passed if the first encoder returns 3 and the second 5?

If you try to think about it you will realize that the problem has ambiguously many solutions. Now what if 3 and 5 are the speeds of the wheels turning. Can you predict where the robot will be after a unit of time or what its path will be if you just let it run?

(Don’t look at the second attachment just yet - it has a hint. I will explain it in a later post)
filter_posEst1.jpg
filter_posEst2.jpg

V5 and the Gyro sensor
#12

it seems to me like you would need more information, you would need to know, for example the distance between the wheels, or you could not know this distance between the wheels, if you instead could get angle readings from a gyro

EDIT:
oops you did give a distance between the wheels, so yes, using law of cosines you could calculate this, now let me go away, do some math and come back to you

#13

so the robot is moving along a circle here, but the inner wheel (the one that returns 3) is moving along a smaller circle than the outer wheel, knowing the distance between the wheels, given these values we can determine the the radius of the circle each wheel is moving on

so the equation for the radius of the larger circle is
WheelDistance/((largervalue/smallerValue)-1)
or 1/((5/3)-1) or 1.5
and that means the radius of the smaller circle is .5
now we must calculate how many degrees around the circle we have gone, this will be the same for both circles

DegreesTraveled = (SensorValue/(2RPI))*360 which in this case is 191 degrees, given this we can calculate the arc straight line distance around our circle

originally I did law of cosines for this, but since have found an easier way, we can multiply our curved distance (in this case 5) by a value to get the straight distance, and the value is determined by the degrees around the arc, lets call this value P, and lets call the degrees around the arc from the last step A
P=SIN(A/2)/(PI*(A/360);

then P * Value (in our case 5) = straight line distance we moved, and the angle of that line is the starting angle(in our case 90) + the ending angle (in our case 90+191) /divided by two

so when we solve for that, our left wheel moved 1.659 units at 185.5 degrees, and some trig could give us all the answers we needed, if it was speed, and we would say, it went that number of units every second, you could calculate that out for however long a time frame you wanted and preform the same calculations listed above, so if you went for 10 seconds you values would be 30 and 50

#14

Collin_Stiers, thank you for doing the homework.

Here is my version of the solution:

https://vexforum.com/attachment.php?attachmentid=9615&d=1444006862

Once you think about it it will be obvious that the robot will travel in circles. During an interval of time (t) it will sweep a sector corresponding to an angle (theta) and a simple equation will let you solve for ®.

And, as Collin correctly pointed out, once you know ® a simple trigonometry will let you determine changes to you x,y, and heading angle without any ambiguity.

As you can see, switching to the first derivative (velocity) from just considering encoder readings makes it much simpler to even think about robot navigation.

However, in order to accurately compute speed you need to avoid any round off errors.

Who can tell me how many timer ticks (at the highest timer resolution) do you have per single quad encoder change when your robot travels at the highest speed?

#15

You mean like 2.778 ms per encoder tick assuming 100% system efficiency?

#16

Yes, that sounds about right. According to RobotC help quad encoder reports 360 ticks per revolution. The highest rate you could get with direct drive would be max speed of 240 RPM with turbo gears which will correspond to 1440 ticks per second. If we assume the slowest rate to be 393 default gears runnning at less than half nominal speed at 40 RPM then we get 240 ticks per second.

Cortex timer resolution is 1 msec (1000 ticks per second). This means that we could expect to have somewhere between {1.5 encoder ticks / per 1 timer tick} and {4 timer tics per / 1 encoder tick}.

Every time you read an encoder or nSysTime value you could have caught it any time during the tick. It could be .01 or .99 and you wouldn’t be able to tell the difference. So, without any special processing, you are looking at +/- 0.5 tick error on average. For example if we are slow moving (2.5 quad ticks / 10 msec) and are reading values on 10 msec wait cycles than we are looking at up to 20% error in quad reading (0.5/2.5) and 5% error in timer reading (0.5/10.0). This puts total expected error at up to 25%!

There are several things we could do. One is simply to increase measurement cycle (25 quad ticks / 100 msec) will be much better but will still give us close to 5% of cumulative error.

We, really, want to model our robot movements at the fine intervals while keeping errors at minimum. This presents a conflict and an interesting engineering problem to solve. There are several methods we could use.

First trick is to create a high priority measurement task using hogCPU() function. Once again, I blindly assume that Cortex runs system code which updates sensor values on the separate from user code CPU. Somebody needs to test if this actually works :slight_smile:

If we know that even the least frequent encoder tick comes every 4 msec we could hogCPU() and then start a tight loop running up a counter and reading nSysTime and our encoder values. When we sense an encoder change we record counter value when it happened. After all encoders, that we expect would change, do change we run up to the next timer tick and stop. Now we know how many counter increments there were during observed timer ticks and we can compute fractional measurement timestamps. Finally, we releaseCPU().

If we want to get fancy and count even slower changing sensors than, based on the current change rate, we predict when the next sensor change will occur and insert appropriate delay() cycles into the task to wake one timer tick before sensor is expected to change again. This technique is not unusual for embedded platforms. I tried to search for an existing VEX library, but didn’t find any. If anyone knows of one, please, speak up.

Second trick / method is to smooth the readings over several measurement intervals using running averages, as I described in one of the earlier posts. This method depends on the type of the measurement you are trying to process. As an independent preprocessor it makes sense only for slow(er) changing parameters. If you are using Filter-Smoother pair then for best result it needs to be tightly integrated with modeling underlying physics of the process.

Finally, if we use Fixed or Variable Lag Smoother, then the prediction and final (and most accurate) state correction will be done over multiple lag intervals which will significantly reduce any round-off errors. Note the smoother lag could be seconds, but we still get all the benefits of tracking intermediate robot path with filter while updating filter state on shorter measurement intervals.

#17

If you could precisely measure true robot speed at short enough intervals you could accurately plot its path using nothing but a good measurement task.

However, robots turn and accelerate causing wheels to slip and some of the sensors (like gyro) are noisier than others and quickly accumulate errors. To address those issues we intend to employ filtering and it needs an accurate physics modeled to a higher degree than just velocity. We need to model accelerations and at least some forces that cause them.

The most important forces to consider are: torque generated by the motors, friction from the floor tiles and friction inside drive-train components. The last but not least force acting upon the robot is gravity. What may feel to the robot as high friction when moving is actually caused by the robot wheels creating indentations in the soft floor tiles and then fighting the force of gravity to climb out of those “wells” as it moves.

So what do you need to know?

  1. Inertia: Robot has mass and rotating drive-train components have moment of inertia - it takes force and time to pump in some kinetic energy to accelerate them.

  2. Friction: It takes a bit more force to start, but once drivet-rain components are moving the force of friction remain constant and does not depend on velocity. However, the energy spent on friction does depend on the distance which itself is a product of speed and time. Only at higher speeds friction force may became non-linear function of speed. For example, air drag at high velocity.

  3. Friction between tires and floor tiles is, actually, a good thing. Traction control systems take this force into consideration to balance motor power and ensure that none of the wheels slip. We need, at least, to be aware of it in order to detect any slips.

#18

I built my self a test robot today, and so will do your Hw assignment, by measuring free speed of the wheels on my bot, then with the bot on level surface, and on slants, I will try to measure speed, and acceleration for different situations

#19

That’s great! I could draw pretty pictures, cite online help, and reference any Wikipedia article, but with micro-controllers and electro-mechanical systems the theory is only right when the real hardware behaves as expected in the field.

There are way too many examples, when things happen that nobody expected.

#20

There aren’t many advanced math or physics topics you need to know to read this thread. However, few things are important and we need to make sure that everybody is on the same page before we move on to the motor inner-workings.

Mechanics

First of all, I am sure everybody knows that velocity=distance/time (v = S/t) or, as some would say, instantaneous velocity it is the rate of change of position (in time).

Second, you need to have a solid understanding of acceleration, which is the rate of change of velocity. You need to know what F=m*a means and why it is so important.

You, also, need to know that moving object possesses kinetic energy which proportional to velocity squared E = (m*v^2)/2.

In addition to linear velocity, acceleration, force, mass, and energy you need to know about their angular counterparts such as angular velocity, angular acceleration, torque, moment of inertia, and rotational energy of a flywheel.

Finally, everybody should be able to tell the difference between Force, Power, and Energy. Force is a static phenomena - there is no energy expanded if something just sits there pushing with force on something else. However, once you start moving something while employing a force you spending energy which is proportional to the force * distance. The power is the rate at which you spend that energy. Power is proportional to the force*velocity.

Electromagnetism

There are several simple, yet important laws that you need to know:

Ohm’s law states that current is proportional to the applied voltage I=V/R, where I is electric current, V is voltage or as some say potential differential, and R is the resistance of the conductor through which the current flows. When the current flows electric energy turns to heat at a rate proportional to P=V*I

When conductor (wire) is placed inside magnetic field it will experience the force proportional to the current. This is the basic principle of how electric motors convert electric energy into mechanical energy (force doing some work over some distance).

When conductor (wire) is moved through the magnetic fields it generates electromotive force Vemf or voltage differential. This is the main principle of operation of electric generators that convert mechanical energy to electric energy.

Analogy

Making analogy with mechanics. Voltage is equivalent to force and current is equivalent to velocity. The work is only being done (energy spent) when something moves or current flows. The power is the energy spent over a unit of time and Power = Force*Velocity = Voltage * Current.

And for those who likes homework, here is a tricky question: what is electrical equivalent of distance?