diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 352d9a0ce9..c2282fbb9d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2376,6 +2376,12 @@ int AttPosEKF::CheckAndBound() ret = 5; } + if (current_ekf_state.covariancesExcessive) { + FillErrorReport(&last_ekf_error); + current_ekf_state.covariancesExcessive = false; + ret = 6; + } + if (ret) { ekfDiverged = true; }