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 936d141eb0..f6da313112 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -39,6 +39,8 @@ #include "ekf.h" #include +#include + void Ekf::controlMagFusion() { static constexpr const char *AID_SRC_NAME = "mag"; @@ -68,8 +70,6 @@ void Ekf::controlMagFusion() if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { - resetEstimatorAidStatus(aid_src); - if (mag_sample.reset || (_mag_counter == 0)) { // sensor or calibration has changed, reset low pass filter _control_status.flags.mag_fault = false; @@ -101,6 +101,37 @@ void Ekf::controlMagFusion() _control_status.flags.synthetic_mag_z = false; } + + resetEstimatorAidStatus(aid_src); + aid_src.timestamp_sample = mag_sample.time_us; + + // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations + const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); + + // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains + Vector3f mag_innov; + Vector3f innov_var; + + // Observation jacobian and Kalman gain vectors + VectorState H; + sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); + + // do not use the synthesized measurement for the magnetomter Z component for 3D fusion + if (_control_status.flags.synthetic_mag_z) { + mag_innov(2) = 0.0f; + } + + for (int i = 0; i < 3; i++) { + aid_src.observation[i] = mag_sample.mag(i); + aid_src.observation_variance[i] = R_MAG; + aid_src.innovation[i] = mag_innov(i); + aid_src.innovation_variance[i] = innov_var(i); + } + + const float innov_gate = math::max(_params.mag_innov_gate, 1.f); + setEstimatorAidStatusTestRatio(aid_src, innov_gate); + + // determine if we should use mag fusion bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) && _control_status.flags.tilt_align @@ -136,7 +167,7 @@ void Ekf::controlMagFusion() _control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING) || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) - && _control_status.flags.tilt_align + && _control_status.flags.mag && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && !_control_status.flags.mag_fault @@ -161,7 +192,6 @@ void Ekf::controlMagFusion() && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight); if (_control_status.flags.mag) { - aid_src.timestamp_sample = mag_sample.time_us; if (continuing_conditions_passing && _control_status.flags.yaw_align) { @@ -178,7 +208,7 @@ void Ekf::controlMagFusion() // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, aid_src, false); + fuseMag(mag_sample.mag, H, aid_src, false); } else { // The normal sequence is to fuse the magnetometer data first before fusing @@ -186,7 +216,7 @@ 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, aid_src, update_all_states, update_tilt); + fuseMag(mag_sample.mag, H, aid_src, update_all_states, update_tilt); if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); @@ -253,7 +283,7 @@ void Ekf::controlMagFusion() } else { ECL_INFO("starting %s fusion", AID_SRC_NAME); - fuseMag(mag_sample.mag, aid_src, false); + fuseMag(mag_sample.mag, H, aid_src, false); } aid_src.time_last_fuse = _time_delayed_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 1dbff8874c..0b79edb77f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -43,7 +43,6 @@ #include "ekf.h" -#include #include #include @@ -51,39 +50,17 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt) +bool Ekf::fuseMag(const Vector3f &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)); - // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains - Vector3f mag_innov; - Vector3f innov_var; - - // Observation jacobian and Kalman gain vectors - VectorState H; const auto state_vector = _state.vector(); - sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion - if (_control_status.flags.synthetic_mag_z) { - mag_innov(2) = 0.0f; - } - - for (int i = 0; i < 3; i++) { - aid_src_mag.observation[i] = mag(i) - _state.mag_B(i); - aid_src_mag.observation_variance[i] = R_MAG; - aid_src_mag.innovation[i] = mag_innov(i); - aid_src_mag.innovation_variance[i] = innov_var(i); - } - - const float innov_gate = math::max(_params.mag_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate); if (update_all_states) { - _fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]); - _fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]); - _fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]); + _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; @@ -92,28 +69,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } // Perform an innovation consistency check and report the result - _innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f); - _innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f); - - const char *numerical_error_covariance_reset_string = "numerical error - covariance reset"; - - // check innovation variances for being badly conditioned - if (innov_var.min() < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("mag %s", numerical_error_covariance_reset_string); - return false; - } + _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 (aid_src_mag.innovation_rejected) { + if (aid_src.innovation_rejected) { return false; } @@ -127,25 +88,10 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } 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) - sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); + sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - - if (aid_src_mag.innovation_variance[index] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_y = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("magY %s", numerical_error_covariance_reset_string); - return false; - } + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); } else if (index == 2) { // we do not fuse synthesized magnetomter measurements when doing 3D fusion @@ -155,28 +101,33 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); + sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - - if (aid_src_mag.innovation_variance[index] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_z = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("magZ %s", numerical_error_covariance_reset_string); - return false; - } + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); } - VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; + 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 + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + + return false; + } + + VectorState Kfusion = P * H / aid_src.innovation_variance[index]; if (!update_all_states) { // zero non-mag Kalman gains if not updating all states @@ -192,16 +143,13 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } if (!update_tilt) { - Kfusion(State::quat_nominal.idx) = 0.f; + Kfusion(State::quat_nominal.idx + 0) = 0.f; Kfusion(State::quat_nominal.idx + 1) = 0.f; } - if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) { + if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) { fused[index] = true; limitDeclination(); - - } else { - fused[index] = false; } } @@ -212,8 +160,8 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } if (fused[0] && fused[1] && fused[2]) { - aid_src_mag.fused = true; - aid_src_mag.time_last_fuse = _time_delayed_us; + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; if (update_all_states) { _time_last_heading_fuse = _time_delayed_us; @@ -222,7 +170,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo return true; } - aid_src_mag.fused = false; return false; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 91b4682d82..b326382d83 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, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true); + bool fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = true, bool update_tilt = true); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians