From 92df3adb5e289813b45d4d3aa63bb3c66d13d651 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Mar 2015 16:53:31 -0700 Subject: [PATCH] AP_NavEKF : Fix bug in Z accel bias update for IMU1 --- libraries/AP_NavEKF/AP_NavEKF.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 97cda492e4..47d216a4bf 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2191,15 +2191,18 @@ void NavEKF::FuseVelPosNED() // attitude, velocity and position corrections are spread across multiple prediction cycles between now // and the anticipated time for the next measurement. // Don't spread quaternion corrections if total angle change across predicted interval is going to exceed 0.1 rad + // Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations bool highRates = ((gpsUpdateCountMax * correctedDelAng.length()) > 0.1f); for (uint8_t i = 0; i<=21; i++) { - if ((i <= 3 && highRates) || i >= 10 || constPosMode || constVelMode) { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } else { - if (obsIndex == 5) { - hgtIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * hgtUpdateCountMaxInv; + if (i != 13) { + if ((i <= 3 && highRates) || i >= 10 || constPosMode || constVelMode) { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; } else { - gpsIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * gpsUpdateCountMaxInv; + if (obsIndex == 5) { + hgtIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * hgtUpdateCountMaxInv; + } else { + gpsIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * gpsUpdateCountMaxInv; + } } } }