EKF: Add Emergency yaw recovery using EKF-GSF estimator

[priseborough]

Replaces #754

The estimator has been moved into its own class. Some other minor efficiency improvements have been made.

Replay of a flyaway test log shows the same performance .

Before (with N_MODELS_EKFGSF set to 5)

After:

This was tested with the following firmware changes. Note - an externally requested yaw reset has not been tested and may add unnecessary complexity so the commander changes and the ekf2_main call to Ekf::requestEmergencyNavReset is probably unnecessary.

https://github.com/priseborough/Firmware/tree/pr-yawEKFGSF-refactor

[priseborough]

Flight testing was repeated with a total of 9 takeoffs with a multi-rotor.
A bad yaw estimate was created via setting EKF2_MAG_DECL = -90 and EKF2_DECL_TYPE = 0
Testing included takeoffs with immediate post takeoff switch to mission mode. The mission plan loaded had a representative survey waypoint sequence to determine how the main EKF and EKF-GSF would manage subsequent flight with longer non-manoeuvring segments without use of the magnetometer. The flight vehicle was observed to start to flyaway with a visible stop when the reset was commanded. It then continued the mission. Yaw estimation during survey legs was stable.

The following test data was from one of the mission mode flights
https://logs.px4.io/plot_app?log=bdbd4248-28de-4ce1-8a57-bbf5fb55b6e4

Yaw for the whole flight:

Yaw showing the reset event:

GPS velocity innovations for the whole flight:

[kamilritz]

@priseborough Great work moving this to a separate class. I only reviewed part of the PR until now.

[kamilritz]

You could use a lot more of the functionality provided in the matrix library. That would have the benefit, that the operations are already tested. I added some some suggestions.

[kamilritz]

There seems to be some unaddressed suggestions. I hope you don't mind the numerous review comments.

[priseborough]

@bresch I've split out the common element of the various yaw resets into a separate function. I now need to do a reversion test with a RW and FW SITL flight to ensure this refactor has not broken something.

[priseborough]

@priseborough This is great. I also thought about that. May be we can even move the update of the rotation matrix _R_to_earth into the resetQuatStateYaw function. Probably resetHeading would then be a better name. With this we can make better sure that the quaternion and rotation_matrix are synced.

_R_to_earth is updated by this function.

[kamilritz]

@priseborough it might make sense to reopen this as a new PR. With the existing feedback it is quite hard to keep the overview.

[priseborough]

@kamilritz New PR opened here: #766. It has been rebased.