Refactor mag heading into mag 3D fusion

[bresch]

Solved Problem

In its current form, mag heading fusion is made by fusing a yaw measurement based on magnetic data. The issues with that approach are:

• the uncertainty of estimated mag biases are not included in the innovation variance
• a separate control and fusion logic is required
• the mag fusion needs to run in parallel in order to estimate the mag biases

Solution

Heading fusion can be achieved by inhibiting the effect of the fusion on the attitude to only affect the rotation around the vertical axis (NED frame). To do this, I first needed to re-define the quaternion error as a small perturbation in the NED frame (aka: global angular error) instead of in the body frame (aka: local angular error). This allows us to use the exact same fusion process as "mag 3D" and inhibit its effect on the tilt estimate by zeroing the two first elements of the Kalman gain. Note that this is only possible because we're using the Joseph stabilized update.

I haven't yet found how to derive the measurement jacobians using first order retractions (as done in SymForce for local errors). The current version is using a chain rule method that produces a bit more auto-code.

TODO:

• prevent the mag heading fusion from making the EKF too certain about its heading before takeoff
• flight tests

Future work:

• replace external mag bias estimator by bias estimate from EFK2
• try to start GNSS fusion without yaw alignment by setting a large heading uncertainty

Changelog Entry

For release notes:

-
New parameter: -
Documentation: -

[laughlinbarker]

I've recently found this repo and have been reading the code and playing with symforce (what a nice and powerful tool). Out of curiosity, what's prompting you to modify your Jacobian computations in this PR to use the chain rule (as Sola describes), as opposed to the default symforce tangent space Jacobian in your original implementation?

[bresch]

Hi @laughlinbarker , the reason why I used the chain rule (instead of the first order approximation done in SymForce) is because I changed the angular error to be a "global error" (before was a "local error").
As a reminder:

• q_true = q_est * local_error (expected by SymForce, currently used in main)
• q_true = global_error * q_est (used by this PR)

This means that the measurement jacobians are also different and to see if everything worked as expected I quickly implemented the chain rule for global errors (as described by Sola) instead of the first order approximation method because it was simpler. I also tried to chain the "local error jacobian" obtained by SymForce with the transpose of the rotation matrix (because R is the adjoint of SO(3) mapping local to global errors) to convert it into a "global error jacobian" but it gave more auto-code.

I still want to try to implement the first order approximation for global errors as it should produce less code, but this could just be an optimization that can be done later (the result is numerically the same). I'm not against a bit of help, so if you want to try to do it, please either share your results here, open a PR against this one or contact me on the Dronecode Discord server (my name is Bresch and username is mbresch)

FYI, this is how the Jacobians using first order approximations are computed in SymForce: https://github.com/symforce-org/symforce/blob/f04f31141f5573f31aa8bbbf0d800264f023b04f/symforce/jacobian_helpers.py#L34