I didn't want to post these but I've had a bunch of requests for this code. I'm going to make it available because I haven't had time to do the work I wanted. Here are two MPLAB projects. They are both unfinished and not cleaned up but they should be sufficiently commented so someone could pick up where I left off or use some of the routines.
They're a good example of implementation of the methods described and both are working, but neither are optimized and have high computational overhead. The hardware platform is my old development board consisting of dsPIC33FJ256MC710 and Sparkfun razor 6dof.
The first zip is a quaternion EKF with euler updates as described Kingston and Beard Paper "Real-Time Attitude and Position Estimation for Small UAVs Using Low-Cost Sensors". GPS is not implemented.
The second zip contains a quaternion EKF method which integrates vector matching as described in Elkaim paper "MAV Attitude Determination by Vector Matching". An HMC5843 3 axis magnetometer is implemented.
If you have any questions please post them on this forum so we can start a knowledge base here.
Use the code as you please but since I'm making it available, please post anything you do with it back to the public domain! Here?
Applicable papers are all to big to post here. I will keep them online at the links below, with credit to the authors, so long as none of them have a problem with it.
EKF
http://baxtercam.no-ip.org/images/KingstonBeardfull_lowcostimu%20.pdf
http://baxtercam.no-ip.org/images/kingston%20beard%20thesis.pdf
Vector matching
http://baxtercam.no-ip.org/images/MAV_Attitude.pdf
http://baxtercam.no-ip.org/images/demozahrs.pdf
http://baxtercam.no-ip.org/images/Multi_sensor_fusion_GebreEgz.pdf