From bc5efb6cd24ebf8ef9fe877d06ea6e17b4236d16 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 20 Sep 2021 21:20:20 +1000 Subject: [PATCH] AP_NavEKF3: fix wind speed covariance initialisation bug --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 9c416e9b94..2deb56f38e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -88,11 +88,9 @@ void NavEKF3_core::setWindMagStateLearningMode() } // set the wind state variances to the measurement uncertainty - for (uint8_t index=22; index<=23; index++) { - zeroCols(P, 22, 23); - zeroRows(P, 22, 23); - P[index][index] = trueAirspeedVariance; - } + zeroCols(P, 22, 23); + zeroRows(P, 22, 23); + P[22][22] = P[23][23] = trueAirspeedVariance; windStatesAligned = true;