Update To AHRS with Mag!

Since my previous post on AHRS types, I have made some additional changes to the three systems.  Specifically, I have added the magnetometer data as an update to the yaw in each system.  My technique for adding the mag in each system was derived from this paper by Sebastian Madgwick. 

The idea is that the Earth's magnetic field can be described as having a components in a single horizontal axis and a single vertical axis.  By rotating these components inversely, by our quaternion, we can directly compare our results to the measured magnetic field vector.  This was especially helpful because it simplified the equation for R in the EKF and UKF, because I could directly use square of the rms-noise of the magnetometer sensors as the diagonals of the matrix. 

The toughest part of this problem was deriving each of the equations for Madgwick, the EKF and the UKF.  I would post it my work, but it is extremely tedious.  However, the results are very good and you can see that it clearly improved the yaw from the previous post in all three cases.

This was an extremely exciting result for me to see, because I worked very hard on the solutions.  The one area that I need to work on is when to provide updates to the EKF and UKF.  Currently, I use an accelerometer threshold, just to make sure there is no movement.  However, this causes the system to be very sensitive to slight movements.  Preferably, I would like to make a more robust system for performing updates.  That's all for now!