From 6e3403ce28ba583083fcff775f2b4ae7be1b64a5 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 19 Apr 2018 11:03:50 +1000
Subject: [PATCH] EKF: prevent race condition between global position validity
 and eph reporting

---
 EKF/control.cpp    | 62 +++++++++++++++++++++++++---------------------
 EKF/ekf.h          |  1 +
 EKF/ekf_helper.cpp |  5 ++--
 3 files changed, 38 insertions(+), 30 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 3e0300f7d4..71394aebfb 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -1356,39 +1356,45 @@ void Ekf::controlVelPosFusion()
 	if (!_control_status.flags.gps &&
 			!_control_status.flags.opt_flow &&
 			!_control_status.flags.ev_pos &&
-			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
-			((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height))
-	{
-		// Reset position and velocity states if we re-commence this aiding method
-		if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
-			resetPosition();
-			resetVelocity();
-			_fuse_hpos_as_odom = false;
-			if (_time_last_fake_gps != 0) {
-				ECL_WARN("EKF stopping navigation");
-			}
+			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
+		// We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states.
+		_using_synthetic_position = true;
+
+		// Fuse synthetic position observations every 200msec
+		if ((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height) {
+			// Reset position and velocity states if we re-commence this aiding method
+			if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
+				resetPosition();
+				resetVelocity();
+				_fuse_hpos_as_odom = false;
+				if (_time_last_fake_gps != 0) {
+					ECL_WARN("EKF stopping navigation");
+				}
 
-		}
+			}
 
-		_fuse_pos = true;
-		_fuse_hor_vel = false;
-		_fuse_vert_vel = false;
-		_time_last_fake_gps = _time_last_imu;
+			_fuse_pos = true;
+			_fuse_hor_vel = false;
+			_fuse_vert_vel = false;
+			_time_last_fake_gps = _time_last_imu;
 
-		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-			_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
-		} else {
-			_posObsNoiseNE = 0.5f;
-		}
-		_vel_pos_innov[0] = 0.0f;
-		_vel_pos_innov[1] = 0.0f;
-		_vel_pos_innov[2] = 0.0f;
-		_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
-		_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
+			if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
+				_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
+			} else {
+				_posObsNoiseNE = 0.5f;
+			}
+			_vel_pos_innov[0] = 0.0f;
+			_vel_pos_innov[1] = 0.0f;
+			_vel_pos_innov[2] = 0.0f;
+			_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
+			_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
 
-		// glitch protection is not required so set gate to a large value
-		_posInnovGateNE = 100.0f;
+			// glitch protection is not required so set gate to a large value
+			_posInnovGateNE = 100.0f;
 
+		}
+	} else {
+		_using_synthetic_position = false;
 	}
 
 	// Fuse available NED velocity and position data into the main filter
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 07896a4df0..7625bd01aa 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -292,6 +292,7 @@ private:
 
 	uint64_t _time_last_fake_gps{0};	///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_ins_deadreckon_start{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
+	bool _using_synthetic_position{false};	///< true if we are using a synthetic position to constrain drift
 
 	uint64_t _time_last_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
 	uint64_t _time_last_delpos_fuse{0};	///< time the last fusion of incremental horizontal position measurements was performed (uSec)
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index f0228eb7ff..1e48c7e8a1 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1291,8 +1291,9 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first,
 
 bool Ekf::global_position_is_valid()
 {
-	// return true if we are not doing unconstrained free inertial navigation and the origin is set
-	return (_NED_origin_initialised && !_deadreckon_time_exceeded);
+	// return true if the origin is set we are not doing unconstrained free inertial navigation
+	// and have not started using synthetic position observations to constrain drift
+	return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
 }
 
 // return true if we are totally reliant on inertial dead-reckoning for position