From d7ad45ebda9d098f112898c717f065ce27815fed Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 10 Nov 2014 18:39:05 +1100 Subject: [PATCH] AP_NavEKF: Don't fuse flow measurements if too big --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8bfac4010b..5f9095737b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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