ekf2: cleanup zero innovation heading fusion

- refactor updateQuaternion() to compute the yaw jacobian directly (respecting  the rotation sequence determination)
 - fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
 - clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
 - when at rest continue fusing last static heading with very low variance even if other heading sources are active

src/modules/ekf2/CMakeLists.txt

@@ -72,6 +72,7 @@ px4_add_module(
 		EKF/terrain_estimator.cpp
 		EKF/utils.cpp
 		EKF/vel_pos_fusion.cpp
+		EKF/zero_innovation_heading_update.cpp
 		EKF/zero_velocity_update.cpp
 
 		EKF2.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -57,6 +57,7 @@ add_library(ecl_EKF
 	terrain_estimator.cpp
 	utils.cpp
 	vel_pos_fusion.cpp
+	zero_innovation_heading_update.cpp
 	zero_velocity_update.cpp
 )
 

src/modules/ekf2/EKF/control.cpp

@@ -190,6 +190,8 @@ void Ekf::controlFusionModes()
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
 
+	controlZeroInnovationHeadingUpdate();
+
 	controlZeroVelocityUpdate();
 
 	// Fake position measurement for constraining drift when no other velocity or position measurements
@@ -364,14 +366,11 @@ void Ekf::controlExternalVisionFusion()
 				resetYawToEv();
 			}
 
-			if (shouldUse321RotationSequence(_R_to_earth)) {
-				float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
-				fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
+			float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
 
-			} else {
-				float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
-				fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
-			}
+			float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+			float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
+			fuseYaw(innovation, obs_var);
 		}
 
 		// record observation and estimate for use next time

src/modules/ekf2/EKF/ekf.h

@@ -412,6 +412,8 @@ private:
 	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of vertical position measurements was performed (uSec)
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
+	uint64_t _time_last_heading_fuse{0};
+
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
@@ -605,28 +607,10 @@ private:
 	// ekf sequential fusion of magnetometer measurements
 	void fuseMag(const Vector3f &mag);
 
-	// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
-	void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
-
-	// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
-	// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
-	// yaw_variance : variance of the yaw angle observation (rad^2)
-	// zero_innovation : Fuse data with innovation set to zero
-	bool fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
-
-	// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
-	// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
-	// yaw_variance : variance of the yaw angle observation (rad^2)
-	// zero_innovation : Fuse data with innovation set to zero
-	bool fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
-
-	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
+	// update quaternion states and covariances using a yaw innovation and yaw observation variance
 	// innovation : prediction - measurement
 	// variance : observaton variance
-	// gate_sigma : innovation consistency check gate size (Sigma)
-	// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
-	bool updateQuaternion(const float innovation, const float variance, const float gate_sigma,
-			      const Vector4f &yaw_jacobian);
+	bool fuseYaw(const float innovation, const float variance);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();
@@ -910,6 +894,8 @@ private:
 
 	void controlZeroVelocityUpdate();
 
+	void controlZeroInnovationHeadingUpdate();
+
 	// control fusion of auxiliary velocity observations
 	void controlAuxVelFusion();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1262,6 +1262,10 @@ void Ekf::stopMag3DFusion()
 	// save covariance data for re-use if currently doing 3-axis fusion
 	if (_control_status.flags.mag_3D) {
 		saveMagCovData();
+
+		// we are no longer using 3-axis fusion so set the reported test levels to zero
+		_mag_test_ratio.setZero();
+
 		_control_status.flags.mag_3D = false;
 	}
 }
@@ -1775,6 +1779,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 
 	}
 
+	_last_static_yaw = NAN;
+
 	// capture the reset event
 	_state_reset_status.quat_counter++;
 }

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -146,9 +146,6 @@ void Ekf::fuseGpsYaw()
 	// innovation test ratio
 	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
 
