From 569886a4fc04b5544bb84d5d744b5a5e3cb0499c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 3 Mar 2016 17:15:22 +1100 Subject: [PATCH] EKF: Fix bug in velocity and position innovation consistency checks --- EKF/vel_pos_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 6f2214442d..54d59187af 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -122,7 +122,7 @@ void Ekf::fuseVelPosHeight() unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; // Compute the ratio of innovation to gate size - _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov[obs_index]); + _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]); } }