Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add protection for accel bias estimation errors

Browse Source 
Don't do bias estimation if tilted by more than 60 degrees to prevent scale
factor errors affecting result unnecessarily.
Prevent Kalman gain from having the wrong sign due to numerical errors
associated with small process noise values.
Allow smaller Z accel bias process noise values to be set
This commit is contained in:
priseborough 2014-05-17 23:30:27 +10:00 committed by Andrew Tridgell
parent 3222e8f7cb
commit 5fe0d2c1b2
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -195,7 +195,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: ABIAS_PNOISE
// @DisplayName: Accelerometer bias process noise (m/s^2)
// @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
// @Range: 0.0001 - 0.001
// @Range: 0.00001 - 0.001
// @User: advanced
AP_GROUPINFO("ABIAS_PNOISE", 11, NavEKF, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT),
@ -950,7 +950,7 @@ void NavEKF::CovariancePrediction()
windVelSigma = 0.0f;
}
dAngBiasSigma = dt * constrain_float(_gyroBiasProcessNoise, 1e-7f, 1e-5f);
dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-4f, 1e-3f);
dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-5f, 1e-3f);
if (!inhibitMagStates) {
magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f);
magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f);
@ -1823,8 +1823,10 @@ void NavEKF::FuseVelPosNED()
Kfusion[i] = P[i][stateIndex]*SK;
}
// Only height observations are used to update z accel bias estimate
if (obsIndex == 5) {
Kfusion[13] = P[13][stateIndex]*SK;
// 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
if (obsIndex == 5 && prevTnb.c.z > 0.5f) {
Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f);
} else {
Kfusion[13] = 0.0f;
}