diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 242684bb70..de81fd1ea6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -751,8 +751,8 @@ private: bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector - bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status); - bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); + bool fuseYaw(estimator_aid_source1d_s &aid_src_status); + bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const; #if defined(CONFIG_EKF2_GNSS_YAW) diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index e7f1d3c643..74faf69abf 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -92,7 +92,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src); + fuseYaw(aid_src); } else { aid_src.innovation_rejected = true; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 8e95388a4b..d1b7907ac2 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -524,9 +524,12 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f); + _aid_src_mag_heading.observation = measured_hdg; + _aid_src_mag_heading.observation_variance = obs_var; + _aid_src_mag_heading.innovation = innovation; _aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg; - fuseYaw(innovation, obs_var, _aid_src_mag_heading); + fuseYaw(_aid_src_mag_heading); } } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 098373ef68..2e0f2977c5 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -230,98 +230,100 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } // update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status) +bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status) { Vector24f H_YAW; - computeYawInnovVarAndH(variance, aid_src_status.innovation_variance, H_YAW); + computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW); - return fuseYaw(innovation, variance, aid_src_status, H_YAW); + return fuseYaw(aid_src_status, H_YAW); } -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW) +bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW) { - aid_src_status.innovation = innovation; - - float heading_innov_var_inv = 0.f; - - // check if the innovation variance calculation is badly conditioned - if (aid_src_status.innovation_variance >= variance) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.flags.bad_hdg = false; - heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - - } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - ECL_ERR("yaw fusion numerical error - covariance reset"); - return false; - } - - // calculate the Kalman gains - // only calculate gains for states we are using - Vector24f Kfusion; - - for (uint8_t row = 0; row < _k_num_states; row++) { - for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * H_YAW(col); - } - - Kfusion(row) *= heading_innov_var_inv; - } - // define the innovation gate size float gate_sigma = math::max(_params.heading_innov_gate, 1.f); // innovation test ratio setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - // set the magnetometer unhealthy if the test fails - if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; + if (aid_src_status.fusion_enabled) { - // if we are in air we don't want to fuse the measurement - // we allow to use it when on the ground because the large innovation could be caused - // by interference or a large initial gyro bias - if (!_control_status.flags.in_air - && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) - ) { - // constrain the innovation to the maximum set by the gate - // we need to delay this forced fusion to avoid starting it - // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); - aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); - - // also reset the yaw gyro variance to converge faster and avoid - // being stuck on a previous bad estimate - resetZDeltaAngBiasCov(); + // check if the innovation variance calculation is badly conditioned + if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.flags.bad_hdg = false; } else { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("yaw fusion numerical error - covariance reset"); + return false; } - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } + // calculate the Kalman gains + // only calculate gains for states we are using + Vector24f Kfusion; + const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + for (uint8_t row = 0; row < _k_num_states; row++) { + for (uint8_t col = 0; col <= 3; col++) { + Kfusion(row) += P(row, col) * H_YAW(col); + } - _time_last_heading_fuse = _time_delayed_us; + Kfusion(row) *= heading_innov_var_inv; + } - aid_src_status.time_last_fuse = _time_delayed_us; - aid_src_status.fused = true; + // set the magnetometer unhealthy if the test fails + if (aid_src_status.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; - _fault_status.flags.bad_hdg = false; + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (!_control_status.flags.in_air + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) + ) { + // constrain the innovation to the maximum set by the gate + // we need to delay this forced fusion to avoid starting it + // immediately after touchdown, when the drone is still armed + float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); - return true; + // also reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetZDeltaAngBiasCov(); + + } else { + return false; + } + + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } + + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + + _time_last_heading_fuse = _time_delayed_us; + + aid_src_status.time_last_fuse = _time_delayed_us; + aid_src_status.fused = true; + + _fault_status.flags.bad_hdg = false; + + return true; + + } else { + _fault_status.flags.bad_hdg = true; + } } // otherwise aid_src_status.fused = false; - _fault_status.flags.bad_hdg = true; return false; } diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index b416edeef1..cfc48a14a9 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -50,16 +50,21 @@ void Ekf::controlZeroInnovationHeadingUpdate() // Use an observation variance larger than usual but small enough // to constrain the yaw variance just below the threshold - float obs_var = 0.25f; - estimator_aid_source1d_s aid_src_status; + const float obs_var = 0.25f; + + estimator_aid_source1d_s aid_src_status{}; + aid_src_status.observation = getEulerYaw(_state.quat_nominal); + aid_src_status.observation_variance = obs_var; + aid_src_status.innovation = 0.f; + Vector24f H_YAW; computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { // The yaw variance is too large, fuse fake measurement - float innovation = 0.f; - fuseYaw(innovation, obs_var, aid_src_status, H_YAW); + aid_src_status.fusion_enabled = true; + fuseYaw(aid_src_status, H_YAW); } } }