AVC: Heading and Position Estimation

How does Data Bus, my AVC robot, know where it is and at what heading it's pointing--its pose?

Data Bus uses three primary sensors for pose estimation. Dual wheel odometers provide distance measurement. An L3G4200D MEMS gyro provides heading information. Finally, a Venus638FLPX GPS provides additional heading information. A Kalman Filter fuses the data but there's another trick: lag compensation. First, let's look at the sensors.

Wheel Encoders

If your robot knows what direction it is pointing and how far it traveled, and where it started, it should know where it is. Wheel odometers provide an accurate (measured to < 1% error) distance measurement. The two encoders are located on the rear, driven wheels.

The downside of driven wheel encoders is inaccuracy from wheel slip that occurs during acceleration or braking. I've configured Data Bus to run safely within traction limits.

Gyro Heading Rate

I wanted to find out for myself exactly what gyro error and noise looked like. What I found was the following (and I encourage you to find out for yourself)

• The gyro bias doesn't stabilize on power up, but takes about 5-10 minutes
• Attempts to compute the bias by averaging the gyro signal at rest did not fully remove drift
• Even a small heading drift due to bias miscalculation is a huge problem over a 270m course
• The bias actually tended to change throughout the course possibly due to temperature variations

An absolute reference is needed. For Data Bus, GPS heading provided that absolute reference. Some have better luck with magnetometers but I didn't.

GPS Heading

The Venus GPS does a great deal of filtering and so it cannot properly track quick changes in direction by a 1/10th RC car like Data Bus rounding a corner at over 10 mph. When heading rate is low, the GPS provides a decent absolute reference.

In the last several practice runs and on AVC run 3, I notice some inaccuracies that demand further analysis. I suspect the GPS heading accuracy may suffer depending on which satellites are in view and where they are in the sky.

Kalman Filter Sensor Fusion

The next step is to fuse GPS heading and gyro heading rate. I used a relatively simple Kalman Filter implementation. Kalman filters estimate system signals based on a mathematical model of the system being estimated and using many measurements over time.

In this case, the system is simple. You have heading (theta), and you have heading rate (omega). It's described like this, analogous to position and velocity.

The new heading equals the old heading plus the heading rate times the sample time dt. The heading rate is modeled as:

It's just a model; reality is that the heading rate probably changes, but you can fudge around this in your implementation of the Kalman Filter.  These equations can be put in state-space form, basically a matrix version of the two equations above, which is friendly for most Kalman Filter implementations. You have system state:

where

and

When you multiply this out, you end up with the two state equations above. Then you have outputs (measurements):

where

and

In essence, C maps the measurements to the state variables. The Kalman Filter uses these two equations, and some other matrices.

It tracks not only the estimate of system state, but also the covariance matrix, P, which is basically a measure of how well the state variables and their measurements track together.

It also uses a measurement noise matrix, R, so you can tell the filter how much noise to expect in each of the signals.

Lastly, there's a process noise covariance, Q, used to model noise of the rest of the system, such as potholes, imprecise actuators, etc. The numbers in this matrix can be increased to fudge around errors due to an approximate model of the system state, mentioned previously.

In Octave, the algorithm looks like this:


And that's it. The Kalman Filter used on Data Bus. A two-state, 1st order (because heading rate is modeled as constant) Kalman filter. This is about as simple as it gets. Here's the source: kalman.c

But there's a catch: GPS lag.

GPS Lag Compensation

When the GPS reports a heading, it is actually lagged by a full second. It's reporting the heading from one second ago. So if the robot is making steering changes in realtime based on data that is a full second old, instability results.

If you're familiar with feedback audio amplifier design, you know that anytime you have a phase shift (lag) that is too high, with an amplifier gain that is too high, you get instability--oscillation.

Same thing with a rover that attempts to correct heading error too aggressively (high gain) with a feedback signal that lags the input by too much. You get a robot whose heading and steering oscillate and the robot wanders all over the place. It can never quite 'catch up' with the error.

The solution? Well, you can turn the gain down but that wouldn't help when the robot has to be fast. It'd have to turn very, very slowly.

Instead, find the gyro drift error from a second ago, and use that to correct the real-time gyro.

Calculating Pose In Near Realtime

Let's take a step back and pretend we're operating off gyro alone. At every sample step (every 10ms), we compute the new heading based on the old heading + the heading rate times 0.010 seconds.

hdg += hdgRate*0.010;

Position is calculated based on the prior position, the distance traveled, and the heading.

x += distance * sin(hdg); // eq 1a
y += distance * cos(hdg); // eq 1b

An exercise for the reader: why did I use sin for x and cos for y? There's a good reason. And other ways to do this. At any rate, we now have an estimate for position and heading aka pose.

Except, the heading calculated from gyro alone will drift over time.

How can we find out the drift and correct it?

Correcting Heading

Gyro drift acts slowly. It's on the order of a few hundred degrees per hour, say 0.2°/sec. The drift error from one second ago is very close to the error in the present moment. To correct the current heading calculated from the gyro, just find the drift error from a second ago, using the Kalman Filter.

Let's say our heading error one second ago was +0.1°. We simply subtract 0.1° from the last second's worth of heading calculations. Or some proportion of that error.

To implement this algorithm, save a second's worth of gyro data. Feed gyro heading rate data from one second ago along with GPS heading data into the Kalman Filter and come up with an estimate for heading at t-1.

Compare this estimate to the gyro-based heading calculation from t-1. That gives us our error from a second ago. Now, subtract the error from the last second's worth of saved gyro data.

In short, Data Bus uses the Kalman Filter to remove gyro bias so the gyro can be used in real time.

Correcting Position

But calculating heading isn't quite enough. The heading error generates a position error, too, one that grows quickly with time.

If we save a second's worth of position estimation data, then, after we correct the last second's worth of heading data, we can simply run back through and recalculate x and y from equations 1a and 1b above.

Except that doing so is incredibly slow on a system without floating point support. Yes, even on a 96MHz ARM. I tried it. Fortunately, there's a better way.

If the last 100 positions were off due to heading error of 0.1° we can just rotate all of those points by -0.1°. We rotate the positions around the position from a second ago. We can pre-compute the rotation matrix and then the operation is just a bunch of matrix multiplication which goes much more quickly.

The source code for lag compensation, measurement, and calling the Kalman Filter are found here in updater.c

Results and Future Work

We get the best of both worlds out of this solution. Fairly high dynamic range heading estimates by running off the gyro in real time, very low heading drift by fusing gyro and GPS heading data, and good position estimation.

I have not formally measured yet, but in the half-dozen or more successful runs between Thursday June 14 and Saturday June 16, I estimate that the final cross track error as the robot crossed the finish line was no greater than 1m. Additionally, the robot very consistently traveled the same path on each successful run, so precision appears to be decent.

Accuracy may suffer if extremely rapid heading changes are made because of the large error that will result between gyro and GPS. I made sure the robot executed fairly smooth, sweeping turns. Further accuracy may be gained by addressing this bandwidth mismatch.

As mentioned before, accuracy of the GPS heading is somewhat in question based on data logs from the last several successful runs. Investigation of GPS heading error and its impact on the pose estimation algorithm may lead to a more accurate solution for heading and position estimation, which will likely be an advantage for the 2013 AVC.

Addition of one or more sensors that measure heading or heading rate, along with software to fuse the data with the other sensor data, should further improve the heading estimate.