From c828bdfbc91b59f4ccf94e4f9bd3d3f8aeeae1d5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 22 Sep 2021 10:49:29 +1000 Subject: [PATCH] AP_NavEKF3: Reduce output observer vertical velocity error when bad IMU --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 20 ++++++++++++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 + 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8158133dbc..bd7bd9f961 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -955,6 +955,14 @@ void NavEKF3_core::calcOutputStates() Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity); Vector3F posErr = (stateStruct.position - outputDataDelayed.position); + if (badIMUdata) { + // When IMU accel is bad, calculate an integral that will be used to drive the difference + // between the output state and internal EKF state at the delayed time horizon to zero. + badImuVelErrIntegral += (stateStruct.velocity.z - outputDataNew.velocity.z); + } else { + badImuVelErrIntegral = velErrintegral.z; + } + // collect magnitude tracking error for diagnostics outputTrackError.x = deltaAngErr.length(); outputTrackError.y = velErr.length(); @@ -969,8 +977,16 @@ void NavEKF3_core::calcOutputStates() // use a PI feedback to calculate a correction that will be applied to the output state history posErrintegral += posErr; velErrintegral += velErr; - Vector3F velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; - Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f; + Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1F; + Vector3F velCorrection; + velCorrection.x = velErr.x * velPosGain + velErrintegral.x * sq(velPosGain) * 0.1F; + velCorrection.y = velErr.y * velPosGain + velErrintegral.y * sq(velPosGain) * 0.1F; + if (badIMUdata) { + velCorrection.z = velErr.z * velPosGain + badImuVelErrIntegral * sq(velPosGain) * 0.07F; + velErrintegral.z = badImuVelErrIntegral; + } else { + velCorrection.z = velErr.z * velPosGain + velErrintegral.z * sq(velPosGain) * 0.1F; + } // loop through the output filter state history and apply the corrections to the velocity and position states // this method is too expensive to use for the attitude states due to the quaternion operations required diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 0398c2eb4f..fa1c56bea7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1091,6 +1091,7 @@ private: Vector3F delAngCorrection; // correction applied to delta angles used by output observer to track the EKF Vector3F velErrintegral; // integral of output predictor NED velocity tracking error (m) Vector3F posErrintegral; // integral of output predictor NED position tracking error (m.sec) + ftype badImuVelErrIntegral; // integral of output predictor D velocity tracking error when bad IMU data is detected (m) ftype innovYaw; // compass yaw angle innovation (rad) uint32_t timeTasReceived_ms; // time last TAS data was received (msec) bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system