-	// we are no longer using 3-axis fusion so set the reported test levels to zero
-	_mag_test_ratio.setZero();
-
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 		return;
@@ -159,8 +156,8 @@ void Ekf::fuseGpsYaw()
 
 	_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
 
-	if (!_control_status.flags.in_air
-	    && fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) {
+	if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
+		&& !_control_status.flags.in_air && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)) {
 
 		// A constant large signed test ratio is a sign of wrong gyro bias
 		// Reset the yaw gyro variance to converge faster and avoid
@@ -192,6 +189,7 @@ void Ekf::fuseGpsYaw()
 
 	if (is_fused) {
 		_time_last_gps_yaw_fuse = _time_last_imu;
+		_time_last_heading_fuse = _time_last_imu;
 	}
 }
 

src/modules/ekf2/EKF/mag_control.cpp

@@ -78,25 +78,11 @@ void Ekf::controlMagFusion()
 		_num_bad_flight_yaw_events = 0;
 	}
 
-	// When operating without a magnetometer and no other source of yaw aiding is active,
-	// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
-	// ground and to prevent uncontrolled yaw variance growth
-	// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
 	if (_params.mag_fusion_type >= MagFuseType::NONE
 	    || _control_status.flags.mag_fault
 	    || !_control_status.flags.tilt_align) {
 
 		stopMagFusion();
-
-		if (noOtherYawAidingThanMag()) {
-			// TODO: setting _is_yaw_fusion_inhibited to true is required to tell
-			// fuseHeading to perform a "zero innovation heading fusion"
-			// We should refactor it to avoid using this flag here
-			_is_yaw_fusion_inhibited = true;
-			fuseHeading();
-			_is_yaw_fusion_inhibited = false;
-		}
-
 		return;
 	}
 
@@ -347,16 +333,19 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 	if (_control_status.flags.mag_3D) {
 		run3DMagAndDeclFusions(mag);
 
-	} else if (_control_status.flags.mag_hdg) {
+	} else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) {
 		// Rotate the measurements into earth frame using the zero yaw angle
 		Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
 
 		Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
 
 		// the angle of the projection onto the horizontal gives the yaw angle
+		// calculate the yaw innovation and wrap to the interval between +-pi
 		float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
-		fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
+		float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+		float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
+		fuseYaw(innovation, obs_var);
 	}
 }
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -429,7 +429,8 @@ void Ekf::fuseMag(const Vector3f &mag)
 	}
 }
 
-bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
+// update quaternion states and covariances using the yaw innovation and yaw observation variance
+bool Ekf::fuseYaw(const float innovation, const float variance)
 {
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);
@@ -437,170 +438,113 @@ bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	const float q2 = _state.quat_nominal(2);
 	const float q3 = _state.quat_nominal(3);
 
-	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
-	const float measurement = wrap_pi(yaw);
-
-	// calculate 321 yaw observation matrix
 	// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
 	bool canUseA = false;
-	const float SA0 = 2*q3;
-	const float SA1 = 2*q2;
-	const float SA2 = SA0*q0 + SA1*q1;
-	const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
-	float SA4, SA5_inv;
+	bool canUseB = false;
+
+	float SA0;
+	float SA1;
+	float SA2;
+	float SA3;
+	float SA4;
+	float SA5_inv;
+
+	float SB0;
+	float SB1;
+	float SB2;
+	float SB3;
+	float SB4;
+	float SB5_inv;
+
+	const bool fuse_yaw_321 = shouldUse321RotationSequence(_R_to_earth);
+
+	if (fuse_yaw_321) {
+		// calculate 321 yaw observation matrix
+		SA0 = 2*q3;
+		SA1 = 2*q2;
+		SA2 = SA0*q0 + SA1*q1;
+		SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+
+		SB0 = 2*q0;
+		SB1 = 2*q1;
+		SB2 = SB0*q3 + SB1*q2;
+		SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+
+	} else {
+		// calculate 312 yaw observation matrix
+		SA0 = 2*q3;
+		SA1 = 2*q2;
+		SA2 = SA0*q0 - SA1*q1;
+		SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
+
+		SB0 = 2*q0;
+		SB1 = 2*q1;
+		SB2 = -SB0*q3 + SB1*q2;
+		SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
+	}
 
 	if (sq(SA3) > 1e-6f) {
-		SA4 = 1.0F/sq(SA3);
-		SA5_inv = sq(SA2)*SA4 + 1;
+		SA4 = 1.f/sq(SA3);
+		SA5_inv = sq(SA2)*SA4 + 1.f;
 		canUseA = fabsf(SA5_inv) > 1e-6f;
 	}
 
