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 f6da313112..a919032d8f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -101,6 +101,11 @@ void Ekf::controlMagFusion() _control_status.flags.synthetic_mag_z = false; } + // reset flags + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + resetEstimatorAidStatus(aid_src); aid_src.timestamp_sample = mag_sample.time_us; @@ -131,6 +136,10 @@ void Ekf::controlMagFusion() const float innov_gate = math::max(_params.mag_innov_gate, 1.f); setEstimatorAidStatusTestRatio(aid_src, innov_gate); + // Perform an innovation consistency check and report the result + _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); + _innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f); + _innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f); // determine if we should use mag fusion bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) @@ -208,7 +217,7 @@ void Ekf::controlMagFusion() // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, H, aid_src, false); + fuseMag(mag_sample.mag, R_MAG, H, aid_src); } else { // The normal sequence is to fuse the magnetometer data first before fusing @@ -216,7 +225,14 @@ void Ekf::controlMagFusion() // 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, H, aid_src, update_all_states, update_tilt); + 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]); + } if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); @@ -276,19 +292,19 @@ void Ekf::controlMagFusion() bool reset_heading = !_control_status.flags.yaw_align; resetMagStates(_mag_lpf.getState(), reset_heading); + aid_src.time_last_fuse = _time_delayed_us; + _nb_mag_3d_reset_available = 2; if (reset_heading) { _control_status.flags.yaw_align = true; } } else { - ECL_INFO("starting %s fusion", AID_SRC_NAME); - fuseMag(mag_sample.mag, H, aid_src, false); + if (fuseMag(mag_sample.mag, R_MAG, H, aid_src)) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + _nb_mag_3d_reset_available = 2; + } } - - aid_src.time_last_fuse = _time_delayed_us; - - _nb_mag_3d_reset_available = 2; } } 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 0b79edb77f..ad03b85331 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -50,41 +50,22 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt) +bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt) { - // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); - - const auto state_vector = _state.vector(); - - if (update_all_states) { - _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]); - - } else { - _fault_status.flags.bad_mag_x = false; - _fault_status.flags.bad_mag_y = false; - _fault_status.flags.bad_mag_z = false; - } - - // Perform an innovation consistency check and report the result - _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f); - _innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f); - - // if any axis fails, abort the mag fusion + // if any axis failed, abort the mag fusion if (aid_src.innovation_rejected) { return false; } + const auto state_vector = _state.vector(); + bool fused[3] {false, false, false}; // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { // Calculate Kalman gains and observation jacobians if (index == 0) { - // everything was already computed above + // everything was already computed } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) @@ -108,13 +89,6 @@ bool Ekf::fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s } if (aid_src.innovation_variance[index] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - if (update_all_states) { - _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]); - } - ECL_ERR("mag numerical error covariance reset"); // we need to re-initialise covariances and abort this fusion step diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b326382d83..95add3dec9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -758,7 +758,7 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = true, bool update_tilt = true); + bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians