Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: reset body mag variances at key points

Browse Source 
reset on sensor switch or when changing to 3D fusion
This commit is contained in:
Andrew Tridgell 2020-09-17 10:39:04 +10:00
parent 8920362212
commit 35e95d3328
4 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -105,6 +105,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
bool setMagInhibit = !magCalRequested || magCalDenied;
if (!inhibitMagStates && setMagInhibit) {
inhibitMagStates = true;
// variances will be reset in CovariancePrediction
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
if (magFieldLearned) {

7
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -222,6 +222,9 @@ void NavEKF2_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;
}
}
@ -285,6 +288,10 @@ void NavEKF2_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;

17
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1082,6 +1082,23 @@ void NavEKF2_core::CovariancePrediction()
zeroCols(P,22,23);
}
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];
}
lastInhibitMagStates = inhibitMagStates;
nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[9][0]*SPP[22] + P[12][0]*SPP[18] + P[2][0]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[9][1]*SPP[22] + P[12][1]*SPP[18] + P[2][1]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[8]*(P[0][2]*SPP[5] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18] - P[1][2]*(2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12])) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[9][9]*SPP[22] + P[12][9]*SPP[18] + P[2][9]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[9][12]*SPP[22] + P[12][12]*SPP[18] + P[2][12]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]));
nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18]) - (2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9])*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[9]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -946,6 +946,8 @@ private:
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool lastInhibitMagStates; // previous inhibitMagStates
bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction
bool gpsNotAvailable; // bool true when valid GPS data is not available
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
struct Location EKF_origin; // LLH origin of the NED axis system