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

AP_NavEKF3: Fade each vert vel variance clip count over 1 second

Browse Source
This commit is contained in:
Paul Riseborough 2021-06-18 07:33:58 +10:00 committed by Randy Mackay
parent ac92182153
commit de3c6d6e5c
3 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

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

15
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -232,7 +232,7 @@ void NavEKF3_core::InitialiseVariables()
hgtTimeout = true; hgtTimeout = true;
tasTimeout = true; tasTimeout = true;
badIMUdata = false; badIMUdata = false;
vertVelVarClipCount = 0; vertVelVarClipCounter = 0;
finalInflightYawInit = false; finalInflightYawInit = false;
dtIMUavg = ins.get_loop_delta_t(); dtIMUavg = ins.get_loop_delta_t();
dtEkfAvg = EKF_TARGET_DT; dtEkfAvg = EKF_TARGET_DT;
@ -1717,6 +1717,10 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr)
// constrain values to prevent ill-conditioning // constrain values to prevent ill-conditioning
ConstrainVariances(); ConstrainVariances();
if (vertVelVarClipCounter > 0) {
vertVelVarClipCounter--;
}
calcTiltErrorVariance(); calcTiltErrorVariance();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -1806,8 +1810,11 @@ void NavEKF3_core::ConstrainVariances()
// check for collapse of the vertical velocity variance // check for collapse of the vertical velocity variance
if (P[6][6] < VEL_STATE_MIN_VARIANCE) { if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
P[6][6] = VEL_STATE_MIN_VARIANCE; P[6][6] = VEL_STATE_MIN_VARIANCE;
vertVelVarClipCount++; // this counter is decremented by 1 each prediction cycle in CovariancePrediction
if (vertVelVarClipCount > VERT_VEL_VAR_CLIP_COUNT_LIM) { // resulting in the count from each clip event fading to zero over 1 second which
// is sufficient to capture collapse from fusion of the lowest update rate sensor
vertVelVarClipCounter += EKF_TARGET_RATE_HZ;
if (vertVelVarClipCounter > VERT_VEL_VAR_CLIP_COUNT_LIM) {
// reset the corresponding covariances // reset the corresponding covariances
zeroRows(P,6,6); zeroRows(P,6,6);
zeroCols(P,6,6); zeroCols(P,6,6);
@ -1821,7 +1828,7 @@ void NavEKF3_core::ConstrainVariances()
{ {
P[6][6] = sq(frontend->_gpsVertVelNoise); P[6][6] = sq(frontend->_gpsVertVelNoise);
} }
vertVelVarClipCount = 0; vertVelVarClipCounter = 0;
} }
} }

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -86,7 +86,8 @@
// maximum number of times the vertical velocity variance can hit the lower limit before the // maximum number of times the vertical velocity variance can hit the lower limit before the
// associated states, variances and covariances are reset // associated states, variances and covariances are reset
#define VERT_VEL_VAR_CLIP_COUNT_LIM 5 #define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT)
#define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
class NavEKF3_core : public NavEKF_core_common class NavEKF3_core : public NavEKF_core_common
{ {
@ -945,7 +946,7 @@ private:
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out 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 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 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 uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix Matrix24 P; // covariance matrix