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

@@ -0,0 +1,656 @@
+#include "EKFGSF_yaw.h"
+#include <cstdlib>
+
+EKFGSF_yaw::EKFGSF_yaw()
+{
+	// this flag must be false when we start
+	_ahrs_ekf_gsf_tilt_aligned = false;
+
+	// these objects are initialised in initialise() before being used internally, but can be reported for logging before then
+	memset(&_ahrs_ekf_gsf, 0, sizeof(_ahrs_ekf_gsf));
+	memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
+	_gsf_yaw = 0.0f;
+	_ahrs_accel.zero();
+}
+
+void EKFGSF_yaw::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.
+{
+	// copy to class variables
+	_delta_ang = del_ang;
+	_delta_vel = del_vel;
+	_delta_ang_dt = del_ang_dt;
+	_delta_vel_dt = del_vel_dt;
+	_run_ekf_gsf = run_EKF;
+	_true_airspeed = airspeed;
+
+	// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
+	const float filter_coef = fminf(10.0f * _delta_vel_dt * _tilt_gain, 1.0f);
+	const Vector3f accel = _delta_vel / fmaxf(_delta_vel_dt, 0.001f);
+	_ahrs_accel = _ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
+
+	// Initialise states first time
+	if (!_ahrs_ekf_gsf_tilt_aligned) {
+		// check for excessive acceleration to reduce likelihood of large inital roll/pitch errors
+		// due to vehicle movement
+		const float accel_norm_sq = accel.norm_squared();
+		const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
+		const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
+		const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
+		if (ok_to_align) {
+			initialiseEKFGSF();
+			ahrsAlignTilt();
+			_ahrs_ekf_gsf_tilt_aligned = true;
+		}
+		return;
+	}
+
+	// AHRS prediction cycle for each model - this always runs
+	ahrsCalcAccelGain();
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		predictEKF(model_index);
+	}
+
+	// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
+	if (_run_ekf_gsf && _vel_data_updated) {
+		if (!_ekf_gsf_vel_fuse_started) {
+			initialiseEKFGSF();
+			ahrsAlignYaw();
+			_ekf_gsf_vel_fuse_started = true;
+		} else {
+			float total_w = 0.0f;
+			float newWeight[N_MODELS_EKFGSF];
+			bool bad_update = false;
+			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+				// subsequent measurements are fused as direct state observations
+				if (!updateEKF(model_index)) {
+					bad_update = true;
+				}
+			}
+
+			if (!bad_update) {
+				// calculate weighting for each model assuming a normal distribution
+				for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+					newWeight[model_index] = fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f);
+					total_w += newWeight[model_index];
+				}
+
+				// normalise the weighting function
+				if (_ekf_gsf_vel_fuse_started && total_w > 1e-15f && !bad_update) {
+					float total_w_inv = 1.0f / total_w;
+					for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+						_ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv;
+					}
+				}
+
+				// Enforce a minimum weighting value. This was added during initial development but has not been needed
+				// subsequently, so this block of code and the corresponding _weight_min can be removed if we get
+				// through testing without any weighting function issues.
+				if (_weight_min > FLT_EPSILON) {
+					float correction_sum = 0.0f; // amount the sum of weights has been increased by application of the limit
+					bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased
+					float unmodified_weights_sum = 0.0f; // sum of unmodified weights
+					for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+						if (_ekf_gsf[model_index].W < _weight_min) {
+							correction_sum += _weight_min - _ekf_gsf[model_index].W;
+							_ekf_gsf[model_index].W = _weight_min;
+							change_mask[model_index] = true;
+						} else {
+							unmodified_weights_sum += _ekf_gsf[model_index].W;
+						}
+					}
+
+					// rescale the unmodified weights to make the total sum unity
+					const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min);
+					for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+						if (!change_mask[model_index]) {
+							_ekf_gsf[model_index].W = _weight_min + scale_factor * (_ekf_gsf[model_index].W - _weight_min);
+						}
+					}
+				}
+			}
+		}
+	} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
+		// wait to fly again
+		_ekf_gsf_vel_fuse_started = false;
+	}
+
+	// Calculate a composite yaw vector as a weighted average of the states for each model.
+	// To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
+	// equal to the weighting value before it is summed.
+	Vector2f yaw_vector = {};
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		yaw_vector(0) += _ekf_gsf[model_index].W * cosf(_ekf_gsf[model_index].X(2));
+		yaw_vector(1) += _ekf_gsf[model_index].W * sinf(_ekf_gsf[model_index].X(2));
+	}
+	_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
+
+	// calculate a composite variance for the yaw state from a weighted average of the variance for each model
+	// models with larger innovations are weighted less
+	_gsf_yaw_variance = 0.0f;
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
+		_gsf_yaw_variance += _ekf_gsf[model_index].W * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
+	}
+
+	// prevent the same velocity data being used more than once
+	_vel_data_updated = false;
+}
+
+void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
+{
+	// generate attitude solution using simple complementary filter for the selected model
+
+	Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;
+
+	// Accelerometer correction
+	// Project 'k' unit vector of earth frame to body frame
+	// Vector3f k = quaterion.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
+	// Optimized version with dropped zeros
+	const Vector3f k(_ahrs_ekf_gsf[model_index].R(2,0), _ahrs_ekf_gsf[model_index].R(2,1), _ahrs_ekf_gsf[model_index].R(2,2));
+
+	// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
+	// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
+	Vector3f tilt_correction = {};
+	if (_ahrs_accel_fusion_gain > 0.0f) {
+
+		Vector3f accel = _ahrs_accel;
+
+		if (_true_airspeed > FLT_EPSILON) {
+			// turn rate is component of gyro rate about vertical (down) axis
+			const float turn_rate = _ahrs_ekf_gsf[model_index].R(2,0) * ang_rate(0)
+					  + _ahrs_ekf_gsf[model_index].R(2,1) * ang_rate(1)
+					  + _ahrs_ekf_gsf[model_index].R(2,2) * ang_rate(2);
+
+			// use measured airspeed to calculate centripetal acceleration if available
+			float centripetal_accel = _true_airspeed * turn_rate;
+
+			// project Y body axis onto horizontal and multiply by centripetal acceleration to give estimated
+			// centripetal acceleration vector in earth frame due to coordinated turn
+			Vector3f centripetal_accel_vec_ef = {_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f};
+			if (_ahrs_ekf_gsf[model_index].R(2,2) > 0.0f) {
+				// vehicle is upright
+				centripetal_accel_vec_ef *= centripetal_accel;
+			} else {
+				// vehicle is inverted
+				centripetal_accel_vec_ef *= - centripetal_accel;
+			}
+
+			// rotate into body frame
+			Vector3f centripetal_accel_vec_bf = _ahrs_ekf_gsf[model_index].R.transpose() * centripetal_accel_vec_ef;
+
+			// correct measured accel for centripetal acceleration
+			accel -= centripetal_accel_vec_bf;
+		}
+
+		tilt_correction = (k % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
+
+	}
+
+	// Gyro bias estimation
+	const float gyro_bias_limit = 0.05f;
+	const float spinRate = ang_rate.length();
+	if (spinRate < 0.175f) {
+		_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
+
+		for (int i = 0; i < 3; i++) {
+			_ahrs_ekf_gsf[model_index].gyro_bias(i) = math::constrain(_ahrs_ekf_gsf[model_index].gyro_bias(i), -gyro_bias_limit, gyro_bias_limit);
+		}
+	}
+
+	// delta angle from previous to current frame
+	Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;
+
+	// Apply delta angle to rotation matrix
+	_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
+
+}
+
+void EKFGSF_yaw::ahrsAlignTilt()
+{
+	// Rotation matrix is constructed directly from acceleration measurement and will be the same for
+	// all models so only need to calculate it once. Assumptions are:
+	// 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences.
+	// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
+
+	// Calculate earth frame Down axis unit vector rotated into body frame
+	Vector3f down_in_bf = -_delta_vel;
+	down_in_bf.normalize();
+
+	// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
+	// * operator is overloaded to provide a dot product
+	const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
+	Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf);
+	north_in_bf.normalize();
+
+	// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
+	// % operator is overloaded to provide a cross product
+	const Vector3f east_in_bf = down_in_bf % north_in_bf;
+
+	// Each column in a rotation matrix from earth frame to body frame represents the projection of the
+	// corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column.
+	// We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body
+	// frame are copied into corresponding rows instead.
+	Dcmf R;
+	R.setRow(0, north_in_bf);
+	R.setRow(1, east_in_bf);
+	R.setRow(2, down_in_bf);
+
+}
+
+void EKFGSF_yaw::ahrsAlignYaw()
+{
+	// Align yaw angle for each model
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+		if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
+			// get the roll, pitch, yaw estimates from the rotation matrix using a  321 Tait-Bryan rotation sequence
+			Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
+
+			// set the yaw angle
+			euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2));
+
+			// update the rotation matrix
+			_ahrs_ekf_gsf[model_index].R = Dcmf(euler_init);
+
+		} else {
+			// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
+			Vector3f rot312;
+			rot312(0) = wrap_pi(_ekf_gsf[model_index].X(2)); // first rotation (yaw) taken from EKF model state
+			rot312(1) = asinf(_ahrs_ekf_gsf[model_index].R(2, 1)); // second rotation (roll)
+			rot312(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), _ahrs_ekf_gsf[model_index].R(2, 2));  // third rotation (pitch)
+
+			// Calculate the body to earth frame rotation matrix
+			_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
+
+		}
+		_ahrs_ekf_gsf[model_index].aligned = true;
+	}
+}
+
+void EKFGSF_yaw::predictEKF(const uint8_t model_index)
+{
+	// generate an attitude reference using IMU data
+	ahrsPredict(model_index);
+
+	// we don't start running the EKF part of the algorithm until there are regular velocity observations
+	if (!_ekf_gsf_vel_fuse_started) {
+		return;
+	}
+
+	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
+	if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
+		// use 321 Tait-Bryan rotation to define yaw state
+		_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
+	} else {
+		// use 312 Tait-Bryan rotation to define yaw state
+		_ekf_gsf[model_index].X(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(0, 1), _ahrs_ekf_gsf[model_index].R(1, 1)); // first rotation (yaw)
+	}
+
+	// calculate delta velocity in a horizontal front-right frame
+	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
+	const float dvx =   del_vel_NED(0) * cosf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * sinf(_ekf_gsf[model_index].X(2));
+	const float dvy = - del_vel_NED(0) * sinf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * cosf(_ekf_gsf[model_index].X(2));
+
+	// sum delta velocities in earth frame:
+	_ekf_gsf[model_index].X(0) += del_vel_NED(0);
+	_ekf_gsf[model_index].X(1) += del_vel_NED(1);
+
+	// predict covariance - autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPupdate.txt
+
+	// Local short variable name copies required for readability
+	// Compiler might be smart enough to optimise these out
+	const float &P00 = _ekf_gsf[model_index].P(0,0);
+	const float &P01 = _ekf_gsf[model_index].P(0,1);
+	const float &P02 = _ekf_gsf[model_index].P(0,2);
+	const float &P10 = _ekf_gsf[model_index].P(1,0);
+	const float &P11 = _ekf_gsf[model_index].P(1,1);
+	const float &P12 = _ekf_gsf[model_index].P(1,2);
+	const float &P20 = _ekf_gsf[model_index].P(2,0);
+	const float &P21 = _ekf_gsf[model_index].P(2,1);
+	const float &P22 = _ekf_gsf[model_index].P(2,2);
+
+	// Use fixed values for delta velocity and delta angle process noise variances
+	const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
+	const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
+	const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2
+
+	const float t2 = sinf(_ekf_gsf[model_index].X(2));
+	const float t3 = cosf(_ekf_gsf[model_index].X(2));
+	const float t4 = dvy*t3;
+	const float t5 = dvx*t2;
+	const float t6 = t4+t5;
+	const float t8 = P22*t6;
+	const float t7 = P02-t8;
+	const float t9 = dvx*t3;
+	const float t11 = dvy*t2;
+	const float t10 = t9-t11;
+	const float t12 = dvxVar*t2*t3;
+	const float t13 = t2*t2;
+	const float t14 = t3*t3;
+	const float t15 = P22*t10;
+	const float t16 = P12+t15;
+
+	const float min_var = 1e-6f;
+	_ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var);
+	_ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
+	_ekf_gsf[model_index].P(0,2) = t7;
+	_ekf_gsf[model_index].P(1,0) = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3;
+	_ekf_gsf[model_index].P(1,1) = fmaxf(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16 , min_var);
+	_ekf_gsf[model_index].P(1,2) = t16;
+	_ekf_gsf[model_index].P(2,0) = P20-t8;
+	_ekf_gsf[model_index].P(2,1) = P21+t15;
+	_ekf_gsf[model_index].P(2,2) = fmaxf(P22+dazVar , min_var);
+
+	// force symmetry
+	_ekf_gsf[model_index].P.makeBlockSymmetric<3>(0);
+}
+
+// Update EKF states and covariance for specified model index using velocity measurement
+bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
+{
+	// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
+	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f));
+
+	// calculate velocity observation innovations
+	_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);
+	_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
+
+	// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
+	const float &P00 = _ekf_gsf[model_index].P(0,0);
+	const float &P01 = _ekf_gsf[model_index].P(0,1);
+	const float &P02 = _ekf_gsf[model_index].P(0,2);
+	const float &P10 = _ekf_gsf[model_index].P(1,0);
+	const float &P11 = _ekf_gsf[model_index].P(1,1);
+	const float &P12 = _ekf_gsf[model_index].P(1,2);
+	const float &P20 = _ekf_gsf[model_index].P(2,0);
+	const float &P21 = _ekf_gsf[model_index].P(2,1);
+	const float &P22 = _ekf_gsf[model_index].P(2,2);
+
+	// calculate innovation variance
+	_ekf_gsf[model_index].S(0,0) = P00 + velObsVar;
+	_ekf_gsf[model_index].S(1,1) = P11 + velObsVar;
+	_ekf_gsf[model_index].S(0,1) = P01;
+	_ekf_gsf[model_index].S(1,0) = P10;
+
+	// Update the inverse of the innovation covariance matrix S_inverse
+	updateInnovCovMatInv(model_index);
+
+	// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
+	float innov_comp_scale_factor = 1.0f;
+
+	// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
+	const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
+
+	// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
+	// This protects from large measurement spikes
+	if (test_ratio > 25.0f) {
+		innov_comp_scale_factor = sqrtf(25.0f / test_ratio);
+	}
+
+	// calculate Kalman gain K  nd covariance matrix P
+	// autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt
+	// and https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPmat.txt
+	const float t2 = P00*velObsVar;
+ 	const float t3 = P11*velObsVar;
+	const float t4 = velObsVar*velObsVar;
+	const float t5 = P00*P11;
+	const float t9 = P01*P10;
+	const float t6 = t2+t3+t4+t5-t9;
+	float t7;
+	if (fabsf(t6) > 1e-6f) {
+		t7 = 1.0f/t6;
+	} else {
+		// skip this fusion step
+		return false;
+	}
+	const float t8 = P11+velObsVar;
+	const float t10 = P00+velObsVar;
+
+	matrix::Matrix<float, 3, 2> K;
+ 	K(0,0) = -P01*P10*t7+P00*t7*t8;
+	K(0,1) = -P00*P01*t7+P01*t7*t10;
+	K(1,0) = -P10*P11*t7+P10*t7*t8;
+	K(1,1) = -P01*P10*t7+P11*t7*t10;
+	K(2,0) = -P10*P21*t7+P20*t7*t8;
+	K(2,1) = -P01*P20*t7+P21*t7*t10;
+
+	const float t11 = P00*P01*t7;
+	const float t15 = P01*t7*t10;
+	const float t12 = t11-t15;
+	const float t13 = P01*P10*t7;
+	const float t16 = P00*t7*t8;
+	const float t14 = t13-t16;
+	const float t17 = t8*t12;
+	const float t18 = P01*t14;
+	const float t19 = t17+t18;
+	const float t20 = t10*t14;
+	const float t21 = P10*t12;
+	const float t22 = t20+t21;
+	const float t27 = P11*t7*t10;
+	const float t23 = t13-t27;
+	const float t24 = P10*P11*t7;
+	const float t26 = P10*t7*t8;
+	const float t25 = t24-t26;
+	const float t28 = t8*t23;
+	const float t29 = P01*t25;
+	const float t30 = t28+t29;
+	const float t31 = t10*t25;
+	const float t32 = P10*t23;
+	const float t33 = t31+t32;
+	const float t34 = P01*P20*t7;
+	const float t38 = P21*t7*t10;
+	const float t35 = t34-t38;
+	const float t36 = P10*P21*t7;
+	const float t39 = P20*t7*t8;
+	const float t37 = t36-t39;
+	const float t40 = t8*t35;
+	const float t41 = P01*t37;
+	const float t42 = t40+t41;
+	const float t43 = t10*t37;
+	const float t44 = P10*t35;
+	const float t45 = t43+t44;
+
+	const float min_var = 1e-6f;
+	_ekf_gsf[model_index].P(0,0) = fmaxf(P00-t12*t19-t14*t22 , min_var);
+	_ekf_gsf[model_index].P(0,1) = P01-t19*t23-t22*t25;
+	_ekf_gsf[model_index].P(0,2) = P02-t19*t35-t22*t37;
+	_ekf_gsf[model_index].P(1,0) = P10-t12*t30-t14*t33;
+	_ekf_gsf[model_index].P(1,1) = fmaxf(P11-t23*t30-t25*t33 , min_var);
+	_ekf_gsf[model_index].P(1,2) = P12-t30*t35-t33*t37;
+	_ekf_gsf[model_index].P(2,0) = P20-t12*t42-t14*t45;
+	_ekf_gsf[model_index].P(2,1) = P21-t23*t42-t25*t45;
+	_ekf_gsf[model_index].P(2,2) = fmaxf(P22-t35*t42-t37*t45 , min_var);
+
+	// force symmetry
+	_ekf_gsf[model_index].P.makeBlockSymmetric<3>(0);
+
+	// Correct the state vector and capture the change in yaw angle
+	const float oldYaw = _ekf_gsf[model_index].X(2);
+	_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
+	float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
+
+	// apply the change in yaw angle to the AHRS
+	// take advantage of sparseness in the yaw rotation matrix
+	const float cosYaw = cosf(yawDelta);
+	const float sinYaw = sinf(yawDelta);
+	float  R_prev[2][3];
+	memcpy(&R_prev, &_ahrs_ekf_gsf[model_index].R, sizeof(R_prev));
+	_ahrs_ekf_gsf[model_index].R(0,0) = R_prev[0][0] * cosYaw - R_prev[1][0] * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(0,1) = R_prev[0][1] * cosYaw - R_prev[1][1] * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(0,2) = R_prev[0][2] * cosYaw - R_prev[1][2] * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(1,0) = R_prev[0][0] * sinYaw + R_prev[1][0] * cosYaw;
+	_ahrs_ekf_gsf[model_index].R(1,1) = R_prev[0][1] * sinYaw + R_prev[1][1] * cosYaw;
+	_ahrs_ekf_gsf[model_index].R(1,2) = R_prev[0][2] * sinYaw + R_prev[1][2] * cosYaw;
+
+	return true;
+}
+
+void EKFGSF_yaw::initialiseEKFGSF()
+{
+	_gsf_yaw = 0.0f;
+	_ekf_gsf_vel_fuse_started = false;
+	_gsf_yaw_variance = M_PI_2_F * M_PI_2_F;
+	memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
+	const float yaw_increment = 2.0f * M_PI_F / (float)N_MODELS_EKFGSF;
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+		// evenly space initial yaw estimates in the region between +-Pi
+		_ekf_gsf[model_index].X(2) = -M_PI_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
+
+		// All filter models start with the same weight
+		_ekf_gsf[model_index].W = 1.0f / (float)N_MODELS_EKFGSF;
+
+		// take velocity states and corresponding variance from last meaurement
+		_ekf_gsf[model_index].X(0) = _vel_NE(0);
+		_ekf_gsf[model_index].X(1) = _vel_NE(1);
+		_ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);
+		_ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);
+
+		// use half yaw interval for yaw uncertainty
+		_ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);
+	}
+}
+
+float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
+{
+	// calculate inv(S) * innovation
+	const matrix::Vector2f tempVec = _ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov;
+
+	// calculate transpose(innovation) * inv(S) * innovation
+	// * operator is overloaded to provide a dot product
+	const float normDist = _ekf_gsf[model_index].innov * tempVec;
+
+	return M_TWOPI_INV * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
+}
+
+void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index)
+{
+	// calculate determinant for innovation covariance matrix
+	const float t2 = _ekf_gsf[model_index].S(0,0) * _ekf_gsf[model_index].S(1,1);
+	const float t5 = _ekf_gsf[model_index].S(0,1) * _ekf_gsf[model_index].S(1,0);
+	const float t3 = t2 - t5;
+
+	// calculate determinant inverse and protect against badly conditioned matrix
+	_ekf_gsf[model_index].S_det_inverse = 1.0f / fmaxf(t3 , 1e-12f);
+
+	// calculate inv(S)
+	_ekf_gsf[model_index].S_inverse(0,0) =   _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,1);
+	_ekf_gsf[model_index].S_inverse(1,1) =   _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,0);
+	_ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,1);
+	_ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,0);
+
+}
+
+bool EKFGSF_yaw::getLogData(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])
+{
+	if (_ekf_gsf_vel_fuse_started) {
+		*yaw_composite = _gsf_yaw;
+		*yaw_variance = _gsf_yaw_variance;
+		for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+			yaw[model_index] = _ekf_gsf[model_index].X(2);
+			innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
+			innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
+			weight[model_index] = _ekf_gsf[model_index].W;
+		}
+		return true;
+	}
+	return false;
+}
+
+Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312)
+{
+		// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
+		const float c2 = cosf(rot312(2));
+		const float s2 = sinf(rot312(2));
+		const float s1 = sinf(rot312(1));
+		const float c1 = cosf(rot312(1));
+		const float s0 = sinf(rot312(0));
+		const float c0 = cosf(rot312(0));
+
+		Dcmf R;
+		R(0, 0) = c0 * c2 - s0 * s1 * s2;
+		R(1, 1) = c0 * c1;
+		R(2, 2) = c2 * c1;
+		R(0, 1) = -c1 * s0;
+		R(0, 2) = s2 * c0 + c2 * s1 * s0;
+		R(1, 0) = c2 * s0 + s2 * s1 * c0;
+		R(1, 2) = s0 * s2 - s1 * c0 * c2;
+		R(2, 0) = -s2 * c1;
+		R(2, 1) = s1;
+
+		return R;
+}
+
+void EKFGSF_yaw::ahrsCalcAccelGain()
+{
+	// calculate common values used by the AHRS complementary filter models
+	_ahrs_accel_norm = _ahrs_accel.norm();
+
+	// Calculate the acceleration fusion gain using a continuous function that is unity at 1g and zero
+	// at the min and mas g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
+	// acceleration correction as higher and more sustained g will be experienced.
+	// Use a quadratic finstead of linear unction to prevent vibration around 1g reducing the tilt correction effectiveness.
+	float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G;
+	if (accel_g > 1.0f) {
+		if (_true_airspeed > FLT_EPSILON && accel_g < 2.0f) {
+			_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f - accel_g);
+		} else if (accel_g < 1.5f) {
+			_ahrs_accel_fusion_gain = _tilt_gain * sq(3.0f - 2.0f * accel_g);
+		} else {
+			_ahrs_accel_fusion_gain = 0.0f;
+		}
+	} else if (accel_g > 0.5f) {
+		_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f * accel_g - 1.0f);
+	} else {
+		_ahrs_accel_fusion_gain = 0.0f;
+	}
+}
+
+Matrix3f EKFGSF_yaw::ahrsPredictRotMat(Matrix3f &R, Vector3f &g)
+{
+	Matrix3f ret = R;
+	ret(0,0) += R(0,1) * g(2) - R(0,2) * g(1);
+	ret(0,1) += R(0,2) * g(0) - R(0,0) * g(2);
+	ret(0,2) += R(0,0) * g(1) - R(0,1) * g(0);
+	ret(1,0) += R(1,1) * g(2) - R(1,2) * g(1);
+	ret(1,1) += R(1,2) * g(0) - R(1,0) * g(2);
+	ret(1,2) += R(1,0) * g(1) - R(1,1) * g(0),
+        ret(2,0) += R(2,1) * g(2) - R(2,2) * g(1);
+	ret(2,1) += R(2,2) * g(0) - R(2,0) * g(2);
+	ret(2,2) += R(2,0) * g(1) - R(2,1) * g(0);
+
+	// Renormalise rows
+	float rowLengthSq;
+	for (uint8_t r = 0; r < 3; r++) {
+		rowLengthSq = ret.row(r).norm_squared();
+		if (rowLengthSq > FLT_EPSILON) {
+			// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
+			const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
+			ret(r,0) *= rowLengthInv;
+			ret(r,1) *= rowLengthInv;
+			ret(r,2) *= rowLengthInv;
+		}
+        }
+
+	return ret;
+}
+
+bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance)
+{
+	if(_ekf_gsf_vel_fuse_started) {
+		*yaw = _gsf_yaw;
+		*yaw_variance = _gsf_yaw_variance;
+		return true;
+	}
+	return false;
+}
+
+void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)
+{
+	_vel_NE = velocity;
+	_vel_accuracy = accuracy;
+	_vel_data_updated = true;
+}

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 @@ 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();
+
 };

