Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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++) { for (uint8_t i= 0; i<=12; i++) {
Kfusion[i] = P[i][stateIndex]*SK; 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 // 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 && 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); Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f);
} else { } else {
Kfusion[13] = 0.0f; Kfusion[13] = 0.0f;