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

AP_NavEKF: Use GPS vertical velocity observations to estimate Z accel bias

This commit is contained in:
Paul Riseborough 2015-04-18 18:21:47 +10:00 committed by Randy Mackay
parent e48171ab11
commit 3e67080002
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2166,10 +2166,10 @@ void NavEKF::FuseVelPosNED()
for (uint8_t i= 0; i<=12; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
}
// Only height 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
// 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) {
if ((obsIndex == 5 || obsIndex == 2) && prevTnb.c.z > 0.5f) {
Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f);
} else {
Kfusion[13] = 0.0f;