-	bool canUseB = false;
-	const float SB0 = 2*q0;
-	const float SB1 = 2*q1;
-	const float SB2 = SB0*q3 + SB1*q2;
-	const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
-	float SB3, SB5_inv;
-
 	if (sq(SB2) > 1e-6f) {
-		SB3 = 1.0F/sq(SB2);
-		SB5_inv = SB3*sq(SB4) + 1;
+		SB3 = 1.f/sq(SB2);
+		SB5_inv = SB3*sq(SB4) + 1.f;
 		canUseB = fabsf(SB5_inv) > 1e-6f;
 	}
 
 	Vector4f H_YAW;
 
 	if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
-		const float SA5 = 1.0F/SA5_inv;
-		const float SA6 = 1.0F/SA3;
+		const float SA5 = 1.f/SA5_inv;
+		const float SA6 = 1.f/SA3;
 		const float SA7 = SA2*SA4;
 		const float SA8 = 2*SA7;
 		const float SA9 = 2*SA6;
 
-		H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
-		H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
-		H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
-		H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
+		if (fuse_yaw_321) {
+			// calculate 321 yaw observation matrix
+			H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
+			H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
+			H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
+			H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
+
+		} else {
+			// calculate 312 yaw observation matrix
+			H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
+			H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
+			H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
+			H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
+		}
+
 	} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
-		const float SB5 = 1.0F/SB5_inv;
-		const float SB6 = 1.0F/SB2;
+		const float SB5 = 1.f/SB5_inv;
+		const float SB6 = 1.f/SB2;
 		const float SB7 = SB3*SB4;
 		const float SB8 = 2*SB7;
 		const float SB9 = 2*SB6;
 
-		H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
-		H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
-		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
-		H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
+		if (fuse_yaw_321) {
+			// calculate 321 yaw observation matrix
+			H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
+			H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
+			H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
+			H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
+
+		} else {
+			// calculate 312 yaw observation matrix
+			H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
+			H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
+			H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
+			H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
+		}
+
 	} else {
 		return false;
 	}
 
