EKF2: only fuse zero innovation if yaw var is large

src/modules/ekf2/EKF/common.h

@@ -360,7 +360,6 @@ struct parameters {
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
 	float mag_yaw_rate_gate{0.25f};         ///< yaw rate threshold used by mode select logic (rad/sec)
-	const float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this
 
 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)

src/modules/ekf2/EKF/ekf.h

@@ -666,9 +666,9 @@ private:
 	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
-	// innovation : prediction - measurement
-	// variance : observaton variance
-	bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status);
+	bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status);
+	bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
+	void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -226,16 +226,15 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
 bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status)
 {
-	aid_src_status.innovation = innovation;
-
 	Vector24f H_YAW;
+	computeYawInnovVarAndH(variance, aid_src_status.innovation_variance, H_YAW);
 
-	if (shouldUse321RotationSequence(_R_to_earth)) {
-		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &aid_src_status.innovation_variance, &H_YAW);
+	return fuseYaw(innovation, variance, aid_src_status, H_YAW);
+}
 
-	} else {
-		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &aid_src_status.innovation_variance, &H_YAW);
-	}
+bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW)
+{
+	aid_src_status.innovation = innovation;
 
 	float heading_innov_var_inv = 0.f;
 
@@ -330,6 +329,16 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 	return false;
 }
 
+void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
+{
+	if (shouldUse321RotationSequence(_R_to_earth)) {
+		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+
+	} else {
+		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+	}
+}
+
 bool Ekf::fuseDeclination(float decl_sigma)
 {
 	// observation variance (rad**2)

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -74,12 +74,19 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 	} else {
 		// vehicle moving and tilt alignment completed
 
-		// fuse zero innovation at a limited rate (every 200 milliseconds)
+		// fuse zero innovation at a limited rate if the yaw variance is too large
 		if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
-			float innovation = 0.f;
-			float obs_var = 0.01f;
-			estimator_aid_source1d_s unused;
-			fuseYaw(innovation, obs_var, unused);
+			float obs_var = 0.25f;
+			estimator_aid_source1d_s aid_src_status;
+			Vector24f H_YAW;
+
+			computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
+
+			if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
+				// The yaw variance is too large, fuse fake measurement
+				float innovation = 0.f;
+				fuseYaw(innovation, obs_var, aid_src_status, H_YAW);
+			}
 		}
 
 		_last_static_yaw = NAN;