ekf2: add mag fusion timestamps

src/modules/ekf2/EKF/ekf.h

@@ -420,6 +420,8 @@ private:
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint64_t _time_last_mag_heading_fuse{0};
+	uint64_t _time_last_mag_3d_fuse{0};
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
@@ -605,9 +607,9 @@ private:
 	void predictCovariance();
 
 	// ekf sequential fusion of magnetometer measurements
-	void fuseMag(const Vector3f &mag);
+	bool fuseMag(const Vector3f &mag, bool update_all_states = true);
 
-	// update quaternion states and covariances using a yaw innovation and yaw observation variance
+	// 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);
@@ -621,7 +623,7 @@ private:
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
-	void fuseDeclination(float decl_sigma);
+	bool fuseDeclination(float decl_sigma);
 
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
@@ -1035,7 +1037,7 @@ private:
 	// 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);
+	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true);
 
 	// Declarations used to control use of the EKF-GSF yaw estimator
 

src/modules/ekf2/EKF/mag_control.cpp

@@ -345,12 +345,19 @@ 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);
-		fuseYaw(innovation, obs_var);
+
+		if (fuseYaw(innovation, obs_var)) {
+			_time_last_mag_heading_fuse = _time_last_imu;
+		}
 	}
 }
 
 void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
 		// covariances need to be corrected to incorporate knowledge of the declination
@@ -358,13 +365,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag);
+		fuseMag(mag, update_all_states);
 
 	} else {
 		// The normal sequence is to fuse the magnetometer data first before fusing
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
-		fuseMag(mag);
+		fuseMag(mag, update_all_states);
 
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -45,7 +45,7 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::fuseMag(const Vector3f &mag)
+bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 {
 	// assign intermediate variables
 	const float q0 = _state.quat_nominal(0);
@@ -96,7 +96,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_x = false;
@@ -138,7 +138,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_y = false;
@@ -150,7 +150,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_z = false;
@@ -174,20 +174,24 @@ void Ekf::fuseMag(const Vector3f &mag)
 	for (uint8_t index = 0; index <= 2; index++) {
 		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
 
-		const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f);
+		bool rejected = (_mag_test_ratio(index) > 1.f);
 
-		if (innov_check_fail) {
+		switch (index) {
-			all_innovation_checks_passed = false;
+		case 0:
-		}
+			_innov_check_fail_status.flags.reject_mag_x = rejected;
+			break;
 
-		if (index == 0) {
+		case 1:
-			_innov_check_fail_status.flags.reject_mag_x = innov_check_fail;
+			_innov_check_fail_status.flags.reject_mag_y = rejected;
+			break;
 
-		} else if (index == 1) {
+		case 2:
-			_innov_check_fail_status.flags.reject_mag_y = innov_check_fail;
+			_innov_check_fail_status.flags.reject_mag_z = rejected;
+			break;
+		}
 
-		} else {
+		if (rejected) {
-			_innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
+			all_innovation_checks_passed = false;
 		}
 	}
 
@@ -196,13 +200,9 @@ void Ekf::fuseMag(const Vector3f &mag)
 
 	// if any axis fails, abort the mag fusion
 	if (!all_innovation_checks_passed) {
-		return;
+		return false;
 	}
 
-	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
-	// before they are used to constrain heading drift
-	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
-
 	// Observation jacobian and Kalman gain vectors
 	SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
 	Vector24f Kfusion;
@@ -284,7 +284,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 				// we need to re-initialise covariances and abort this fusion step
 				resetMagRelatedCovariances();
 				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-				return;
+				return false;
 			}
 			const float HKY24 = 1.0F/_mag_innov_var(1);
 
@@ -364,7 +364,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 				// we need to re-initialise covariances and abort this fusion step
 				resetMagRelatedCovariances();
 				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-				return;
+				return false;
 			}
 
 			const float HKZ24 = 1.0F/_mag_innov_var(2);
@@ -427,6 +427,13 @@ void Ekf::fuseMag(const Vector3f &mag)
 			limitDeclination();
 		}
 	}
+
+	if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) {
+		_time_last_mag_3d_fuse = _time_last_imu;
+		return true;
+	}
+
+	return false;
 }
 
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
@@ -678,7 +685,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 	return false;
 }
 
-void Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
 	const float magN = _state.mag_I(0);
@@ -693,7 +700,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	// Calculate intermediate variables
 	if (fabsf(magN) < sq(N_field_min)) {
 		// calculation is badly conditioned close to +-90 deg declination
-		return;
+		return false;
 	}
 
 	const float HK0 = ecl::powf(magN, -2);
@@ -712,7 +719,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 		HK9 = HK4/innovation_variance;
 	} else {
 		// variance calculation is badly conditioned
-		return;
+		return false;
 	}
 
 	// Calculate the observation Jacobian
@@ -745,6 +752,8 @@ void Ekf::fuseDeclination(float decl_sigma)
 	if (is_fused) {
 		limitDeclination();
 	}
+
+	return is_fused;
 }
 
 void Ekf::limitDeclination()