ekf2-mag: add inclination check

src/modules/ekf2/EKF/common.h

@@ -494,6 +494,7 @@ struct parameters {
 	int32_t synthesize_mag_z{0};
 	int32_t mag_check{0};
 	float mag_check_strength_tolerance_gs{0.2f};
+	float mag_check_inclination_tolerance_deg{20.f};
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)

src/modules/ekf2/EKF/estimator_interface.h

@@ -216,10 +216,10 @@ public:
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
-	bool get_mag_decl_deg(float *val) const
+	bool get_mag_decl_deg(float &val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			*val = math::degrees(_mag_declination_gps);
+			val = math::degrees(_mag_declination_gps);
 			return true;
 
 		} else {

src/modules/ekf2/EKF/mag_control.cpp

@@ -471,6 +471,28 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 		}
 	}
 
+	const Vector3f mag_earth = _R_to_earth * mag_sample;
+	const float mag_inclination = asin(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
+
+	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
+		if (PX4_ISFINITE(_mag_inclination_gps)) {
+			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
+			const float inc_error_rad = wrap_pi(mag_inclination - _mag_inclination_gps);
+
+			if (fabsf(inc_error_rad) > inc_tol_rad) {
+				_control_status.flags.mag_field_disturbed = true;
+				is_check_failing = true;
+			}
+
+		} else if (_params.mag_check & static_cast<int32_t>(MagCheckMask::FORCE_WMM)) {
+			is_check_failing = true;
+
+		} else {
+			// No check possible when the global position is unknown
+			// TODO: add parameter to remember the inclination between boots
+		}
+	}
+
 	if (is_check_failing || (_time_last_mag_check_failing == 0)) {
 		_time_last_mag_check_failing = _time_delayed_us;
 	}

src/modules/ekf2/EKF2.cpp

@@ -191,6 +191,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #endif // CONFIG_EKF2_BARO_COMPENSATION
 	_param_ekf2_mag_check(_params->mag_check),
 	_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
+	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
 	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
 {
@@ -2532,7 +2533,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 	if (!_mag_decl_saved) {
 		float declination_deg;
 
-		if (_ekf.get_mag_decl_deg(&declination_deg)) {
+		if (_ekf.get_mag_decl_deg(declination_deg)) {
 			_param_ekf2_mag_decl.update();
 
 			if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {

src/modules/ekf2/EKF2.hpp

@@ -738,6 +738,7 @@ private:
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
 		(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
+		(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
 		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
 		_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
 

src/modules/ekf2/ekf2_params.c

@@ -1510,7 +1510,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
  *
  * Set bits to 1 to enable checks. Checks enabled by the following bit positions
  * 0 : Magnetic field strength. Set tolerance using EKF2_MAG_CHK_STR
- * 1 : Reserved
+ * 1 : Magnetic field inclination. Set tolerance using EKF2_MAG_CHK_INC
  * 2 : Wait for GNSS to find the theoretical strength and inclination using the WMM
  *
  * @group EKF2
@@ -1518,7 +1518,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
  * @min 0
  * @max 7
  * @bit 0 Strength (EKF2_MAG_CHK_STR)
- * @bit 1 Reserved
+ * @bit 1 Inclination (EKF2_MAG_CHK_INC)
  * @bit 2 Wait for WMM
  */
 PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
@@ -1536,6 +1536,19 @@ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
  */
 PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_STR, 0.2f);
 
+/**
+ * Magnetic field inclination check tolerance
+ *
+ * Maximum allowed deviation from the expected magnetic field inclination to pass the check.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 90.0
+ * @unit deg
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_INC, 20.f);
+
 /**
  * Enable synthetic magnetometer Z component measurement.
  *

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -214,6 +214,15 @@ void EkfWrapper::enableMagStrengthCheck()
 {
 	_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::STRENGTH);
 }
+
+void EkfWrapper::enableMagInclinationCheck()
+{
+	_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::INCLINATION);
+}
+
+void EkfWrapper::enableMagCheckForceWMM()
+{
+	_ekf_params->mag_check |= static_cast<int32_t>(MagCheckMask::FORCE_WMM);
 }
 
 bool EkfWrapper::isWindVelocityEstimated() const

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -102,6 +102,8 @@ public:
 	bool isIntendingMag3DFusion() const;
 	void setMagFuseTypeNone();
 	void enableMagStrengthCheck();
+	void enableMagInclinationCheck();
+	void enableMagCheckForceWMM();
 
 	bool isWindVelocityEstimated() const;
 

src/modules/ekf2/test/test_EKF_mag.cpp

@@ -95,7 +95,7 @@ TEST_F(EkfMagTest, fusionStartWithReset)
 	Vector3f mag_earth = _ekf->getMagEarthField();
 	float mag_decl = atan2f(mag_earth(1), mag_earth(0));
 	float mag_decl_wmm_deg = 0.f;
-	_ekf->get_mag_decl_deg(&mag_decl_wmm_deg);
+	_ekf->get_mag_decl_deg(mag_decl_wmm_deg);
 	EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f);
 }
 
@@ -141,3 +141,70 @@ TEST_F(EkfMagTest, suddenLargeStrength)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
 }
+
+TEST_F(EkfMagTest, noInitLargeInclination)
+{
+	// GIVEN: a really large magnetic field
+	_ekf_wrapper.enableMagInclinationCheck();
+	// To prevent an early pass of the inclination check, "force WMM" must be set
+	_ekf_wrapper.enableMagCheckForceWMM();
+	_sensor_simulator.startGps();
+	Vector3f mag_data(0.4f, 0.f, 0.f);
+	_sensor_simulator._mag.setData(mag_data);
+
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+	_sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass
+
+	// THEN: the fusion shouldn't start
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+
+	// BUT then: as soon as there is some meaningful data
+	const float mag_heading = -M_PI_F / 7.f;
+	mag_data = Vector3f(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f);
+	_sensor_simulator._mag.setData(mag_data);
+
+	_sensor_simulator.runSeconds(2.f);
+
+	float decl_deg = 0.f;
+	_ekf->get_mag_decl_deg(decl_deg);
+
+	// THEN: the fusion initializes using the mag data and runs normally
+	EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f));
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+}
+
+TEST_F(EkfMagTest, suddenInclinationChange)
+{
+	_ekf_wrapper.enableMagInclinationCheck();
+	_ekf_wrapper.enableMagCheckForceWMM();
+	_sensor_simulator.startGps();
+
+	// GIVEN: some meaningful mag data
+	const float mag_heading = -M_PI_F / 7.f;
+	Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f);
+	_sensor_simulator._mag.setData(mag_data);
+
+	_sensor_simulator.runSeconds(_init_duration_s + 10.f);
+
+	float decl_deg = 0.f;
+	_ekf->get_mag_decl_deg(decl_deg);
+
+	// THEN: the fusion initializes using the mag data and runs normally
+	EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f));
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
+	// BUT WHEN: the mag field inclination suddenly changes
+	mag_data(2) = -mag_data(2);
+	_sensor_simulator._mag.setData(mag_data);
+	_sensor_simulator.runSeconds(6.f);
+
+	// THEN: the mag fusion should stop after some time
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+}