Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Prevent Z accel bias changing during ground effect takeoff 

The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
This commit is contained in:
Paul Riseborough 2015-04-28 07:22:06 +10:00 committed by Randy Mackay
parent 2cba60c731
commit 3986851c51
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1254,9 +1254,13 @@ void NavEKF::CovariancePrediction()
} }
} }
// 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
if (takeoffExpected) {
processNoise[13] = 0.0f;
} else if (!vehicleArmed) {
processNoise[13] = dVelBiasSigma * accelBiasNoiseScaler;
} else {
processNoise[13] = dVelBiasSigma; processNoise[13] = dVelBiasSigma;
if (!vehicleArmed) {
processNoise[13] *= accelBiasNoiseScaler;
} }
for (uint8_t i=14; i<=15; i++) processNoise[i] = windVelSigma; for (uint8_t i=14; i<=15; i++) processNoise[i] = windVelSigma;
for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma; for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;
@ -2172,7 +2176,8 @@ void NavEKF::FuseVelPosNED()
// Only height and height rate observations are used to update z accel bias estimate // Only height and height rate observations are used to update z accel bias estimate
// Protect Kalman gain from ill-conditioning // Protect Kalman gain from ill-conditioning
// Don't update Z accel bias if off-level by greater than 60 degrees to avoid scale factor error effects // Don't update Z accel bias if off-level by greater than 60 degrees to avoid scale factor error effects
if ((obsIndex == 5 || obsIndex == 2) && prevTnb.c.z > 0.5f) { // Don't update if we are taking off with ground effect
if ((obsIndex == 5 || obsIndex == 2) && prevTnb.c.z > 0.5f && !getTakeoffExpected()) {
Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f); Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f);
} else { } else {
Kfusion[13] = 0.0f; Kfusion[13] = 0.0f;