From 8a4b0f858a4995a3e9bff0dc77400e3c98630755 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 17 Sep 2020 12:32:22 +1000
Subject: [PATCH] AP_NavEKF3: reset body mag variances at key points
we need to reset the body mag variances if we change sensors or if we
are starting 3D fusion. When not doing 3D fusion we zero the
variances, so they must be initialised again when we restart
fusion. This fixes a bug in handling the variances on a 2nd flight
---
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 ++
.../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 7 +++++++
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 19 ++++++++++++++++---
libraries/AP_NavEKF3/AP_NavEKF3_core.h | 2 ++
4 files changed, 27 insertions(+), 3 deletions(-)
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 51c894d107..245d6200f4 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -108,6 +108,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
if (!inhibitMagStates && setMagInhibit) {
inhibitMagStates = true;
updateStateIndexLim();
+ // variances will be reset in CovariancePrediction
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
updateStateIndexLim();
@@ -164,6 +165,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
if (onGround) {
finalInflightYawInit = false;
finalInflightMagInit = false;
+ magFieldLearned = false;
}
updateStateIndexLim();
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index d4af4ae59a..7d6596bcb6 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -278,6 +278,8 @@ void NavEKF3_core::tryChangeCompass(void)
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
+ // reset body mag variances on next CovariancePrediction
+ needMagBodyVarReset = true;
return;
}
}
@@ -339,6 +341,11 @@ void NavEKF3_core::readMagData()
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
+
+ // reset body mag variances on next
+ // CovariancePrediction. This copes with possible errors
+ // in the new offsets
+ needMagBodyVarReset = true;
}
lastMagOffsets = nowMagOffsets;
lastMagOffsetsValid = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 75003f5364..b8024bc4ba 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -1022,12 +1022,28 @@ void NavEKF3_core::CovariancePrediction()
}
}
+ if (!inhibitMagStates && lastInhibitMagStates) {
+ // when starting 3D fusion we want to reset body mag variances
+ needMagBodyVarReset = true;
+ }
+
+ if (needMagBodyVarReset) {
+ // reset body mag variances
+ needMagBodyVarReset = false;
+ zeroCols(P,19,21);
+ zeroRows(P,19,21);
+ P[19][19] = sq(frontend->_magNoise);
+ P[20][20] = P[19][19];
+ P[21][21] = P[19][19];
+ }
+
if (!inhibitMagStates) {
float magEarthVar = sq(dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f));
float magBodyVar = sq(dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f));
for (uint8_t i=6; i<=8; i++) processNoiseVariance[i] = magEarthVar;
for (uint8_t i=9; i<=11; i++) processNoiseVariance[i] = magBodyVar;
}
+ lastInhibitMagStates = inhibitMagStates;
if (!inhibitWindStates) {
float windVelVar = sq(dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)));
@@ -1727,9 +1743,6 @@ void NavEKF3_core::resetMagFieldStates()
P[20][20] = P[18][18];
P[21][21] = P[18][18];
- // prevent reset of variances in ConstrainVariances()
- inhibitMagStates = false;
-
// record the fact we have initialised the magnetic field states
recordMagReset();
}
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index cf16036506..98f56bdb3d 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1088,6 +1088,8 @@ private:
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states are inactive
+ bool lastInhibitMagStates; // previous inhibitMagStates
+ bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction
bool inhibitDelVelBiasStates; // true when IMU delta velocity bias states are inactive
bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive
bool gpsNotAvailable; // bool true when valid GPS data is not available