aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF : Fix bug in Z accel bias update for IMU1

Browse Source
This commit is contained in:
Paul Riseborough 2015-03-20 16:53:31 -07:00 committed by Andrew Tridgell
parent 5d0952ba23
commit 92df3adb5e
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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;
}
}
}
}