EKF/ekf_helper.cpp

@@ -426,56 +426,43 @@ bool Ekf::realignYawGPS()
 				_control_status.flags.mag_fault = true;
 			}
 
-			// save a copy of the quaternion state for later use in calculating the amount of reset change
+			// calculate new yaw estimate
-			const Quatf quat_before_reset = _state.quat_nominal;
+			float yaw_new;
-
-			// update transformation matrix from body to world frame using the current state estimate
-			_R_to_earth = Dcmf(_state.quat_nominal);
-
-			// get quaternion from existing filter states and calculate roll, pitch and yaw angles
-			Eulerf euler321(_state.quat_nominal);
-
-			// apply yaw correction
 			if (!_control_status.flags.mag_aligned_in_flight) {
 				// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
 				// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
-				euler321(2) += courseYawError;
+				Eulerf euler321(_state.quat_nominal);
+				yaw_new = euler321(2) + courseYawError;
 				_control_status.flags.mag_aligned_in_flight = true;
 
 			} else if (_control_status.flags.wind) {
 				// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
 				// aligned with the wind relative GPS velocity vector
-				euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
+				yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
 						     (_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
 
 			} else {
 				// we don't have wind estimates, so align yaw to the GPS velocity vector
-				euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
+				yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
 
 			}
 
-			// calculate new filter quaternion states using corrected yaw angle
-			_state.quat_nominal = Quatf(euler321);
-			_R_to_earth = Dcmf(_state.quat_nominal);
-			uncorrelateQuatFromOtherStates();
-
-			// If heading was bad, then we also need to reset the velocity and position states
-			_velpos_reset_request = badMagYaw;
-
-			// Use the last magnetometer measurements to reset the field states
-			_state.mag_B.zero();
-			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
-
 			// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
 			float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4,4) + P(5,5);
 			float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
