Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Protect against collapse of velocity and position variances

This commit is contained in:
Paul Riseborough 2021-06-17 10:21:22 +10:00 committed by Andrew Tridgell
parent e5c34172c1
commit 2565f3917c
3 changed files with 36 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -274,6 +274,7 @@ void NavEKF3_core::ResetHeight(void)
{
P[6][6] = sq(frontend->_gpsVertVelNoise);
}
vertVelVarClipCount = 0;
}
// Zero the EKF height datum

29
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -232,6 +232,7 @@ void NavEKF3_core::InitialiseVariables()
hgtTimeout = true;
tasTimeout = true;
badIMUdata = false;
vertVelVarClipCount = 0;
finalInflightYawInit = false;
dtIMUavg = ins.get_loop_delta_t();
dtEkfAvg = EKF_TARGET_DT;
@ -1800,9 +1801,31 @@ void NavEKF3_core::ForceSymmetry()
void NavEKF3_core::ConstrainVariances()
{
for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
for (uint8_t i=7; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
P[9][9] = constrain_float(P[9][9],0.0f,1.0e6f); // vertical position
for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_float(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3f); // NE velocity
// check for collapse of the vertical velocity variance
if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
P[6][6] = VEL_STATE_MIN_VARIANCE;
vertVelVarClipCount++;
if (vertVelVarClipCount > VERT_VEL_VAR_CLIP_COUNT_LIM) {
// reset the corresponding covariances
zeroRows(P,6,6);
zeroCols(P,6,6);
// set the variances to the measurement variance
#if EK3_FEATURE_EXTERNAL_NAV
if (useExtNavVel) {
P[6][6] = sq(extNavVelDelayed.err);
} else
#endif
{
P[6][6] = sq(frontend->_gpsVertVelNoise);
}
vertVelVarClipCount = 0;
}
}
for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i], POS_STATE_MIN_VARIANCE, 1.0e6f); // NED position
if (!inhibitDelAngBiasStates) {
for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg));

9
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -80,6 +80,14 @@
// number of continuous valid GSF yaw estimates required to confirm valid hostory
#define GSF_YAW_VALID_HISTORY_THRESHOLD 5
// minimum variances allowed for velocity and position states
#define VEL_STATE_MIN_VARIANCE 1E-4f
#define POS_STATE_MIN_VARIANCE 1E-4f
// maximum number of times the vertical velocity variance can hit the lower limit before the
// associated states, variances and covariances are reset
#define VERT_VEL_VAR_CLIP_COUNT_LIM 5
class NavEKF3_core : public NavEKF_core_common
{
public:
@ -937,6 +945,7 @@ private:
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badIMUdata; // boolean true if the bad IMU data is detected
uint8_t vertVelVarClipCount; // number of times the vertical velocity variance has exveeded the lower limit and been clipped since the last reset
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix