Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -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);
}
vertVelVarClipCount = 0;
vertVelVarClipCounter = 0;
}
// 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;
tasTimeout = true;
badIMUdata = false;
vertVelVarClipCount = 0;
vertVelVarClipCounter = 0;
finalInflightYawInit = false;
dtIMUavg = ins.get_loop_delta_t();
dtEkfAvg = EKF_TARGET_DT;
@ -1717,6 +1717,10 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr)
// constrain values to prevent ill-conditioning
ConstrainVariances();
if (vertVelVarClipCounter > 0) {
vertVelVarClipCounter--;
}
calcTiltErrorVariance();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -1806,8 +1810,11 @@ void NavEKF3_core::ConstrainVariances()
// 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) {
// this counter is decremented by 1 each prediction cycle in CovariancePrediction
// 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
zeroRows(P,6,6);
zeroCols(P,6,6);
@ -1821,7 +1828,7 @@ void NavEKF3_core::ConstrainVariances()
{
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
// 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
{
@ -945,7 +946,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
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
Matrix24 P; // covariance matrix