aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Reduce output observer vertical velocity error when bad IMU

Browse Source
This commit is contained in:
Paul Riseborough 2021-09-22 10:49:29 +10:00 committed by Andrew Tridgell
parent 74a7e3b64a
commit c828bdfbc9
2 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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