From d656c94bbc3c1d96eea18eac35ad58c2541e0df3 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 7 Jan 2015 14:48:49 +1100 Subject: [PATCH] AP_NavEKF: Fix out of bounds index bug --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index ff2d459515..4978740735 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2835,7 +2835,7 @@ void NavEKF::FuseOptFlow() tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); // calculate observation jacobians for X LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[i] = 0; + for (uint8_t i = 0; i < 22; i++) H_LOS[i] = 0; H_LOS[0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; H_LOS[1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); H_LOS[2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); @@ -2899,7 +2899,7 @@ void NavEKF::FuseOptFlow() tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; // Calculate observation jacobians for Y LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[i] = 0; + for (uint8_t i = 0; i < 22; i++) H_LOS[i] = 0; H_LOS[0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; H_LOS[1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; H_LOS[2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3];