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/ekf.cpp

@@ -83,6 +83,8 @@ void Ekf::reset()
 
 	_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
 
+	_ang_rate_delayed_raw.zero();
+
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
 
@@ -116,6 +118,9 @@ bool Ekf::update()
 		runTerrainEstimator();
 
 		updated = true;
+
+		// run EKF-GSF yaw estimator
+		runYawEKFGSF();
 	}
 
 	// the output observer always runs
@@ -291,6 +296,14 @@ void Ekf::predictState()
 	// filter and limit input between -50% and +100% of nominal value
 	input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
 	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
+
+	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
+	// protect angainst possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
+	// due to insufficient averaging
+	if (_imu_sample_delayed.delta_ang_dt > 0.25f * FILTER_UPDATE_PERIOD_S) {
+		_ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
+	}
+
 }
 
 /*