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

AP_NavEKF : Fix bug that prevents Zacc bias state variance being updated

This commit is contained in:
Paul Riseborough 2014-03-02 11:18:11 +11:00 committed by Andrew Tridgell
parent 573b3210dd
commit a39d00fc13
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1719,11 +1719,8 @@ void NavEKF::FuseVelPosNED()
{
states[i] = states[i] - Kfusion[i] * hgtInnov2; // IMU2 velNED,posD
}
// don't allow subsequent fusion operations to modify the Z accel bias state
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
} else if (obsIndex == 0 || obsIndex == 1 || obsIndex == 2){
// don't allow subsequent fusion operations to modify the Z accel bias state
// don't modify the Z accel bias states when fusing GPS velocity measurements
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
// Correct single IMU prediction states using velocity measurements
@ -1736,14 +1733,17 @@ void NavEKF::FuseVelPosNED()
states[i] = states[i] - Kfusion[i] * velInnov2[obsIndex]; // IMU2 velNED,posD
}
} else {
// don't allow subsequent fusion operations to modify the Z accel bias state
// don't modify the Z accel bias states for IMU1 and IMU2 when fusing GPS horizontal position measurements
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
}
// Calculate state corrections and re-normalise the quaternions for blended IMU data predicted states
// Don't update the Zacc bias state becasue it has already been updated
for (uint8_t i = 0; i<=indexLimit; i++)
{
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
if (i != 13) {
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
}
}
state.quat.normalize();
// Update the covariance - take advantage of direct observation of a