EKF2: improve resilience against incorrect mag data

- when GNSS is used require low mag heading innovations during
  horizontal acceleration (yaw observable) to validate the mag
- only fuse mag heading just enough to constrain the yaw estimate
  variance to a sane value. Leave enough uncertainty to allow for a
  correction when the yaw is observable through GNSS fusion

msg/EstimatorStatusFlags.msg

@@ -41,6 +41,7 @@ bool cs_fake_hgt                # 33 - true when fake height measurements are be
 bool cs_gravity_vector          # 34 - true when gravity vector measurements are being fused
 bool cs_mag                     # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 bool cs_ev_yaw_fault            # 36 - true when the EV heading has been declared faulty and is no longer being used
+bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag data is declared consistent with the filter
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/EKF/common.h

@@ -600,6 +600,7 @@ union filter_control_status_u {
 		uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
 		uint64_t mag                     : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
 		uint64_t ev_yaw_fault            : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
+		uint64_t mag_heading_consistent  : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
 
 	} flags;
 	uint64_t value;

src/modules/ekf2/EKF/ekf.h

@@ -581,6 +581,7 @@ class Ekf final : public EstimatorInterface
 	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
+	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
 	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
 	uint8_t _nb_mag_heading_reset_available{0};
@@ -1029,6 +1030,7 @@ class Ekf final : public EstimatorInterface
 
 	void checkYawAngleObservability();
 	void checkMagBiasObservability();
+	void checkMagHeadingConsistency();
 
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -63,6 +63,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
 				       && _control_status.flags.mag
 				       && _control_status.flags.mag_aligned_in_flight
+				       && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
 				       && !_control_status.flags.mag_fault
 				       && isRecent(aid_src.time_last_fuse, 500'000)
 				       && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f))

src/modules/ekf2/EKF/mag_control.cpp

@@ -50,6 +50,7 @@ void Ekf::controlMagFusion()
 	// check mag state observability
 	checkYawAngleObservability();
 	checkMagBiasObservability();
+	checkMagHeadingConsistency();
 
 	if (_mag_bias_observable || _yaw_angle_observable) {
 		_time_last_mov_3d_mag_suitable = _time_delayed_us;
@@ -257,6 +258,19 @@ void Ekf::checkMagBiasObservability()
 	_time_yaw_started = _time_delayed_us;
 }
 
+void Ekf::checkMagHeadingConsistency()
+{
+	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
+		if (_yaw_angle_observable) {
+			// yaw angle must be observable to consider consistency
+			_control_status.flags.mag_heading_consistent = true;
+		}
+
+	} else {
+		_control_status.flags.mag_heading_consistent = false;
+	}
+}
+
 bool Ekf::checkMagField(const Vector3f &mag_sample)
 {
 	_control_status.flags.mag_field_disturbed = false;
@@ -357,6 +371,9 @@ void Ekf::resetMagHeading(const Vector3f &mag)
 	_mag_heading_last_declination = declination;
 
 	_time_last_heading_fuse = _time_delayed_us;
+
+	_mag_heading_innov_lpf.reset(0.f);
+	_control_status.flags.mag_heading_consistent = true;
 }
 
 float Ekf::getMagDeclination()

src/modules/ekf2/EKF/mag_heading_control.cpp

@@ -68,18 +68,23 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
 	if (_control_status.flags.yaw_align) {
 		// mag heading
 		aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
+		_mag_heading_innov_lpf.update(aid_src.innovation);
 
 	} else {
 		// mag heading delta (logging only)
 		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
 					     - wrap_pi(measured_hdg - _mag_heading_prev));
+		_mag_heading_innov_lpf.reset(0.f);
 	}
 
 	// determine if we should use mag heading aiding
+	const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
+
 	bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING)
 					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
 					     && _control_status.flags.tilt_align
-					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
+					         || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
 					     && !_control_status.flags.mag_fault
 					     && !_control_status.flags.mag_field_disturbed
 					     && !_control_status.flags.ev_yaw
@@ -99,6 +104,7 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
 
 			const bool declination_changed = _control_status_prev.flags.mag_hdg
 							 && (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
+			bool skip_timeout_check = false;
 
 			if (mag_sample.reset || declination_changed) {
 				if (declination_changed) {
@@ -113,10 +119,21 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
 				aid_src.time_last_fuse = _time_delayed_us;
 
 			} else {
-				fuseYaw(aid_src);
+				Vector24f H_YAW;
+				computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW);
+
+				if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) {
+					// Only fuse mag to constrain the yaw variance to a safe value
+					fuseYaw(aid_src, H_YAW);
+
+				} else {
+					// Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check
+					skip_timeout_check = true;
+					aid_src.test_ratio = 0.f; // required for preflight checks to pass
+				}
 			}
 
-			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check;
 
 			if (is_fusion_failing) {
 				if (_nb_mag_heading_reset_available > 0) {

src/modules/ekf2/EKF2.cpp

@@ -1771,6 +1771,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_gravity_vector          = _ekf.control_status_flags().gravity_vector;
 		status_flags.cs_mag                     = _ekf.control_status_flags().mag;
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
+		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;