+			float yaw_variance_new = sq(asinf(sineYawError));
 
-			// adjust the quaternion covariances estimated yaw error
+			// Apply updated yaw and yaw variance to states and covariances
-			increaseQuatYawErrVariance(sq(asinf(sineYawError)));
+			resetQuatStateYaw(yaw_new, yaw_variance_new, true);
 
-			// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
+			// Use the last magnetometer measurements to reset the field states
+			_state.mag_B.zero();
+			_R_to_earth = Dcmf(_state.quat_nominal);
+			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
+
+			// reset the corresponding rows and columns in the covariance matrix and set the
+			// variances on the magnetic field states to the measurement variance
 			clearMagCov();
-
 			if (_control_status.flags.mag_3D) {
 				for (uint8_t index = 16; index <= 21; index ++) {
 					P(index,index) = sq(_params.mag_noise);
@@ -488,20 +475,8 @@ bool Ekf::realignYawGPS()
 			// record the start time for the magnetic field alignment
 			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
-			// calculate the amount that the quaternion has changed by
+			// If heading was bad, then we also need to reset the velocity and position states
-			_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
+			_velpos_reset_request = badMagYaw;
-
-			// add the reset amount to the output observer buffered data
-			for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-				_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
-			}
-
-			// apply the change in attitude quaternion to our newest quaternion estimate
-			// which was already taken out from the output buffer
-			_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
-
-			// capture the reset event
-			_state_reset_status.quat_counter++;
 
 			return true;
 
@@ -549,126 +524,61 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		return false;
 	}
 
