diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 328054b3d0..dc3e7f8664 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1544,7 +1544,7 @@ void NavEKF::FuseVelPosNED() // because there may be no stored states due to lack of real measurements. // in static mode, only position and height fusion is used if (staticMode) { - for (uint8_t i=7; i<=9; i++) { + for (uint8_t i=0; i<=30; i++) { statesAtPosTime[i] = states[i]; statesAtHgtTime[i] = states[i]; }