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

AP_NavEKF3: Fix indexing error 

Caused by copy and paste from AP_NavEKF2 which useds index 5 for vertical velocity. EKF3  should use index 6.
This commit is contained in:
Paul Riseborough 2019-03-15 06:06:41 +11:00 committed by Andrew Tridgell
parent 0d3e1a7d23
commit b7e0a2345c
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -112,7 +112,7 @@ void NavEKF3_core::EstimateTerrainOffset()
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5];
float Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms;