EKF: use low pass filtered mag measurements for heading initialization

[CarlOlsson]

in initialiseFilter() we low pass filter the measurements from the magnetometer, however when initializing the heading we took the last value from the mag instead of the filtered values. At least in SITL this sometimes results in up to 5 degrees heading error at bootup

[priseborough]

This is not going to work. mag_init is not updated after Ekf::initialiseFilter() has completed and is taken from _mag_filt_state. I would move the calculation of _mag_filt_state to EstimatorInterface::setMagData and use that instead.

[CarlOlsson]

mag_init in this case is what is passed into the function so if I don't miss something it is indeed updated
https://github.com/PX4/ecl/blob/c49ab760c78fc6dc4cb174d6f9b7b39b01e19a7b/EKF/ekf_helper.cpp#L546-L546

In initialiseFilter() we call resetMagHeading with the low pass filtered magnetometer measurements, but in all other cases we call it with the last magnetometer measurements.

So this PR would solve the first issue mentioned here that we should use the low pass filtered mag measurements for heading initialization, but not the fact that as soon as we get GPS lock we again reinitialize the heading without a low pass filter on the mag measurements.

[CarlOlsson]

I would move the calculation of _mag_filt_state to EstimatorInterface::setMagData and use that instead.

I could do that after this is merged. However, before I start I just want to hear your opinion on the following:

Say we constantly run a low pass filter on the mag data and use that every time we reset the heading based on the magnetometer. Currently that would mean that we first initialize the heading at boot up, then when we get GPS lock we again reset the heading, still on the ground so all fine. However, then we take off, say with automatic switching of 3D fusion active. We reach e.g. 30 meter altitude and stay there for some time, then we start rotating in yaw, now since the mag bias is declared observable we activate 3D fusion for the first time and hence reset the heading. Since we are rotating there will be a lag between the mag measurements and the true heading so we might end up with a worse heading estimate than before.
I would propose to remove the reset of the heading the first time we commence 3D fusion and instead always reset the heading when we are away from ground interference at 1.5 meter altitude.

[priseborough]

OK, I think I now understand the problem and what you are proposing. I checked out this PR and following the code through it looks like there are no adverse effects and it solves the issue of noise affecting the initial alignment so I will approve and merge it once there is a test log.

With respect to yawing and lag due to filtering, because we run the fusion on a delayed time horizon, by filtering the data going onto the buffer and using a filter that has a group delay equivalent to the buffer length, the effects of filter lag will be cancelled.

[CarlOlsson]

it looks like there are no adverse effects and it solves the issue of noise affecting the initial alignment so I will approve and merge it once there is a test log.

I don't have access to a system running standard PX4 at the moment, Is a sitl log enough?

With respect to yawing and lag due to filtering, because we run the fusion on a delayed time horizon, by filtering the data going onto the buffer and using a filter that has a group delay equivalent to the buffer length, the effects of filter lag will be cancelled.

True, nice!

[priseborough]

SITL will be OK

[CarlOlsson]

Here is a sitl logfile with current master PX4 and this PR