-	// save a copy of the quaternion state for later use in calculating the amount of reset change
+	// calculate the observed yaw angle and yaw variance
-	const Quatf quat_before_reset = _state.quat_nominal;
+	float yaw_new;
-	Quatf quat_after_reset = _state.quat_nominal;
+	float yaw_new_variance = 0.0f;
+	if (_control_status.flags.ev_yaw) {
+		// convert the observed quaternion to a rotation matrix
+		const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
 
-	// update transformation matrix from body to world frame using the current estimate
+		// calculate the yaw angle for a 312 sequence
-	_R_to_earth = Dcmf(_state.quat_nominal);
+		yaw_new = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
 
-	// calculate the initial quaternion
+		if (increase_yaw_var) {
-	// determine if a 321 or 312 Euler sequence is best
+			yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
-	if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
-		// use a 321 sequence
-
-		// rotate the magnetometer measurement into earth frame
-		Eulerf euler321(_state.quat_nominal);
-
-		// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
-		euler321(2) = 0.0f;
-
-		// calculate the observed yaw angle
-		if (_control_status.flags.ev_yaw) {
-			// convert the observed quaternion to a rotation matrix
-			const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
-			// calculate the yaw angle for a 312 sequence
-			euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
-
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
-			const Dcmf R_to_earth(euler321);
-			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			const Vector3f mag_earth_pred = R_to_earth * mag_init;
-			// the angle of the projection onto the horizontal gives the yaw angle
-			euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
-
-		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
-			// we are operating without knowing the earth frame yaw angle
-			return true;
-
-		} else {
-			// there is no yaw observation
-			return false;
 		}
 
-		// calculate initial quaternion states for the ekf
+	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
-		// we don't change the output attitude to avoid jumps
+		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		quat_after_reset = Quatf(euler321);
+		Dcmf R_to_earth;
+		if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+			// rolled more than pitched so use 321 rotation order
+			Eulerf euler321(_state.quat_nominal);
+			euler321(2) = 0.0f;
+			R_to_earth = Dcmf(euler321);
+
+		} else {
+			// pitched more than rolled so use 312 rotation order
+			Vector3f rotVec312;
+			rotVec312(0) = 0.0f;  // first rotation (yaw)
+			rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
+			rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));  // third rotation (pitch)
+			R_to_earth = taitBryan312ToRotMat(rotVec312);
+
+		}
+
+		// the angle of the projection onto the horizontal gives the yaw angle
+		const Vector3f mag_earth_pred = R_to_earth * mag_init;
+		yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+
+		if (increase_yaw_var) {
+			yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+		}
+
+	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
+		// we are operating without knowing the earth frame yaw angle
+		return true;
 
 	} else {
-		// use a 312 sequence
+		// there is no yaw observation
-
+		return false;
-		// Calculate the 312 sequence euler angles that rotate from earth to body frame
-		// See http://www.atacolorado.com/eulersequences.doc
-		Vector3f euler312;
-		euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));  // first rotation (yaw)
-		euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));  // third rotation (pitch)
-
-		// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
-		euler312(0) = 0.0f;
-
-		// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
-		float c2 = cosf(euler312(2));
-		float s2 = sinf(euler312(2));
-		float s1 = sinf(euler312(1));
-		float c1 = cosf(euler312(1));
-		float s0 = sinf(euler312(0));
-		float c0 = cosf(euler312(0));
-
-		Dcmf R_to_earth;
-		R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
-		R_to_earth(1, 1) = c0 * c1;
-		R_to_earth(2, 2) = c2 * c1;
-		R_to_earth(0, 1) = -c1 * s0;
-		R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
-		R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
-		R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
-		R_to_earth(2, 0) = -s2 * c1;
-		R_to_earth(2, 1) = s1;
-
-		// calculate the observed yaw angle
-		if (_control_status.flags.ev_yaw) {
-			// convert the observed quaternion to a rotation matrix
-			const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);
-			// calculate the yaw angle for a 312 sequence
-			euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
-
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
-			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			const Vector3f mag_earth_pred = R_to_earth * mag_init;
-			// the angle of the projection onto the horizontal gives the yaw angle
-			euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
-
-		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
-			// we are operating without knowing the earth frame yaw angle
-			return true;
-
-		} else {
-			// there is no yaw observation
-			return false;
-		}
-
-		// re-calculate the rotation matrix using the updated yaw angle
-		s0 = sinf(euler312(0));
-		c0 = cosf(euler312(0));
-		R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
-		R_to_earth(1, 1) = c0 * c1;
-		R_to_earth(2, 2) = c2 * c1;
-		R_to_earth(0, 1) = -c1 * s0;
-		R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
-		R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
-		R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
-		R_to_earth(2, 0) = -s2 * c1;
-		R_to_earth(2, 1) = s1;
-
-		// calculate initial quaternion states for the ekf
-		// we don't change the output attitude to avoid jumps
-		quat_after_reset = Quatf(R_to_earth);
 	}
 
