From 9f48c0505bb093cabbb1b8818131422eb4dc0ed0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 10 Apr 2017 15:55:47 +1000
Subject: [PATCH] EKF: improve covariance stability when estimating wind

---
 EKF/covariance.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 63a66a8670..49549cfbc9 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -204,7 +204,7 @@ void Ekf::predictCovariance()
 	float wind_vel_sig;
 
 	// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
-	if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 1000.0f) {
+	if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f) {
 		wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
 
 	} else {