ekf2-mag_auto: always use mag 3D after takeoff

[bresch]

Solved Problem

The mag heading fusion might be more robust in some cases, but doesn't consider the mag bias uncertainty and is affecting the heading quite strongly (because it is a direct heading observation, without bias). To prevent this, we're currently allowing it to fuse a mag sample only if the heading uncertainty is becoming too large. This protection has other sides effects as it under-constrains the heading when the mag bias have been correctly estimated. This degrades dead-reckoning performance and creates large position corrections every time a new mag sample is fused.

Solution

Mag 3D fusion is the ideal way of fusing magnetometer measurements in an EKF as the original measurement is fused (instead of preprocessing it using the states -> correlation with the filter). Since the mag bias uncertainty is initially large, the heading isn't over-constrained at takeoff and an incorrect initial heading can easily be corrected through GNSS fusion or angular motions.

As a first step, this PR changes the mag "auto" logic to switch into mag 3D fusion right after the "in flight mag reset" occurring at 1.5m after takeoff. If no issue is observed, I would like to completely remove the mag heading fusion feature as this would greatly simplify the code and remove complicated code paths.

Changelog Entry

For release notes:

New parameter: -
Documentation: No

Alternatives

I tried to compute the actual mag heading uncertainty using the quaternion and mag bias covariance, but the result is still under-estimated.

Test coverage

SITL tests:

• Initial heading error of 90 degrees due to wrong mag bias calibration (0.2Gs on x and y). Heading and mag biases are quickly corrected.

• Incorrect mag data plumbing (mag_x = -mag_y, mag_z = mag_z - mag_x); tested with and without GNSS fusion. In this case, the mag fusion is easily rejected; the attitude is not affected and the heading is correct when fusion is active.

[dagar]

Update EKF2_MAG_ACCLIM description?

https://github.com/PX4/PX4-Autopilot/blob/c697c0397e4cfc8c0b112b10bc6e3ee3416a6039/src/modules/ekf2/ekf2_params.c#L505-L516

[RomanBapst]

@bresch I tested this PR with the deadreckoning scenario and it seems to work fine, I was not able to introduce the position jumps anymore, and in general the dead-reckoning performance is much better.

[bresch]

I tested this PR with the deadreckoning scenario and it seems to work fine, I was not able to introduce the position jumps anymore, and in general the dead-reckoning performance is much better.

Great, thanks. I just need to investigate the optical flow unit test failure and we should be good to go.