-	// calculate the yaw innovation and wrap to the interval between +-pi
-	float innovation;
-
-	if (zero_innovation) {
-		innovation = 0.0f;
-
-	} else {
-		innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
-	}
-
-	// define the innovation gate size
-	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-
-	// Update the quaternion states and covariance matrix
-	return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
-}
-
-bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
-{
-	// assign intermediate state variables
-	const float q0 = _state.quat_nominal(0);
-	const float q1 = _state.quat_nominal(1);
-	const float q2 = _state.quat_nominal(2);
-	const float q3 = _state.quat_nominal(3);
-
-	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
-	const float measurement = wrap_pi(yaw);
-
-	// calculate 312 yaw observation matrix
-	// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
-	bool canUseA = false;
-	const float SA0 = 2*q3;
-	const float SA1 = 2*q2;
-	const float SA2 = SA0*q0 - SA1*q1;
-	const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
-	float SA4, SA5_inv;
-
-	if (sq(SA3) > 1e-6f) {
-		SA4 = 1.0F/sq(SA3);
-		SA5_inv = sq(SA2)*SA4 + 1;
-		canUseA = fabsf(SA5_inv) > 1e-6f;
-	}
-
-	bool canUseB = false;
-	const float SB0 = 2*q0;
-	const float SB1 = 2*q1;
-	const float SB2 = -SB0*q3 + SB1*q2;
-	const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
-	float SB3, SB5_inv;
-
-	if (sq(SB2) > 1e-6f) {
-		SB3 = 1.0F/sq(SB2);
-		SB5_inv = SB3*sq(SB4) + 1;
-		canUseB = fabsf(SB5_inv) > 1e-6f;
-	}
-
-	Vector4f H_YAW;
-
-	if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
-		const float SA5 = 1.0F/SA5_inv;
-		const float SA6 = 1.0F/SA3;
-		const float SA7 = SA2*SA4;
-		const float SA8 = 2*SA7;
-		const float SA9 = 2*SA6;
-
-		H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
-		H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
-		H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
-		H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
-	} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
-		const float SB5 = 1.0F/SB5_inv;
-		const float SB6 = 1.0F/SB2;
-		const float SB7 = SB3*SB4;
-		const float SB8 = 2*SB7;
-		const float SB9 = 2*SB6;
-
-		H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
-		H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
-		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
-		H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
-	} else {
-		return false;
-	}
-
-	float innovation;
-
-	if (zero_innovation) {
-		innovation = 0.0f;
-
-	} else {
-		// calculate the the innovation and wrap to the interval between +-pi
-		innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
-	}
-
-	// define the innovation gate size
-	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-
-	// Update the quaternion states and covariance matrix
-	return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
-}
-
-// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
-bool Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
-			   const Vector4f &yaw_jacobian)
-{
 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
 	// calculate the innovation variance
 	_heading_innov_var = variance;
@@ -609,10 +553,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 		float tmp = 0.0f;
 
 		for (uint8_t col = 0; col <= 3; col++) {
-			tmp += P(row, col) * yaw_jacobian(col);
+			tmp += P(row, col) * H_YAW(col);
 		}
 
-		_heading_innov_var += yaw_jacobian(row) * tmp;
+		_heading_innov_var += H_YAW(row) * tmp;
 	}
 
 	float heading_innov_var_inv;
@@ -629,7 +573,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
-		ECL_ERR("mag yaw fusion numerical error - covariance reset");
+		ECL_ERR("yaw fusion numerical error - covariance reset");
 		return false;
 	}
 
@@ -639,7 +583,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 	for (uint8_t row = 0; row <= 15; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
-			Kfusion(row) += P(row, col) * yaw_jacobian(col);
+			Kfusion(row) += P(row, col) * H_YAW(col);
 		}
 
 		Kfusion(row) *= heading_innov_var_inv;
@@ -648,19 +592,19 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 	if (_control_status.flags.wind) {
 		for (uint8_t row = 22; row <= 23; row++) {
 			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * yaw_jacobian(col);
+				Kfusion(row) += P(row, col) * H_YAW(col);
 			}
 
 			Kfusion(row) *= heading_innov_var_inv;
 		}
 	}
 
+	// define the innovation gate size
+	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
+
 	// innovation test ratio
 	_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
 
-	// we are no longer using 3-axis fusion so set the reported test levels to zero
-	_mag_test_ratio.setZero();
-
 	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;
@@ -668,7 +612,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 		// if we are in air we don't want to fuse the measurement
 		// we allow to use it when on the ground because the large innovation could be caused
 		// by interference or a large initial gyro bias
