ekf2: Only reset to GNSS heading if necessary

[bresch]

Solved Problem

True heading is of primary importance for GNSS fusion in order to align the body and NED frames. Following a heading measurement isn't really important in the NE fusion doesn't report any issue and resetting to a heading measurement when not needed can create more issues than it solves.
Also, recent experiments showed that dual-antenna GNSS receivers are sometimes diverging and re-converging during operation. Since this can occur several times during the mission and isn't a true failure of the system, marking it as faulty for the rest of the flight is too restrictive.

Solution

When North-East aiding is active (e.g.: GNSS pos/vel) , the heading estimate is constrained and consistent with the vel/pos aiding. Reset to GNSS heading should only occur if no N-E aiding is active or if the filter is not yes aligned. Otherwise, just wait for the consistency check to pass again (will pass at some point if the heading uncertainty of the filter is getting too high).

Note: we should do the same changes for mag and ev yaw control logic (i.e.: no reset unless really required)

Changelog Entry

For release notes:

Rework GNSS heading control logic
New parameter: -
Documentation: No

Alternatives

We could also ...

Test coverage

unit tests