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

AP_NavEKF: Don't fuse flow measurements if too big

This commit is contained in:
priseborough 2014-11-10 18:39:05 +11:00 committed by Andrew Tridgell
parent 2dff76394d
commit d7ad45ebda
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3019,8 +3019,8 @@ void NavEKF::FuseOptFlow()
// calculate the innovation consistency test ratio
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(_flowInnovGate) * varInnovOptFlow[obsIndex]);
// Check the innovation for consistency and don't fuse if out of bounds
if (flowTestRatio[obsIndex] < 1.0f) {
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if ((flowTestRatio[obsIndex]) < 1.0f && (flowRadXY[obsIndex] < 2.0f)) {
// Reset the timer for velocity fusion timeout. This prevents a velocity timeout from being declared if we have to momentarily go into velocity hold mode.
velFailTime = imuSampleTime_ms;
// correct the state vector