ekf2: simplify fuseYaw() signature and use consistently

- make it safe to call for other aid sources, no EKF state is changed unless fusion_enabled

src/modules/ekf2/EKF/ekf.h

@@ -751,8 +751,8 @@ 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
-	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);
+	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
+	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;
 
 #if defined(CONFIG_EKF2_GNSS_YAW)

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -92,7 +92,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 				}
 
 			} else if (quality_sufficient) {
-				fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src);
+				fuseYaw(aid_src);
 
 			} else {
 				aid_src.innovation_rejected = true;

src/modules/ekf2/EKF/mag_control.cpp

@@ -524,9 +524,12 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
 		float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
 
+		_aid_src_mag_heading.observation = measured_hdg;
+		_aid_src_mag_heading.observation_variance = obs_var;
+		_aid_src_mag_heading.innovation = innovation;
 		_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
 
-		fuseYaw(innovation, obs_var, _aid_src_mag_heading);
+		fuseYaw(_aid_src_mag_heading);
 	}
 }
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -230,98 +230,100 @@ 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)
+bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status)
 {
 	Vector24f H_YAW;
-	computeYawInnovVarAndH(variance, aid_src_status.innovation_variance, H_YAW);
+	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
 
-	return fuseYaw(innovation, variance, aid_src_status, H_YAW);
+	return fuseYaw(aid_src_status, H_YAW);
 }
 
-bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW)
+bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW)
 {
-	aid_src_status.innovation = innovation;
-
-	float heading_innov_var_inv = 0.f;
-
-	// check if the innovation variance calculation is badly conditioned
-	if (aid_src_status.innovation_variance >= variance) {
-		// the innovation variance contribution from the state covariances is not negative, no fault
-		_fault_status.flags.bad_hdg = false;
-		heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
-
-	} else {
-		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-		_fault_status.flags.bad_hdg = true;
-
-		// we reinitialise the covariance matrix and abort this fusion step
-		initialiseCovariance();
-		ECL_ERR("yaw fusion numerical error - covariance reset");
-		return false;
-	}
-
-	// calculate the Kalman gains
-	// only calculate gains for states we are using
-	Vector24f Kfusion;
-
-	for (uint8_t row = 0; row < _k_num_states; row++) {
-		for (uint8_t col = 0; col <= 3; 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
 	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
 
-	// set the magnetometer unhealthy if the test fails
-	if (aid_src_status.innovation_rejected) {
-		_innov_check_fail_status.flags.reject_yaw = true;
+	if (aid_src_status.fusion_enabled) {
 
-		// 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)
-		&& isTimedOut(aid_src_status.time_last_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
-			float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
-			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
-
-			// also reset the yaw gyro variance to converge faster and avoid
-			// being stuck on a previous bad estimate
-			resetZDeltaAngBiasCov();
+		// check if the innovation variance calculation is badly conditioned
+		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
+			// the innovation variance contribution from the state covariances is not negative, no fault
+			_fault_status.flags.bad_hdg = false;
 
 		} else {
+			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+			_fault_status.flags.bad_hdg = true;
+
+			// we reinitialise the covariance matrix and abort this fusion step
+			initialiseCovariance();
+			ECL_ERR("yaw fusion numerical error - covariance reset");
+
 			return false;
 		}
 
-	} else {
-		_innov_check_fail_status.flags.reject_yaw = false;
-	}
+		// calculate the Kalman gains
+		// only calculate gains for states we are using
+		Vector24f Kfusion;
+		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
 
-	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+		for (uint8_t row = 0; row < _k_num_states; row++) {
+			for (uint8_t col = 0; col <= 3; col++) {
+				Kfusion(row) += P(row, col) * H_YAW(col);
+			}
 
-		_time_last_heading_fuse = _time_delayed_us;
+			Kfusion(row) *= heading_innov_var_inv;
+		}
 
-		aid_src_status.time_last_fuse = _time_delayed_us;
-		aid_src_status.fused = true;
+		// set the magnetometer unhealthy if the test fails
+		if (aid_src_status.innovation_rejected) {
+			_innov_check_fail_status.flags.reject_yaw = true;
 
-		_fault_status.flags.bad_hdg = false;
+			// 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)
+			&& isTimedOut(aid_src_status.time_last_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
+				float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
+				aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
 
-		return true;
+				// also reset the yaw gyro variance to converge faster and avoid
+				// being stuck on a previous bad estimate
+				resetZDeltaAngBiasCov();
+
+			} else {
+				return false;
+			}
+
+		} else {
+			_innov_check_fail_status.flags.reject_yaw = false;
+		}
+
+		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+
+			_time_last_heading_fuse = _time_delayed_us;
+
+			aid_src_status.time_last_fuse = _time_delayed_us;
+			aid_src_status.fused = true;
+
+			_fault_status.flags.bad_hdg = false;
+
+			return true;
+
+		} else {
+			_fault_status.flags.bad_hdg = true;
+		}
 	}
 
 	// otherwise
 	aid_src_status.fused = false;
-	_fault_status.flags.bad_hdg = true;
 	return false;
 }
 

src/modules/ekf2/EKF/zero_innovation_heading_update.cpp

@@ -50,16 +50,21 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 
 		// Use an observation variance larger than usual but small enough
 		// to constrain the yaw variance just below the threshold
-		float obs_var = 0.25f;
-		estimator_aid_source1d_s aid_src_status;
+		const float obs_var = 0.25f;
+
+		estimator_aid_source1d_s aid_src_status{};
+		aid_src_status.observation = getEulerYaw(_state.quat_nominal);
+		aid_src_status.observation_variance = obs_var;
+		aid_src_status.innovation = 0.f;
+
 		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);
+			aid_src_status.fusion_enabled = true;
+			fuseYaw(aid_src_status, H_YAW);
 		}
 	}
 }