From bdf5b3e00324ea3a9357c8e916f57554f514b179 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 7 May 2018 11:20:17 +1000
Subject: [PATCH] EKF: Don't assume large position uncertainty when starting
 optical flow nav

---
 EKF/ekf_helper.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 5ecc258e82..aea0ad8fbf 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -168,7 +168,9 @@ bool Ekf::resetPosition()
 
 		}
 
-		setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
+		// estimate is relative to initial positon in this mode, so we start with zero error.
+		zeroCols(P,7,8);
+		zeroRows(P,7,8);
 
 	} else {
 		// Used when falling back to non-aiding mode of operation