Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fix bug in small EKF velocity fusion

Browse Source
This commit is contained in:
Paul Riseborough 2015-01-31 22:29:17 +11:00 committed by Andrew Tridgell
parent 8d6f0d08c9
commit 925d625ed1
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_SmallEKF.cpp
View File

@ -566,7 +566,9 @@ void SmallEKF::fuseVelocity(bool yawInit)
// Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
// if heading isn't aligned, use zero velocity (static assumption)
if (yawInit) {
innovation[obsIndex] = state.velocity[obsIndex] - state.velocity[obsIndex];
Vector3f measVelNED;
_main_ekf.getVelNED(measVelNED);
innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
} else {
innovation[obsIndex] = state.velocity[obsIndex];
}