Data logging and Kalman Filtering

Matt again! Our copter project is alive again and making excellent progress. I’ve finished my last year of marching band, something I already miss dearly, but one upside is that I’ve got enough time to get back to quadcopter coding.

Last Sunday, we made the decision to abandon our homemade serial shifter and continue to use Sparkfun’s. I won’t get my one touch programming, but thats obviously not terribly necessary to the actual quad. We put serial headers back on the board, and now we’re back in business for programming.

Over the past few days I’ve improved the visualization capabilities of “QuadComm,” something I don’t think I’ve shown yet on the blog. It can now display real time graphs of numbers received over the serial connection, which is pretty fun to play with. Here’s a pic of it in action:

QuadComm ploting accelerometer angle and gyro integration

This is a 3 second interval me rocking the board back and forth on its roll axis. The jagged upper line is the angle computed from the accelerometer, using arctan. As you can see, its pretty jagged, particularly on the right side of the image, where I began to shake the board a little. The smooth line below it is the integral of the gyro signal. Though much cleaner then the accelerometer signal, because the integration process, it slowly accumulates error and drifts away from the actual angle. In this picture, the gyro output is below the actual angle of the board by nearly 10 degrees!

Anyone whose ever made a quadcopter, balancing robot, or otherwise used gyros and accelerometers knows that the way to fix the problem of gyro drift is by using a Kalman filter. The Kalman filter is able to track the drift of the gyro using the accelerometer as an absolute reference. By taking the best of both sensors, the filter gives a signal that doesn’t drift like an accelerometer, but isn’t prone to spikes or bumps like a gyro.

Unfortunately, Kalman filters are rather complex. To make matters worse, I’m fairly certain that numerous simplified explanations and example code on the internet are incorrect. They may “make sense” and produce functioning results, but don’t actually implement a correct Kalman filter. Here’s a few of the sources I used to piece together my understanding:

• http://en.wikipedia.org/wiki/Kalman_filter – Uses some statistics terms, but if you look them all up it begins to make sense.
• http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html – Uses lots of statistics and control theory terms, but the example and pictures at the end helped me understand exactly what the different parameters affected.

Lots of reading, re-reading, note taking, and experimenting eventually yielded a GNU Octave script that produced this chart:

Output of my GNU Octave Kalman filter script

On the chart, red is the accelerometer, green is a naive integration of the gyro signal, and blue is the output of the filter. As you can see, the Kalman filter is basically outputting the gyro signal, only offset to be near what the accelerometer is currently reading. The first third of the chart is me slowly turning the board left and right. Because I just booted up the system, the gyro hasn’t drifted much yet. Then I kept tilting the board but began shaking it along its axis, producing the massive accelerometer jitter that the filter manages to plot right through. Finally, I hold the board still. By now the naive integration gyro has drifted downward significantly, yet even with me introducing a massive amount of accelerometer noise it was able to track the drift quite well.

To wrap up this post, I’m going to include a few notes on the implementation of the above filter that took me a while to figure out.

• States are completely independent of sensors. The states of the system are angle, angular velocity, and gyro velocity drift. The measurement is accelerator angle and gyro angular rate.
• There is no control vector ‘u’, or at least one isn’t needed. A few places incorrectly claim that the gyro is a control vector. If you wanted to add a control vector, it would be the output of the motors, along with a B matrix to convert motor outputs to a change in state. Nobody seems to have needed this for a workable filter, but I may try and add it later to see what happens.
• The filter itself performs the integration of the gyro. A source I found somewhere gave the impression that you needed to integrate the gyro and pass the angle as an input to the filter. While this may work, I think a measurement vector consisting of an accelerometer angle and the gyro angular rate will enable better tracking of angular velocity, which aids in the prediction of the angle.
• Gyro’s output will change slightly due to temperature and other factors even when completely stationary. Its not simply noise, the signal actually holds quite stable but circles around a bit over time, or has a variable bias. Thus, your kalman filter should really track this velocity bias, and not the angular drift/error that is accumulated by integrating it.
• The Q matrix should not have zeros. I don’t understand how best to find covariance, but I found somewhere that you can get a passable approximation by multiplying a vector of the variances of the sensors by the vector’s transpose, as I did in the example script below:

I also took the time to clean up, organize, and comment my Kalman filter script. I feel its considerably more simple, concise and understandable than any other script I stumbled upon. I’d definitely be interested to see if anyone else finds it helpful in learning the Kalman filter! Here’s a link to it on the google code SVN: http://code.google.com/p/mjwquadcopter/source/browse/trunk/Octave/kalman.m.

With a basic understanding of the filter in hand, I’ll begin work on writing it again in C. Then its optimization time!

Update:

I’ve finished rewriting the Kalman filter in C. With virtually no optimizations using explicit floating point matrix operations, it can already compute roll and pitch in just under a millisecond. With yaw thrown in it the update interrupt shouldn’t exceed 1.5 ms, and since the Kalman filter represents the bulk of the computations being done, I may be able to set the sampling rate to 2ms, or 500hz! I’m also looking at performing oversampling using DMA and the ADC’s continuous sample mode to get some additional accuracy on the analog readings. Very glad we went with the stm32 over an AVR or Propeller!

I’ve also tuned the Kalman filter some more. I improved the “recovery time” when errors are introduced tricking the gyro using various means, at the expense of increasing the jitter due to accelerometer noise. I expect, however, that the motors will produce noise of a much higher frequency than my shaking the board, which I think will mean that in practice the output of the filter will still be pretty smooth. This also caused the Kalman filter to settle much closer to the actual angle during periods of relative calm, as opposed to before when even nearly stationary, the filter would never get within 2 degrees of the actual angle, something that I think would make stationary hovering very difficult.