-		if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) {
+		if (!_control_status.flags.in_air
+		&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
+		&& isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)
+		) {
 			// constrain the innovation to the maximum set by the gate
 			// we need to delay this forced fusion to avoid starting it
 			// immediately after touchdown, when the drone is still armed
@@ -696,10 +643,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 	for (unsigned row = 0; row < _k_num_states; row++) {
 
-		KH[0] = Kfusion(row) * yaw_jacobian(0);
-		KH[1] = Kfusion(row) * yaw_jacobian(1);
-		KH[2] = Kfusion(row) * yaw_jacobian(2);
-		KH[3] = Kfusion(row) * yaw_jacobian(3);
+		KH[0] = Kfusion(row) * H_YAW(0);
+		KH[1] = Kfusion(row) * H_YAW(1);
+		KH[2] = Kfusion(row) * H_YAW(2);
+		KH[3] = Kfusion(row) * H_YAW(3);
 
 		for (unsigned column = 0; column < _k_num_states; column++) {
 			float tmp = KH[0] * P(0, column);
@@ -723,67 +670,14 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);
 
+		_time_last_heading_fuse = _time_last_imu;
+
 		return true;
 	}
 
 	return false;
 }
 
-void Ekf::fuseHeading(float measured_hdg, float obs_var)
-{
-	// observation variance
-	float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
-
-	// update transformation matrix from body to world frame using the current state estimate
-	const float predicted_hdg = getEulerYaw(_R_to_earth);
-
-	if (!PX4_ISFINITE(measured_hdg)) {
-		measured_hdg = predicted_hdg;
-	}
-
-	// handle special case where yaw measurement is unavailable
-	bool fuse_zero_innov = false;
-
-	if (_is_yaw_fusion_inhibited) {
-		// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
-		// conditioned covariance matrix developing over time.
-		if (!_control_status.flags.vehicle_at_rest) {
-			// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
-			// unconstrained quaternion variance growth and record the predicted heading
-			// to use as an observation when movement ceases.
-			// TODO a better way of determining when this is necessary
-			const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
-
-			if (sumQuatVar > _params.quat_max_variance) {
-				fuse_zero_innov = true;
-				R_YAW = 0.25f;
-			}
-
-			_last_static_yaw = predicted_hdg;
-
-		} else {
-			// Vehicle is at rest so use the last moving prediction as an observation
-			// to prevent the heading from drifting and to enable yaw gyro bias learning
-			// before takeoff.
-			if (!PX4_ISFINITE(_last_static_yaw)) {
-				_last_static_yaw = predicted_hdg;
-			}
-
-			measured_hdg = _last_static_yaw;
-		}
-
-	} else {
-		_last_static_yaw = predicted_hdg;
-	}
-
-	if (shouldUse321RotationSequence(_R_to_earth)) {
-		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
-
-	} else {
-		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
-	}
-}
-
 void Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file zero_innovation_heading_update.cpp
+ * Control function for ekf heading update when at rest or no other heading source available
+ */
+
+#include "ekf.h"
+
+void Ekf::controlZeroInnovationHeadingUpdate()
+{
+	const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D
+				|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
+
+	if (!_control_status.flags.tilt_align) {
+		// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
+		float innovation = 0.f;
+		float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f;
+		fuseYaw(innovation, obs_var);
+
+		_last_static_yaw = NAN;
+
+	} else if (_control_status.flags.vehicle_at_rest) {
+		// When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro
+		// bias learning when stationary on ground and to prevent uncontrolled yaw variance growth
+		const float euler_yaw = getEulerYaw(_R_to_earth);
+
+		if (PX4_ISFINITE(_last_static_yaw)) {
+			// fuse last static yaw at a limited rate (every 200 milliseconds)
+			if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
+				float innovation = wrap_pi(euler_yaw - _last_static_yaw);
+				float obs_var = 0.01f;
+				fuseYaw(innovation, obs_var);
+			}
+
+		} else {
+			// record static yaw when transitioning to at rest
+			_last_static_yaw = euler_yaw;
+		}
+
+	} else {
+		// vehicle moving and tilt alignment completed
+
+		// fuse zero innovation at a limited rate (every 200 milliseconds)
+		if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
+			float innovation = 0.f;
+			float obs_var = 0.01f;
+			fuseYaw(innovation, obs_var);
+		}
+
+		_last_static_yaw = NAN;
+	}
+}