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.h

@@ -284,6 +284,17 @@ public:
 	// set minimum continuous period without GPS fail required to mark a healthy GPS status
 	void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
 
+	// get solution data from the EKF-GSF emergency yaw estimator
+	// returns false when data is not available
+	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) override;
+
+	// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
+	// and reset the velocity and position states to the GPS. This will cause the EKF
+	// to ignore the magnetomer for the remainder of flight.
+	// This should only be used as a last resort before activating a loss of navigation failsafe
+	// The counter must be incremented for each new reset request
+	void requestEmergencyNavReset(uint8_t counter) override;
+
 private:
 
 	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
@@ -304,6 +315,8 @@ private:
 	float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
 	float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
 
+	Vector3f _ang_rate_delayed_raw;	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
+
 	stateSample _state{};		///< state struct of the ekf running at the delayed time horizon
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
@@ -815,4 +828,32 @@ private:
 
 	void setVelPosFaultStatus(const int index, const bool status);
 
+	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
+	// yaw : Euler yaw angle (rad)
+	// yaw_variance : yaw error variance (rad^2)
+	// update_buffer : true if the state change should be also applied to the output observer buffer
+	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
+
+	// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
+	// to the corresponding rotation matrix that rotates from frame 2 to frame 1
+	// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
+	// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
+	// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
+	// See http://www.atacolorado.com/eulersequences.doc
+	Dcmf taitBryan312ToRotMat(const Vector3f &rot312);
+
+	// Declarations used to control use of the EKF-GSF yaw estimator
+
+	int64_t _emergency_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
+	uint64_t _time_last_on_ground_us{0};	///< last tine we were on the ground (uSec)
+	uint8_t _yaw_extreset_counter{0};	// number of external emergency yaw reset requests
+	bool _do_emergency_yaw_reset{false};	// true when an emergency yaw reset has been requested
+
+	// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
+	void runYawEKFGSF();
+
+	// Resets the main Nav EKf yaw to the esitmator from the EKF-GSF yaw estimator
+	// Returns true if the reset was successful
+	bool resetYawToEKFGSF();
+
 };