From 9e211d6ca1e85b324b73319ae5ffa215aed305a4 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 27 Jun 2020 17:51:50 +0200 Subject: [PATCH] Stop overwriting fault status with false if one component is true --- EKF/vel_pos_fusion.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 9684e0e5c3..d1208bb7d8 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -172,6 +172,7 @@ 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)) { @@ -181,9 +182,6 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o healthy = false; setVelPosFaultStatus(obs_index, true); - - } else { - setVelPosFaultStatus(obs_index, false); } }