The art of reduced expectations

After poring through this code for a while, I ended up with more questions than answers. Are they indeed using a Kalman filter, or an "Extended" Kalman filter, or a "Kalman-Bucy" filter? The use of Jacobian determinants implies that it's an EKF, but where's the nonlinearity that would require an EKF? (This is especially puzzling in this simplified version that a lot of people like to crib from, which also uses a Jacobian even though it's a constant.) The use of the "P_dot = A*P + P*A_transpose + Q" form of the covariance update equation seems to imply the continuous-time ("Kalman-Bucy") formulation, but why would they do that for something that's explicitly using a series of discrete measurements?

What nobody really talks about is that most uses of the Kalman filter for accelerometer/gyro fusion are a little bit of a hack, treating the gyro as a control input, treating the accelerometer as a noisy orientation reference (even though vehicle acceleration does not behave anything like the Gaussian white noise the Kalman filter assumes), and various other questionable things.

This poor fellow had some very similar questions and got some really not very useful answers. Several people declared it was impossible because his system was "not fully determined", other people said things like "You just have to get a book on Kalman filters and work through the examples until you understand" without actually answering the questions. The grand winner of the "insanely awful reply" contest was the guy who said

And remember that for stability, the "stable platform" must have an
undamped resonance with a period of 88 minutes. (88 minutes is the
orbital time at zero altitude assuming no air, and also the period of a
pendulum whose length is the earth's radius, assuming uniform gravity.)

Yes, yes, the Earth's rotation is super important to worry about for our little MEMS accelerometers and gyros. Idiot.

It may well be that the "go read some books" advice is good, but I'm pretty sure that would take another month or six.

Anyway, on top of all of that, the way rotations are represented by quaternions in things like the Paparazzi code are also complicated. I mean, quaternions are simple enough, but mixing them with all this Kalman filter linear algebra gets hairy. And porting their carefully unrolled matrix multiplications to my Propeller processor -- which would almost certainly need to be done in assembly -- seemed more than I wanted to undertake just now. So eventually I decided, okay, screw that crap, I'm going to treat this as independent 1-D tilt estimation problems for pitch, roll, and yaw. They're not actually independent, but they are mostly so when the platform is mostly level, and I'm sort of hoping to keep it mostly level most of the time, and this way I can actually have a fighting chance of getting my head around the equations without exploding.

So, I managed to implement the basic tilt sensor Kalman filter thing for pitch and roll in Python code on my computer, and it's simple enough I bet I can port it to the Propeller pretty readily. Here it is in action:


The Kalman-filtered roll indicator is the needle near the lower-right of the grey data display window. You can see that it's perpindicular to the propeller.


See, it's still perpindicular to the propeller, in this new orientation. Of course, to get the real effect I'd have to post a video or something, but I'm just not that elite, so take my word for it that it seems to work OK and updates pretty fast and doesn't go too haywire if I shake the unit (that is, non-gravitational acceleration doesn't confuse it too badly).


Detail of the IMU test app's data display. I had just set it down on the table when this was taken, which is why pitch and roll are both zero. Check out that wacky, wacky magnetometer.

So, the next step is to port the pitch/roll computation to the Propeller, and make sure it's working okay, and then... set up some kind of simple PID controller to try to take off and keep the platform level. Yaw is going to be an issue unless I can get some kind of absolute heading out of that crazy magnetometer data, but maybe I can sort of fudge that for now with manual control.