AP_NavEKF3: fixed constrain indexing bug

fixes #20180 thanks to @liyue75 for spotting this!
3 years ago · a4da65ea05
1 changed files with 1 additions and 1 deletions
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@ -1945,7 +1945,7 @@ void NavEKF3_core::ConstrainVariances()
        zeroCols(P,13,15);
        zeroRows(P,13,15);
        for (uint8_t i=0; i<=2; i++) {
-            const uint8_t stateIndex = 1 + 13;
+            const uint8_t stateIndex = i + 13;
            P[stateIndex][stateIndex] = fmaxF(P[stateIndex][stateIndex], minStateVarTarget);
        }
    }