From 6dff2df28e2c60a27122c4f54c1896bc1018e699 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 24 Jul 2016 21:27:53 +1000
Subject: [PATCH] EKF: Add method to reset wind state covariances

Set variances in polar coordinates and rotate into cartesian frame
---
 EKF/covariance.cpp | 32 ++++++++++++++++++++++++++++++++
 EKF/ekf.h          |  3 +++
 2 files changed, 35 insertions(+)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index c994c1242b..b6ad09d511 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -794,3 +794,35 @@ void Ekf::resetMagCovariance()
 		P[rc_index][rc_index] = sq(_params.mag_noise);
 	}
 }
+
+void Ekf::resetWindCovariance()
+{
+	// set the wind  covariance terms to zero
+	zeroRows(P,22,23);
+	zeroCols(P,22,23);
+
+	// calculate the wind speed and bearing
+	float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1)));
+	float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0));
+
+	// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
+	// used to calculate the initial wind speed
+	float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+	float R_yaw = sq(0.1745f);
+
+	// calculate the variance and covariance terms for the wind states
+	float cos_yaw = cosf(yaw);
+	float sin_yaw = sinf(yaw);
+	float cos_yaw_2 = sq(cos_yaw);
+	float sin_yaw_2 = sq(sin_yaw);
+	float sin_cos_yaw = sin_yaw*cos_yaw;
+	float spd_2 = sq(spd);
+	P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2;
+	P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw;
+	P[23][22] = P[22][23];
+	P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2;
+
+	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+	P[22][22] += P[4][4];
+	P[23][23] += P[5][5];
+}
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 167ed9cffa..0142a4f0c2 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -430,4 +430,7 @@ private:
 	// perform a limited reset of the magnetic field state covariances
 	void resetMagCovariance();
 
+	// perform a limited reset of the wind state covariances
+	void resetWindCovariance();
+
 };