Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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 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){ } 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[13] = 0.0f;
Kfusion[22] = 0.0f; Kfusion[22] = 0.0f;
// Correct single IMU prediction states using velocity measurements // 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 states[i] = states[i] - Kfusion[i] * velInnov2[obsIndex]; // IMU2 velNED,posD
} }
} else { } 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[13] = 0.0f;
Kfusion[22] = 0.0f; Kfusion[22] = 0.0f;
} }
// Calculate state corrections and re-normalise the quaternions for blended IMU data predicted states // 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++) 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(); state.quat.normalize();
// Update the covariance - take advantage of direct observation of a // Update the covariance - take advantage of direct observation of a