From c69c5440a0378bb48b856a2f25e52f73c9f4530e Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Tue, 20 Jun 2017 14:21:54 +1000
Subject: [PATCH] AP_NavEKF2: Fix calculation of predicted LOS rate in terrain
 estimator

---
 libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
index 05c05d75bb..915c5a19d6 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
@@ -184,10 +184,10 @@ void NavEKF2_core::EstimateTerrainOffset()
 
             // divide velocity by range, subtract body rates and apply scale factor to
             // get predicted sensed angular optical rates relative to X and Y sensor axes
-            losPred =   relVelSensor.length()/flowRngPred;
+            losPred =   norm(relVelSensor.x, relVelSensor.y)/flowRngPred;
 
             // calculate innovations
-            auxFlowObsInnov = losPred - sqrtf(sq(ofDataDelayed.flowRadXYcomp[0]) + sq(ofDataDelayed.flowRadXYcomp[1]));
+            auxFlowObsInnov = losPred - norm(ofDataDelayed.flowRadXYcomp.x, ofDataDelayed.flowRadXYcomp.y);
 
             // calculate observation jacobian
             float t3 = sq(q0);