From aa2c77f92d098be159dae2e8a6305317d979d31a Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 22 Aug 2020 10:24:23 +0200 Subject: [PATCH] Small cleanup in velposfusion --- EKF/vel_pos_fusion.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index d1208bb7d8..4e14773f55 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -172,7 +172,6 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool healthy = true; - setVelPosFaultStatus(obs_index, false); for (int i = 0; i < _k_num_states; i++) { if (P(i, i) < KHP(i, i)) { @@ -180,11 +179,11 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); healthy = false; - - setVelPosFaultStatus(obs_index, true); } } + setVelPosFaultStatus(obs_index, !healthy); + if (healthy) { // apply the covariance corrections P -= KHP;