# Adding magnetic field vector to a Kalman filter

I currently have an error state Kalman filter with the state vector $(p, v, q, \omega, a, g)$ where $q$ is the quaternion orientation. I would like to add the information coming from a magnetometer to this sensor fusion.

I have calibrated the magnetometer and we can assume that we are getting processed data at the point of input to the filter.

1. How do I extend my state vector to account for the new input, or since I do not directly care about estimating it, should I not include it?
2. I think that I can initialize my initial state vector correctly by performing TRIAD using the magnetic field vector, is this the right approach?
3. How does the magnetic field vector help in stabilizing my quaternion attitude?

I tried to search around but I didn't find many resources on how the math works when I include the magnetometer. Any links would be very helpful as well.

• what is the state you are estimating? what is TRIAD? – holmeski Nov 2 '16 at 0:35
• Have you considered the Madgwick filter? A free, open-source filter with code already provided in C, C#, and Matlab, that does quaternion-based pose estimation from an IMU and magnetometer, and performs as well as or better than a Kalman filter? From the paper, "Empirical testing and benchmarking has shown that the filter performs as well as a high quality commercial Kalman-based system, even with a full order of magnitude in reduction of sampling rate." – Chuck Nov 2 '16 at 13:38