From 534e63e2faa916c4f8d66c4b16cc5362ba85bda5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 17 Sep 2020 07:54:34 +1000 Subject: [PATCH] AP_NavEKF2: fixed init of earth state mag variances we were starting with zero variance for X and Y earth states, it should start with sq(magNoise) --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 96045b9a43..59137a5ac7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -117,7 +117,7 @@ void NavEKF2_core::setWindMagStateLearningMode() P[21][21] = bodyMagFieldVar.z; } else { // set the variances equal to the observation variances - for (uint8_t index=18; index<=21; index++) { + for (uint8_t index=16; index<=21; index++) { P[index][index] = sq(frontend->_magNoise); }