+	// update quaternion states and corresponding covarainces
+	resetQuatStateYaw(yaw_new, yaw_new_variance, update_buffer);
+
 	// set the earth magnetic field states using the updated rotation
-	const Dcmf R_to_earth_after(quat_after_reset);
+	_state.mag_I = _R_to_earth * mag_init;
-	_state.mag_I = R_to_earth_after * mag_init;
 
 	// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
 	clearMagCov();
@@ -685,42 +595,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 	// record the time for the magnetic field alignment event
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 
-	// calculate the amount that the quaternion has changed by
-	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
-
-	// update quaternion states
-	_state.quat_nominal = quat_after_reset;
-	_R_to_earth = Dcmf(_state.quat_nominal);
-	uncorrelateQuatFromOtherStates();
-
-	// record the state change
-	_state_reset_status.quat_change = q_error;
-
-	if (increase_yaw_var) {
-		// update the yaw angle variance using the variance of the measurement
-		if (_control_status.flags.ev_yaw) {
-			// using error estimate from external vision data
-			increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
-		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
-			// using magnetic heading tuning parameter
-			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
-		}
-	}
-
-	if (update_buffer) {
-		// add the reset amount to the output observer buffered data
-		for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-			_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
-		}
-
-		// apply the change in attitude quaternion to our newest quaternion estimate
-		// which was already taken out from the output buffer
-		_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
-	}
-
-	// capture the reset event
-	_state_reset_status.quat_counter++;
-
 	return true;
 }
 
