diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index f329dc31e7..13caff5ca2 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/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; - _control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff; + 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_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) ) { - - fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f, update_all_states); + // using previously saved declination + 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; diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index c4922322c0..0bada1b2b6 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/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.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; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fadbd2256c..7be3fdf92a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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 - bool fuseDeclination(float decl_measurement_rad, float decl_sigma, bool update_all_states = false); + // R: declination observation variance (rad**2) + bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false); #endif // CONFIG_EKF2_MAGNETOMETER diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 0d34e676cb..e954433442 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/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);