diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 8e6d02ba56..7bda4870f9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -274,7 +274,7 @@ void NavEKF3_core::ResetHeight(void) { P[6][6] = sq(frontend->_gpsVertVelNoise); } - vertVelVarClipCount = 0; + vertVelVarClipCounter = 0; } // Zero the EKF height datum diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d354b36444..017eeb4558 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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; } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 257ed7d9a0..72aa564c14 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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