From 0d7d804bd543964c97bed8a860f012a6e1571f00 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 20 Jun 2017 09:40:25 +1000
Subject: [PATCH] AP_NavEKF3: Fix terrain estimator innovation consistency
 check

---
 libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
index c7415fcddb..24271a45ec 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
@@ -148,9 +148,8 @@ void NavEKF3_core::EstimateTerrainOffset()
             // calculate the innovation consistency test ratio
             auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
 
-            // Check the innovation for consistency and don't fuse if > 5Sigma
-            if ((sq(innovRng)*SK_RNG) < 25.0f)
-            {
+            // Check the innovation test ratio and don't fuse if too large
+            if (auxRngTestRatio < 1.0f) {
                 // correct the state
                 terrainState -= K_RNG * innovRng;