Fast on the heels of my 1-D Kalman Filter for tracking gyro bias and determining tilt angle in a balancing robot, I've checked in code for an entire GPS aided inertial navigation system. You can see the full source code for the 17 state Kalman filter in gpsins.cpp andgpsins.h.

This filter uses the 3 axis, 6 dof IMU from Rotomotion, a three axis magnetometer from PNI and a u-blox TIM-LP GPS. It computes position in a local tangent frame for ease of use with the flight controller and tracks both gyro and accelerometer biases. We all know about the gyro drift problems, but the ADXL units also have a slight amount of temperature drift that can be tracked by the filter.

I'm still doing static testing on the software and hardware. Hopefully within a few weeks I'll be able to flight test it on my helicopter. As far as I know, this is the only Free Software GPS aided INS in existance.