From bc34b147798c5bfa5a6eaec5787a88a831947a26 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 3 May 2016 18:47:29 +1000
Subject: [PATCH] EKF: Initialise height state variance to a value compatible
 with the measurement

A large height state variance and small measurement variance can destabilise the filter in the first few seconds after alignment
---
 EKF/covariance.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 0755a800b5..c78ab046e0 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -69,7 +69,15 @@ void Ekf::initialiseCovariance()
 	// position
 	P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 	P[8][8] = P[7][7];
-	P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
+	if (_control_status.flags.rng_hgt) {
+		P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
+	} else if (_control_status.flags.gps_hgt) {
+		float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+		float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+		P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
+	} else {
+		P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
+	}
 
 	// gyro bias
 	P[10][10] = sq(0.1f * dt);