@@ -1370,8 +1244,8 @@ void Ekf::fuse(float *K, float innovation)
 
 void Ekf::uncorrelateQuatFromOtherStates()
 {
-	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;	
+	P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
-	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;	
+	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
 }
 
 bool Ekf::global_position_is_valid()
@@ -1819,3 +1693,148 @@ void Ekf::stopFlowFusion()
 	memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
 	memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
 }
+
+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
+{
+		// save a copy of the quaternion state for later use in calculating the amount of reset change
+		const Quatf quat_before_reset = _state.quat_nominal;
+
+		// update transformation matrix from body to world frame using the current estimate
+		_R_to_earth = Dcmf(_state.quat_nominal);
+
+		// update the rotation matrix using the new yaw value
+		// determine if a 321 or 312 Euler sequence is best
+		if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
+			// use a 321 sequence
+			Eulerf euler321(_R_to_earth);
+			euler321(2) = yaw;
+			_R_to_earth = Dcmf(euler321);
+
+		} else {
+			// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
+			// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
+			// to avoid gimbal lock
+			Vector3f rot312;
+			rot312(0) = yaw;
+			rot312(1) = asinf(_R_to_earth(2, 1));
+			rot312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));
+			_R_to_earth = taitBryan312ToRotMat(rot312);
+
+		}
+
+		// calculate the amount that the quaternion has changed by
+		const Quatf quat_after_reset = _R_to_earth;
+		Quatf q_error =  quat_after_reset * quat_before_reset.inversed();
+		q_error.normalize();
+
+		// update quaternion states
+		_state.quat_nominal = quat_after_reset;
+		uncorrelateQuatFromOtherStates();
+
+		// record the state change
+		_state_reset_status.quat_change = q_error;
+
+		// update the yaw angle variance
+		if (yaw_variance > FLT_EPSILON) {
+			increaseQuatYawErrVariance(yaw_variance);
+		}
+
+		// add the reset amount to the output observer buffered data
+		if (update_buffer) {
+			for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+				_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
+			}
+
+			// apply the change in attitude quaternion to our newest quaternion estimate
+			// which was already taken out from the output buffer
+			_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+
+		}
+
+		// capture the reset event
+		_state_reset_status.quat_counter++;
+}
+
+// Reset main nav filter yaw to value from EKF-GSF and reset velocity and position to GPS
+bool Ekf::resetYawToEKFGSF()
+{
+	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
+	// data and the yaw estimate has converged
+	float new_yaw, new_yaw_variance;
+	if (yawEstimator.getYawData(&new_yaw, &new_yaw_variance) && new_yaw_variance < sq(_params.EKFGSF_yaw_err_max)) {
+
+		resetQuatStateYaw(new_yaw, new_yaw_variance, true);
+
+		// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
+		resetVelocity();
+		resetPosition();
+
+		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
+		// and cause another navigation failure
+		_control_status.flags.mag_fault = true;
+
+		// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
+		_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+
+		ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
+
+		return true;
+	}
+
+	return false;
+
+}
+
+void Ekf::requestEmergencyNavReset(uint8_t counter)
+{
+	if (counter > _yaw_extreset_counter) {
+		_yaw_extreset_counter = counter;
+		_do_emergency_yaw_reset = true;
+	}
+}
+
+bool Ekf::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])
+{
+	return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight);
+}
+
+Dcmf Ekf::taitBryan312ToRotMat(const Vector3f &rot312)
+{
+		// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
+		const float c2 = cosf(rot312(2)); // third rotation is pitch
+		const float s2 = sinf(rot312(2));
+		const float s1 = sinf(rot312(1)); // second rotation is roll
+		const float c1 = cosf(rot312(1));
+		const float s0 = sinf(rot312(0)); // first rotation is yaw
+		const float c0 = cosf(rot312(0));
+
+		Dcmf R;
+		R(0, 0) = c0 * c2 - s0 * s1 * s2;
+		R(1, 1) = c0 * c1;
+		R(2, 2) = c2 * c1;
+		R(0, 1) = -c1 * s0;
+		R(0, 2) = s2 * c0 + c2 * s1 * s0;
+		R(1, 0) = c2 * s0 + s2 * s1 * c0;
+		R(1, 2) = s0 * s2 - s1 * c0 * c2;
+		R(2, 0) = -s2 * c1;
+		R(2, 1) = s1;
+
+		return R;
+}
+
+void Ekf::runYawEKFGSF()
+{
+	float TAS;
+	if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) {
+		TAS = _params.EKFGSF_tas_default;
+	} else {
+		TAS = _airspeed_sample_delayed.true_airspeed;
+	}
+	yawEstimator.update(_imu_sample_delayed.delta_ang, _imu_sample_delayed.delta_vel, _imu_sample_delayed.delta_ang_dt, _imu_sample_delayed.delta_vel_dt, _control_status.flags.in_air, TAS);
+
+	// basic sanity check on GPS velocity data
+	if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && isfinite(_gps_sample_delayed.vel(0)) && isfinite(_gps_sample_delayed.vel(1))) {
+		Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
+		yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);
+	}
+}

