EKF: Add Emergency yaw recovery using EKF-GSF estimator (#766)

* EKF: Use common rate vector calculation for offset corrections

* EKF: Remove duplicate matrix entry calculations

* EKF: Create a EKF-GSF yaw estimator class

* EKF: add emergency yaw reset functionality

* EKF: remove un-used function

* EKF: Ensure required constants are defined for all builds

* EKF: Fix CI build error

* Revert "EKF: remove un-used function"

This reverts commit 93005309c7f3794414ad99c86218b3062e00bbd3.

* EKF: Replace in-lined Tait-Bryan 312 conversions with function call

Also remove unnecessary operations

* EKF: Remove unnecessary update of external vision rotation matrix

* EKF: Use const

* EKF: use const

* EKF: don't use class variable as a temporary variable

* EKF: update comments

* EKF: Improve efficiency of yaw reset

Use conversion from rotation matrix to Euler angles instead of quaternion to euler angles.

* EKF: use const

* EKF: remove un-used struct element

* EKF: more descriptive function name

* EKF: use existing matrix row operator

* EKF: remove unnecessary rotation matrix update

* EKF: Use square matrix type

* EKF: Improve protection for bad innovation covariance

* EKF: Use matrix library operations

* EKF: Replace memcpy with better alternative

memcpy bypasses compiler sanity checks and is unnecessary in this instance.

* EKF: Split EKF-GSF yaw reset function

Adds a common function to support yaw reset that can be used elsewhere.

* EKF: Use common function for quaternion state and covariance yaw reset

* EKF: Replace inlined matrix operation

* EKF: Use const

* EKF: Change accessor function name

* EKF: Use const

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Remove duplicate covariance innovation inverse

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Rely on geo library to provide gravity

* EKF: Improve protection from bad updates

* EKF: Reduce effect of vibration on yaw estimator AHRS

* EKF: Improve yaw estimator AHRS accuracy during manoeuvre transients

EKF/control.cpp

@@ -357,9 +357,8 @@ void Ekf::controlExternalVisionFusion()
 			}
 
 			// correct velocity for offset relative to IMU
-			const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
 			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			const Vector3f vel_offset_body = ang_rate % pos_offset_body;
+			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
 			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			vel_aligned -= vel_offset_earth;
 
@@ -657,7 +656,34 @@ void Ekf::controlGpsFusion()
 			// We haven't had an absolute position fix for a longer time so need to do something
 			do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
-			if (do_reset) {
+			// A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable
+			// recovery from a bad magnetometer or field estimate.
+			// This special case reset can also be requested externally.
+			// The minimum time interval between resets to the EKF-GSF estimate must be limited to
+			// allow the EKF-GSF time to improve its estimate if the first reset was not successful.
+			const bool stopped_following_gps_velocity = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
+							   (_time_last_hor_vel_fuse > _time_last_on_ground_us);
+			if (!_control_status.flags.in_air) {
+				_time_last_on_ground_us = _time_last_imu;
+			}
+			const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000);
+			const bool reset_yaw_to_EKFGSF = (do_reset || _do_emergency_yaw_reset || stopped_following_gps_velocity) &&
+								recent_takeoff &&
+								isTimedOut(_emergency_yaw_reset_time, 5000000);
+
+			if (reset_yaw_to_EKFGSF) {
+				if (resetYawToEKFGSF()) {
+					_emergency_yaw_reset_time = _time_last_imu;
+					_do_emergency_yaw_reset = false;
+
+					// Reset the timeout counters
+					_time_last_hor_pos_fuse = _time_last_imu;
+					_time_last_delpos_fuse = _time_last_imu;
+					_time_last_hor_vel_fuse = _time_last_imu;
+					_time_last_of_fuse = _time_last_imu;
+
+				}
+			} else if (do_reset) {
 				// use GPS velocity data to check and correct yaw angle if a FW vehicle
 				if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
 					// if flying a fixed wing aircraft, do a complete reset that includes yaw
@@ -686,9 +712,8 @@ void Ekf::controlGpsFusion()
 			Vector3f gps_pos_obs_var;
 
 			// correct velocity for offset relative to IMU
-			const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
 			const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-			const Vector3f vel_offset_body = ang_rate % pos_offset_body;
+			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
 			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			_gps_sample_delayed.vel -= vel_offset_earth;