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);
         }
     }