From a4da65ea05b48e27355caea3b74768ca7e8c50ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Mar 2022 11:24:44 +1100 Subject: [PATCH] AP_NavEKF3: fixed constrain indexing bug fixes #20180 thanks to @liyue75 for spotting this! --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a465019217..e80533cf50 100644 --- 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); } }