Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: reset body mag variances at key points

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2020-09-17 12:32:22 +10:00
parent e9185f526b
commit 8a4b0f858a
4 changed files with 27 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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();

7
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;

19
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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();
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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