Combining measurements from the L3G4200D Gyroscope and the MicroMag3 Magnetometer using a Kalman Filter

Next up, I wanted to figure out how to combine the gyroscope and compass to provide a filtered estimate of heading. I did a little web searching and came up with a few relevant places with code:

1. http://www.jayconsystems.com/forum/viewtopic.php?f=22&t=40
2. http://nxttime.wordpress.com/2010/10/06/robotc-code-for-the-kalman-filter/
3. http://stackoverflow.com/questions/6013319/kalman-filter-with-complete-data-set

As a reminder, here is a plot showing the measurements we obtained from the two sensors in the last post on this topic.

which shows the accumulated drift error as well as the noise due to erroneous compass measurements. First, I though I would try just a simple combination approach. We know that the integration of the gyro has less noise but drifts. So at each location we predict what the value of the compass setting should be given the previous setting. If the compass disagrees too much, use the gyroscope solution instead. In other words:

predicted = heading(i-1) + gyro(i) * dt;
difference = abs(predicted - compass(i));
if (difference < X)
  heading(i) = compass(i);
else
  heading(i) = predicted;
endif

There is a sensitivity parameter X that needs to be set based on how much discrepancy between the two sensors is allowed say the compass reading should be rejected. This is not much code so it should run fast on the Arduino. In matlab, here are the results using this approach.

This removed the sharp spikes and got rid of the drift, but there is not much smoothing (the values are still jumping around).  To add some smoothing to the result, we can add an IIR filter on top of the selected result. This is accomplished by simply blending the current estimate with the previous.

predicted = heading(i-1) + gyro(i) * dt;

difference = abs(predicted - compass(i));

if (difference < X)

  heading(i) = 0.1 * compass(i) + 0.9 * heading(i-1);

else

  heading(i) = 0.1 * predicted + 0.9 * heading(i-1);

endif

This smoothing result can be seen in the below figure.

This introduces a slight lag when sharp changes are made and the extremes are also removed (when the servo started spinning in the other direction). Again, this approach is reasonably fast with 8 operations and a branch statement. However, it is somewhat ad hoc. 

A better solution might be a Kalman filter. The Kalman filter will incrementally add in new measurement data but automatically learn the gain term (the blending factor picked as 0.1 in the previous example) and allow a more intuitive setting of a noise model. Using the code given at http://nxttime.wordpress.com/2010/10/06/robotc-code-for-the-kalman-filter/, I get the following result.

This approach used a Kalman filter on the compass parameter and only used the gyroscope to detect when the compass gave an erroneous measurement. The Kalman filter also tracks the covariance estimate in how well the prediction matched the new measurements and the Kalman gain. The gain converged to 0.5549 meaning it would average the prediction and the new measurement.

This approach is somewhat limited based on how it is filtering just a single measurement (the compass) and not modeling the dynamics associated with the gyroscope. I did some more searching and found a few good discussions:

1. http://arduino.cc/forum/index.php?topic=87850.0
2. http://arduino.cc/forum/index.php/topic,58048.0.html
3. https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter

I decided to next try out the code on the last link. This combines the gyro and the compass directly within the filter. I had to adjust several of the smoothing parameters to get a good result. I should probably estimate the process and noise covariance directly from sensor measurements, but first I wanted to get all the measurements going into the filter and determine if the Kalman filter is worth the effort and computation (everything is now running in matlab). Here is the result:

Just based on these plots, it is very difficult to determine the best filtering approach. The next steps are to add in the accelerometer into the mix and look at combining heading information from all three sensors. That will involve using the Kalman filter to weight two measures of heading angle (the accelerometer and the magnetometer).

The code to make all of these plots is on the github site in the matlab directory:

https://github.com/mark-r-stevens/Ardadv/tree/master/matlab