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.
- 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?
- I think that I can initialize my initial state vector correctly by performing TRIAD using the magnetic field vector, is this the right approach?
- 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.