From 79228352e93bc33f11335231024938079c6188ae Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 7 Jun 2016 19:54:28 +1000 Subject: [PATCH] EKF: Reset mag and wind state variances to 'reasonable' values If an in-flight reset is required, we do not want these variances going to zero as that will significantly delay recovery --- EKF/covariance.cpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index dd2a8dfa0b..f592dc941b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -91,21 +91,16 @@ void Ekf::initialiseCovariance() P[15][15] = P[13][13]; // variances for optional states - // these state variances are set to zero until the states are required, then they must be initialised - // earth magnetic field - P[16][16] = 0.0f; - P[17][17] = 0.0f; - P[18][18] = 0.0f; - - // body magnetic field - P[19][19] = 0.0f; - P[20][20] = 0.0f; - P[21][21] = 0.0f; + // earth frame and body frame magnetic field + // set to observation variance + for (uint8_t index=16; index <= 21; index ++) { + P[index][index] = sq(_params.mag_noise); + } // wind - P[22][22] = 0.0f; - P[23][23] = 0.0f; + P[22][22] = 1.0f; + P[23][23] = 1.0f; }