From ad02818b3d61bef8e9c1144e2151a3eb09e1ff0b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 27 May 2016 14:00:50 +1000
Subject: [PATCH] EKF: Improve height reset for external vision

reset to measurement closest in time to fusion time horizon
---
 EKF/ekf_helper.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index bc33a37692..bda1ed8da7 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -208,12 +208,15 @@ void Ekf::resetHeight()
 		// initialize vertical position with newest measurement
 		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
 
-		if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
+		// use the most recent data if it's time offset from the fusion time horizon is smaller
+		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
+		int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
+		if (abs(dt_newest) < abs(dt_delayed)) {
 			_state.pos(2) = ev_newest.posNED(2);
-
 		} else {
-			// TODO: reset to last known baro based estimate
+			_state.pos(2) = _ev_sample_delayed.posNED(2);
 		}
+
 	}
 
 	// reset the vertical velocity covariance values