mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: Prevent EKF yaw errors due to fast yaw spins
Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail. This patch introduces the following protections: 1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet) 2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state 3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
This commit is contained in:
parent
08382373f1
commit
a232606fc9
|
@ -1181,6 +1181,16 @@ void NavEKF::UpdateStrapdownEquationsNED()
|
||||||
// capture current angular rate to augmented state vector for use by optical flow fusion
|
// capture current angular rate to augmented state vector for use by optical flow fusion
|
||||||
state.omega = correctedDelAng / dtIMUactual;
|
state.omega = correctedDelAng / dtIMUactual;
|
||||||
|
|
||||||
|
// LPF the yaw rate using a 1 second time constant yaw rate and determine if we are doing continual
|
||||||
|
// fast rotations that can cause problems due to gyro scale factor errors.
|
||||||
|
float alphaLPF = constrain_float(dtIMUactual, 0.0f, 1.0f);
|
||||||
|
yawRateFilt += (state.omega.z - yawRateFilt)*alphaLPF;
|
||||||
|
if (fabs(yawRateFilt) > 1.0f) {
|
||||||
|
highYawRate = true;
|
||||||
|
} else {
|
||||||
|
highYawRate = false;
|
||||||
|
}
|
||||||
|
|
||||||
// limit states to protect against divergence
|
// limit states to protect against divergence
|
||||||
ConstrainStates();
|
ConstrainStates();
|
||||||
}
|
}
|
||||||
|
@ -1247,6 +1257,11 @@ void NavEKF::CovariancePrediction()
|
||||||
processNoise[i] *= gyroBiasNoiseScaler;
|
processNoise[i] *= gyroBiasNoiseScaler;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
// if we are yawing rapidly, inhibit yaw gyro bias learning to prevent gyro scale factor errors from corrupting the bias estimate
|
||||||
|
if (highYawRate) {
|
||||||
|
processNoise[12] = 0.0f;
|
||||||
|
P[12][12] = 0.0f;
|
||||||
|
}
|
||||||
// scale accel bias noise when disarmed to allow for faster bias estimation
|
// scale accel bias noise when disarmed to allow for faster bias estimation
|
||||||
// inhibit bias estimation during takeoff with ground effect to prevent bad bias learning
|
// inhibit bias estimation during takeoff with ground effect to prevent bad bias learning
|
||||||
if (expectGndEffectTakeoff) {
|
if (expectGndEffectTakeoff) {
|
||||||
|
@ -1279,7 +1294,8 @@ void NavEKF::CovariancePrediction()
|
||||||
_gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f);
|
_gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f);
|
||||||
daxCov = sq(dt*_gyrNoise);
|
daxCov = sq(dt*_gyrNoise);
|
||||||
dayCov = sq(dt*_gyrNoise);
|
dayCov = sq(dt*_gyrNoise);
|
||||||
dazCov = sq(dt*_gyrNoise);
|
// Account for 3% scale factor error on Z angular rate. This reduces chance of continuous fast rotations causing loss of yaw reference.
|
||||||
|
dazCov = sq(dt*_gyrNoise) + sq(dt*0.03f*yawRateFilt);
|
||||||
_accNoise = constrain_float(_accNoise, 5e-2f, 1.0f);
|
_accNoise = constrain_float(_accNoise, 5e-2f, 1.0f);
|
||||||
dvxCov = sq(dt*_accNoise);
|
dvxCov = sq(dt*_accNoise);
|
||||||
dvyCov = sq(dt*_accNoise);
|
dvyCov = sq(dt*_accNoise);
|
||||||
|
@ -2604,8 +2620,9 @@ void NavEKF::FuseMagnetometer()
|
||||||
if (!magHealth && !constPosMode) {
|
if (!magHealth && !constPosMode) {
|
||||||
Kfusion[j] *= 0.25f;
|
Kfusion[j] *= 0.25f;
|
||||||
}
|
}
|
||||||
// If in the air and there is no other form of heading reference, we strengthen the compass attitude correction
|
// If in the air and there is no other form of heading reference or we are yawing rapidly which creates larger inertial yaw errors,
|
||||||
if (vehicleArmed && constPosMode && j <= 3) {
|
// we strengthen the magnetometer attitude correction
|
||||||
|
if (vehicleArmed && (constPosMode || highYawRate) && j <= 3) {
|
||||||
Kfusion[j] *= 4.0f;
|
Kfusion[j] *= 4.0f;
|
||||||
}
|
}
|
||||||
// We don't need to spread corrections for non-dynamic states or if we are in a constant postion mode
|
// We don't need to spread corrections for non-dynamic states or if we are in a constant postion mode
|
||||||
|
@ -4645,6 +4662,8 @@ void NavEKF::InitialiseVariables()
|
||||||
gpsSpdAccuracy = 0.0f;
|
gpsSpdAccuracy = 0.0f;
|
||||||
baroHgtOffset = 0.0f;
|
baroHgtOffset = 0.0f;
|
||||||
gpsAidingBad = false;
|
gpsAidingBad = false;
|
||||||
|
highYawRate = false;
|
||||||
|
yawRateFilt = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return true if we should use the airspeed sensor
|
// return true if we should use the airspeed sensor
|
||||||
|
|
|
@ -662,6 +662,8 @@ private:
|
||||||
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
|
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
|
||||||
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
|
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
|
||||||
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
|
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
|
||||||
|
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
|
||||||
|
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
|
||||||
|
|
||||||
// Used by smoothing of state corrections
|
// Used by smoothing of state corrections
|
||||||
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement
|
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement
|
||||||
|
|
Loading…
Reference in New Issue