ekf2: mag declination fusion always if there is no aiding

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -205,14 +205,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
 		// then the declination must be fused as an observation to prevent long term heading drift
-		const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.in_air;
+		const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
-		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff;
+		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
 
 		if (_control_status.flags.mag) {
 
 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
+				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 					aid_src.time_last_fuse = imu_sample.time_us;
@@ -234,19 +234,26 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 					if (_control_status.flags.mag_dec) {
 
+						// observation variance (rad**2)
+						const float R_DECL = sq(0.5f);
+
 						if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
 						    && PX4_ISFINITE(_wmm_declination_rad)
 						   ) {
+							// using declination from the world magnetic model
 							fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states);
 
 						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
 							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
 							  ) {
-
+							// using previously saved declination
-							fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f, update_all_states);
+							fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states);
 
 						} else {
-							_control_status.flags.mag_dec = false;
+							// if there is no aiding coming from an inertial frame we need to fuse some declination
+							// even if we don't know the value, it's better to fuse 0 than nothing
+							float declination_rad = 0.f;
+							fuseDeclination(declination_rad, R_DECL);
 						}
 					}
 				}
@@ -254,7 +261,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
-					if (no_ne_aiding_or_pre_takeoff) {
+					if (no_ne_aiding_or_not_moving) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 						aid_src.time_last_fuse = imu_sample.time_us;

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -151,21 +151,18 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }
 
-bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states)
+bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
 {
-	// observation variance (rad**2)
-	const float R_DECL = sq(decl_sigma);
-
 	VectorState H;
 	float decl_pred;
 	float innovation_variance;
 
-	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON,
+	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
 			&decl_pred, &innovation_variance, &H);
 
 	const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
 
-	if (innovation_variance < R_DECL) {
+	if (innovation_variance < R) {
 		// variance calculation is badly conditioned
 		_fault_status.flags.bad_mag_decl = true;
 		return false;
@@ -187,7 +184,7 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma, bool upd
 		Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 	}
 
-	const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
+	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
 
 	_fault_status.flags.bad_mag_decl = !is_fused;
 

src/modules/ekf2/EKF/ekf.h

@@ -772,8 +772,8 @@ private:
 		     bool update_all_states = false, bool update_tilt = false);
 
 	// fuse magnetometer declination measurement
-	//  declination uncertainty in radians
+	//  R: declination observation variance (rad**2)
-	bool fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states = false);
+	bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -108,6 +108,7 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag);
+	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);