EKF/estimator_interface.h

@@ -46,6 +46,7 @@
 #include "RingBuffer.h"
 #include "AlphaFilter.hpp"
 #include "imu_down_sampler.hpp"
+#include "EKFGSF_yaw.h"
 
 #include <geo/geo.h>
 #include <matrix/math.hpp>
@@ -414,6 +415,13 @@ public:
 	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
 	static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
 
+	// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
+	// argment should be incremented only when a new reset is required
+	virtual void requestEmergencyNavReset(uint8_t counter) = 0;
+
+	// get ekf-gsf debug data
+	virtual 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]) = 0;
+
 protected:
 
 	parameters _params;		// filter parameters
@@ -538,6 +546,9 @@ protected:
 	RingBuffer<dragSample> _drag_buffer;
 	RingBuffer<auxVelSample> _auxvel_buffer;
 
+	// yaw estimator instance
+	EKFGSF_yaw yawEstimator;
+
 	// observation buffer final allocation failed
 	bool _gps_buffer_fail{false};
 	bool _mag_buffer_fail{false};

EKF/mag_fusion.cpp

@@ -624,46 +624,20 @@ void Ekf::fuseHeading()
 
 		}
 
-
+		// Use a Tait-Bryan 312 rotation sequence
-
+		Vector3f rotVec312;
-		/* Calculate the 312 sequence euler angles that rotate from earth to body frame
+		predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));
-		 * Derived from https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
+		rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
-		 * Body to nav frame transformation using a yaw-roll-pitch rotation sequence is given by:
+		rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		 *
+		rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
-		[ cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), -cos(roll)*sin(yaw), cos(yaw)*sin(pitch) + cos(pitch)*sin(roll)*sin(yaw)]
-		[ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll),  cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)]
-		[                               -cos(roll)*sin(pitch),           sin(roll),                                cos(pitch)*cos(roll)]
-		*/
-		float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
-		const float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		const float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
-
-		predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
 
 		// calculate the observed yaw angle
 		if (_control_status.flags.mag_hdg) {
-			// Set the first rotation (yaw) to zero and rotate the measurements into earth frame
-			yaw = 0.0f;
 
 			// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
-			// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
+			// with yaw angle set to to zero
-			Dcmf R_to_earth;
-			float sy = sinf(yaw);
-			float cy = cosf(yaw);
-			float sp = sinf(pitch);
-			float cp = cosf(pitch);
-			float sr = sinf(roll);
-			float cr = cosf(roll);
-			R_to_earth(0,0) = cy*cp-sy*sp*sr;
-			R_to_earth(0,1) = -sy*cr;
-			R_to_earth(0,2) = cy*sp+sy*cp*sr;
-			R_to_earth(1,0) = sy*cp+cy*sp*sr;
-			R_to_earth(1,1) = cy*cr;
-			R_to_earth(1,2) = sy*sp-cy*cp*sr;
-			R_to_earth(2,0) = -sp*cr;
-			R_to_earth(2,1) = sr;
-			R_to_earth(2,2) = cp*cr;
 
+			const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			if (_control_status.flags.mag_3D) {
 				// don't apply bias corrections if we are learning them