From fff2bd50f630a15ac6b5172b8fcb21b676390815 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 10 Feb 2016 12:53:24 +1100
Subject: [PATCH] EKF: Fix bugs in position and velocity resets

The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states  was being incorrectly reset to the GPS position and Baro height on initial alignment.
---
 EKF/ekf.cpp        |  3 ++-
 EKF/ekf.h          |  2 ++
 EKF/ekf_helper.cpp | 25 +++++++++++++++++++------
 3 files changed, 23 insertions(+), 7 deletions(-)

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.h b/EKF/ekf.h
index 74ee55932f..a630f3bc3b 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -181,6 +181,8 @@ private:
 
 	void resetPosition();
 
+	void resetHeight();
+
 	void makeCovSymetrical();
 
 	void limitCov();
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 41e66cad86..c5c830a326 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -54,7 +54,7 @@ void Ekf::resetVelocity()
 	// if we have a valid GPS measurement use it to initialise velocity states
 	gpsSample gps_newest = _gps_buffer.get_newest();
 
-	if (_time_last_imu - gps_newest.time_us < 100000) {
+	if (_time_last_imu - gps_newest.time_us < 400000) {
 		_state.vel = gps_newest.vel;
 
 	} else {
@@ -66,19 +66,32 @@ void Ekf::resetVelocity()
 // gps measurement then use for position initialisation
 void Ekf::resetPosition()
 {
-	// if we have a valid GPS measurement use it to initialise position states
+	// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
 	gpsSample gps_newest = _gps_buffer.get_newest();
 
-	if (_time_last_imu - gps_newest.time_us < 100000) {
-		_state.pos(0) = gps_newest.pos(0);
-		_state.pos(1) = gps_newest.pos(1);
+	float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
+
+	if (time_delay < 0.4f) {
+		_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
+		_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
 
 	} else {
 		// XXX use the value of the last known position
 	}
+}
 
+// Reset height state using the last baro measurement
+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)