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

* 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/CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(ecl_EKF
 	gps_yaw_fusion.cpp
 	range_finder_checks.cpp
 	imu_down_sampler.cpp
+	EKFGSF_yaw.cpp
 )
 
 add_dependencies(ecl_EKF prebuild_targets)

EKF/EKFGSF_yaw.h

@@ -0,0 +1,153 @@
+#pragma once
+
+#include <geo/geo.h>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+using matrix::AxisAnglef;
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Matrix3f;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+#define N_MODELS_EKFGSF 5
+
+#ifndef M_PI_F
+#define M_PI_F 3.14159265f
+#endif
+
+#ifndef M_PI_2_F
+#define M_PI_2_F 1.57079632f
+#endif
+
+#ifndef M_TWOPI_INV
+#define M_TWOPI_INV 0.159154943f
+#endif
+
+class EKFGSF_yaw
+{
+public:
+    	// Constructor
+    	EKFGSF_yaw();
+
+    	// Update Filter States - this should be called whenever new IMU data is available
+	void update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)
+                	const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s)
+                	const float del_ang_dt, // time interval that del_ang was integrated over (sec)
+                	const float del_vel_dt, // time interval that del_vel was integrated over (sec)
+                	bool run_EKF,           // set to true when flying or movement is suitable for yaw estimation
+                	float airspeed);   	// true airspeed used for centripetal accel compensation - set to 0 when not required.
+
+	void setVelocity(Vector2f velocity,	// NE velocity measurement (m/s)
+                     	float accuracy);	// 1-sigma accuracy of velocity measurement (m/s)
+
+	// get solution data for logging
+	bool getLogData(float *yaw_composite,
+			float *yaw_composite_variance,
+			float yaw[N_MODELS_EKFGSF],
+			float innov_VN[N_MODELS_EKFGSF],
+			float innov_VE[N_MODELS_EKFGSF],
+			float weight[N_MODELS_EKFGSF]);
+
+    	// get yaw estimate and the corresponding variance
+    	// return false if no yaw estimate available
+    	bool getYawData(float *yaw, float *yaw_variance);
+
+private:
+	// Parameters - these could be made tuneable
+	const float _gyro_noise{1.0e-1f}; 	// yaw rate noise used for covariance prediction (rad/sec)
+	const float _accel_noise{2.0f};		// horizontal accel noise used for covariance prediction (m/sec**2)
+	const float _tilt_gain{0.2f};		// gain from tilt error to gyro correction for complementary filter (1/sec)
+	const float _gyro_bias_gain{0.04f};	// gain applied to integral of gyro correction for complementary filter (1/sec)
+	const float _weight_min{0.0f};		// minimum value of an individual model weighting
+
+	// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
+
+	Vector3f _delta_ang;	// IMU delta angle (rad)
+	Vector3f _delta_vel;	// IMU delta velocity (m/s)
+	float _delta_ang_dt;	// _delta_ang integration time interval (sec)
+	float _delta_vel_dt;	// _delta_vel integration time interval (sec)
+	float _true_airspeed;	// true airspeed used for centripetal accel compensation (m/s)
+
+	struct _ahrs_ekf_gsf_struct{
+		Dcmf R;				// matrix that rotates a vector from body to earth frame
+		Vector3f gyro_bias;		// gyro bias learned and used by the quaternion calculation
+		bool aligned{false};		// true when AHRS has been aligned
+		float vel_NE[2] {};		// NE velocity vector from last GPS measurement (m/s)
+		bool fuse_gps = false;		// true when GPS should be fused on that frame
+		float accel_dt = 0;		// time step used when generating _simple_accel_FR data (sec)
+	};
+	_ahrs_ekf_gsf_struct _ahrs_ekf_gsf[N_MODELS_EKFGSF];
+	bool _ahrs_ekf_gsf_tilt_aligned = false;// true the initial tilt alignment has been calculated
+	float _ahrs_accel_fusion_gain;		// gain from accel vector tilt error to rate gyro correction used by AHRS calculation
+	Vector3f _ahrs_accel;			// low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
+	float _ahrs_accel_norm;			// length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
+
+	// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
+	void ahrsCalcAccelGain();
+
+	// update specified AHRS rotation matrix using IMU and optionally true airspeed data
+	void ahrsPredict(const uint8_t model_index);
+
+	// align all AHRS roll and pitch orientations using IMU delta velocity vector
+	void ahrsAlignTilt();
+
+	// align all AHRS yaw orientations to initial values
+	void ahrsAlignYaw();
+
+	// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
+	Matrix3f ahrsPredictRotMat(Matrix3f &R, Vector3f &g);
+
+	// Declarations used by a bank of N_MODELS_EKFGSF EKFs
+
+	struct _ekf_gsf_struct{
+		matrix::Vector3f X; // Vel North (m/s),  Vel East (m/s), yaw (rad)s
+		matrix::SquareMatrix<float, 3> P; // covariance matrix
+		float W = 0.0f; // weighting
+		matrix::SquareMatrix<float, 2> S; // innovation covariance matrix
+		matrix::SquareMatrix<float, 2> S_inverse;  // inverse of the innovation covariance matrix
+		float S_det_inverse; // inverse of the innovation covariance matrix determinant
+		matrix::Vector2f innov; // Velocity N,E innovation (m/s)
+	};
+	_ekf_gsf_struct _ekf_gsf[N_MODELS_EKFGSF];
+	bool _vel_data_updated;  // true when velocity data has been updated
+	bool _run_ekf_gsf;       // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data
+	Vector2f _vel_NE;        // NE velocity observations (m/s)
+	float _vel_accuracy;     // 1-sigma accuracy of velocity observations (m/s)
+	bool _ekf_gsf_vel_fuse_started;	// true when the EKF's have started fusing velocity data and the prediction and update processing is active
+
+	// initialise states and covariance data for the GSF and EKF filters
+	void initialiseEKFGSF();
+
+	// predict state and covariance for the specified EKF using inertial data
+	void predictEKF(const uint8_t model_index);
+
+	// update state and covariance for the specified EKF using a NE velocity measurement
+	// return false if update failed
+	bool updateEKF(const uint8_t model_index);
+
+	inline float sq(float x) { return x * x; };
+
+	// 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 by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates
+
+	float _gsf_yaw; 		// yaw estimate (rad)
+	float _gsf_yaw_variance; 	// variance of yaw estimate (rad^2)
+
+	// return the probability of the state estimate for the specified EKF assuming a gaussian error distribution
+	float gaussianDensity(const uint8_t model_index) const;
+
+	// update the inverse of the innovation covariance matrix
+	void updateInnovCovMatInv(const uint8_t model_index);
+
+};

EKF/common.h

@@ -354,6 +354,11 @@ struct parameters {
 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
 	int32_t check_mag_strength{0};
+
+	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
+	float EKFGSF_tas_default{15.0f};	///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
+	unsigned EKFGSF_reset_delay{1000000};	///< Number of uSec of bad innovations on main filter inpost takeoff phase before yaw is reset to EKF-GSF value
+	float EKFGSF_yaw_err_max{0.2f}; 	///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
 };
 
 struct stateSample {

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;
 

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;
+	}
+
 }
 
 /*

EKF/ekf.h

@@ -284,6 +284,17 @@ class Ekf : public EstimatorInterface
 	// 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 @@ class Ekf : public EstimatorInterface
 	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 @@ class Ekf : public EstimatorInterface
 
 	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();
+
 };