From 00973d62156d7627e9e619c4e49c9a4e432fb2d8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 8 May 2016 17:51:47 +1000 Subject: [PATCH] EKF: fix pos/vel fusion innovation variance bug --- 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 c941db89e5..d9a0a960b1 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -153,7 +153,7 @@ void Ekf::fuseVelPosHeight() for (unsigned obs_index = 0; obs_index < 6; obs_index++) { if (fuse_map[obs_index]) { // compute the innovation variance SK = HPH + R - unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state + unsigned state_index = obs_index + 4; // 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]) *