From bd8f05567a2b6c369395a980447fd9b6f56365ae Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Sep 2019 09:17:29 +0200 Subject: [PATCH] set vel_pos_test_ratio to zero if no fusion happens --- EKF/vel_pos_fusion.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c4650b629a..4d03c5b7c2 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -168,6 +168,8 @@ void Ekf::fuseVelPosHeight() // Compute the ratio of innovation to gate size _vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]); + }else{ + _vel_pos_test_ratio[obs_index] = 0; } }