Reduce vulnerability of EKF-GSF yaw estimator to GPS glitches

[priseborough]

Improves behaviour reported here #15813

Replay of log provided with issue shows the EKF-GSF stops updating its yaw estimate during a higher period of reported GPS velocity error and resets the yaw estimation when the glitch finishes:

Before:

After:

GPS reported velocity accuracy:

The second layer of protection added is use of a velocity innovation magnitude from the bank of EKF's in the yaw estimator that is obtained using the same weightings that are used to derive the yaw estimate. For operation with non fly forward vehicle types, this needs to be below 2 m/s which has been set to be just over the value achieved by this log with noisy GPS when not glitching, eg:

Checking another copter log with a good GPS and where yaw was aligned by flying away from the pilot shows that a value of below 1.0 is achieved during the initial convergence, so the value of 2 will be OK for now and can be adjusted later.

TODO: testing of EKF2 changes.

[tridge]

Looks good! thanks for fixing this Paul. Grant and I will fly the changes soon

[priseborough]

Testing after rebase on #15820 and running with the --force-ekf2 option to enable ekf2 use showed that the emergency yaw reset did not occur for the EKF2 instance:

TOGCS: EKF3 IMU0 buffs IMU=19 OBS=7 OF=17 EN:17 dt=0.0120
TOGCS: EKF3 IMU1 buffs IMU=19 OBS=7 OF=17 EN:17 dt=0.0120
TOGCS: EKF3 IMU0 initialised
TOGCS: EKF3 IMU1 initialised
TOGCS: EKF2 IMU0 initial yaw alignment complete
TOGCS: EKF2 IMU1 initial yaw alignment complete
TOGCS: EKF3 IMU0 initial yaw alignment complete
TOGCS: EKF3 IMU1 initial yaw alignment complete
TOGCS: EKF2 IMU0 tilt alignment complete
TOGCS: EKF2 IMU1 tilt alignment complete
TOGCS: EKF3 IMU0 tilt alignment complete
TOGCS: EKF3 IMU1 tilt alignment complete
TOGCS: EKF2 IMU0 origin set
TOGCS: EKF2 IMU1 origin set
TOGCS: EKF3 IMU0 origin set
TOGCS: EKF3 IMU1 origin set
TOGCS: EKF3 IMU0 is using GPS
TOGCS: EKF3 IMU1 is using GPS
TOGCS: EKF2 IMU0 is using GPS
TOGCS: EKF2 IMU1 is using GPS
TOGCS: EKF3 IMU0 in-flight yaw alignment complete
TOGCS: EKF3 IMU1 in-flight yaw alignment complete
TOGCS: EKF2 IMU0 in-flight yaw alignment complete
TOGCS: EKF2 IMU1 in-flight yaw alignment complete
TOGCS: EKF3 lane switch 1
TOGCS: EKF3 lane switch 0
TOGCS: EKF3 lane switch 1
TOGCS: EKF3 lane switch 0
TOGCS: EKF3 lane switch 1
TOGCS: EKF3 lane switch 0
TOGCS: EKF3 lane switch 1
TOGCS: EKF3 lane switch 0
TOGCS: EKF3 lane switch 1
TOGCS: EKF3 lane switch 0

[rmackay9]

Sorry to be a nag but I really would like it if we could avoid any EKF3 changes that conflict with my ekf pos source PR. I understand this is an important fix but the issue has been there for a while and so I'm a little confused as to why this went in first.

[tridge]

Sorry to be a nag but I really would like it if we could avoid any EKF3 changes that conflict with my ekf pos source PR. I understand this is an important fix but the issue has been there for a while and so I'm a little confused as to why this went in first.

because it is a "fly into a tree if you have a M6 GPS". I thought it was important to get the fix in soon.
The rebase of your PR on master to remove the conflict is very simple, here is a branch with it done:
https://github.com/tridge/ardupilot/tree/ekf-pos-source2