diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 8a9987dfea..32e4b87c47 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -272,9 +272,10 @@ bool Ekf::initialiseFilter(void)
 		// calculate the averaged barometer reading
 		_baro_at_alignment = _baro_sum / (float)_baro_counter;
 
+		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
 		resetVelocity();
-		resetPosition();
 
+		// initialise the state covariance matrix
 		initialiseCovariance();
 
 		return true;
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 56f11c3be9..5796cd8628 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -85,7 +85,13 @@ void Ekf::resetHeight()
 {
 	// if we have a valid height measurement, use it to initialise the vertical position state
 	baroSample baro_newest = _baro_buffer.get_newest();
-	_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
+
+	if (_time_last_imu - baro_newest.time_us < 200000) {
+		_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
+
+	} else {
+		// XXX use the value of the last known position
+	}
 }
 
 #if defined(__PX4_POSIX) && !defined(__PX4_QURT)