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

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

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
index 24271a45ec..c0bc369c17 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
@@ -187,10 +187,10 @@ void NavEKF3_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);