From 21270bb06ad7297c1b0e18697576c5a9a87f8eac Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 Jan 2017 08:58:01 +1100
Subject: [PATCH] EKF: Use last known position when resuming OF aiding

---
 EKF/control.cpp | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 0b38b5a34f..28d8cc9465 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -279,15 +279,21 @@ void Ekf::controlOpticalFlowFusion()
 						_state.pos(0) = 0.0f;
 						_state.pos(1) = 0.0f;
 
-						// reset the corresponding covariances
-						// we are by definition at the origin at commencement so variances are also zeroed
-						zeroRows(P,7,8);
-						zeroCols(P,7,8);
-
-						// align the output observer to the EKF states
-						alignOutputFilter();
+					} else {
+						// set to the last known position
+						_state.pos(0) = _last_known_posNE(0);
+						_state.pos(1) = _last_known_posNE(1);
 
 					}
+
+					// reset the corresponding covariances
+					// we are by definition at the origin at commencement so variances are also zeroed
+					zeroRows(P,7,8);
+					zeroCols(P,7,8);
+
+					// align the output observer to the EKF states
+					alignOutputFilter();
+
 				}
 			}