From b64c64d5a3d71a085b603e6f86dfe785a03c6129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 12:00:54 +0200 Subject: [PATCH] Observation index cannot get negative. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index df319a93a4..8c30521e30 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1088,7 +1088,7 @@ void AttPosEKF::FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) + if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; }