AP_NavEKF3: fixed constrain indexing bug

fixes #20180

thanks to @liyue75 for spotting this!

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