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