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 1601178cd2..0077736bb0 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -59,13 +59,6 @@ void Ekf::controlMagFusion() return; } - // stop mag (require a reset before using again) if there was an external yaw reset (yaw estimator, GPS yaw, etc) - if (_mag_decl_cov_reset && (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat)) { - ECL_INFO("yaw reset, stopping mag fusion to force reinitialization"); - stopMagFusion(); - resetMagCov(); - } - magSample mag_sample; if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { @@ -154,11 +147,13 @@ void Ekf::controlMagFusion() // WMM update can occur on the last epoch, just after mag fusion const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); + const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + { - const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; const bool common_conditions_passing = _control_status.flags.mag - && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) + && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed @@ -186,9 +181,8 @@ void Ekf::controlMagFusion() // 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 // fusing declination when gps aiding is available is optional. - const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; _control_status.flags.mag_dec = _control_status.flags.mag - && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight); + && (!using_ne_aiding || !_control_status.flags.mag_aligned_in_flight); if (_control_status.flags.mag) { @@ -200,33 +194,22 @@ void Ekf::controlMagFusion() aid_src.time_last_fuse = _time_delayed_us; } else { - 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 - // before fusing magnetometer data to prevent rapid rotation of the earth field - // states for the first few observations. - fuseDeclination(0.02f); - _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, R_MAG, H, aid_src); + // 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. + const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; + const bool update_tilt = _control_status.flags.mag_3D; + fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt); - } 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. - const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; - const bool update_tilt = _control_status.flags.mag_3D; - fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt); + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + if (update_all_states && update_tilt) { + _fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]); + _fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]); + _fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]); + } - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - if (update_all_states && update_tilt) { - _fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]); - _fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]); - _fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]); - } - - if (_control_status.flags.mag_dec) { - fuseDeclination(0.5f); - } + if (_control_status.flags.mag_dec) { + fuseDeclination(0.5f); } } @@ -272,7 +255,6 @@ void Ekf::controlMagFusion() // activate fusion, reset mag states and initialize variance if first init or in flight reset if (!_control_status.flags.yaw_align || wmm_updated - || !_mag_decl_cov_reset || !_state.mag_I.longerThan(0.f) || (getStateVariance().min() < kMagVarianceMin) || (getStateVariance().min() < kMagVarianceMin) @@ -399,9 +381,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) resetMagHeading(mag); } - // earth field was reset to WMM, skip initial declination fusion - _mag_decl_cov_reset = true; - } else { // mag_B: reset _state.mag_B.zero(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 13f2399be5..a086a11700 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -308,10 +308,7 @@ void Ekf::resetAccelBiasCov() #if defined(CONFIG_EKF2_MAGNETOMETER) void Ekf::resetMagCov() { - if (_mag_decl_cov_reset) { - ECL_INFO("reset mag covariance"); - _mag_decl_cov_reset = false; - } + ECL_INFO("reset mag covariance"); P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 95add3dec9..8ec3c06987 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -705,7 +705,6 @@ private: // used by magnetometer fusion mode selection bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable AlphaFilter _mag_heading_innov_lpf{0.1f}; - 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_3d_reset_available{0}; uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time