From 2ee1c549becdc412eab3a2ca157794c72a5e806f Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 25 Oct 2014 08:03:59 +1100
Subject: [PATCH] AP_NavEKF : improve criteria used to inhibit scale factor
 estimation

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

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 2dd88c7be0..e8621e9bb1 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3689,8 +3689,9 @@ void NavEKF::SetFlightAndFusionModes()
     } else {
         inhibitGndState = false;
     }
-    // Don't update focal length offset state if there is no range finder or GPS, or we are flying slowly
-    if (!useRngFinder() || _fusionModeGPS == 3 || gndSpdSq < 5.0f) {
+    // Don't update focal length offset state if there is no range finder or GPS, or we are not flying fas enough to generate a useful LOS rate
+    float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
+    if (!useRngFinder() || _fusionModeGPS == 3 || losRateSq < 0.09f || gndSpdSq < 9.0f) {
         fScaleInhibit = true;
     } else {
         fScaleInhibit = false;