EKF: update wind covariance initialization

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>

EKF/covariance.cpp

@@ -894,29 +894,26 @@ void Ekf::zeroMagCov()
 void Ekf::resetWindCovariance()
 {
 	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
-		// Use airspeed and zer sideslip assumption to set initial covariance values for wind states
-
-		// 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(math::radians(10.0f));
-
-		// 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);
+		// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
+		// TODO: explicitly include the sideslip angle in the derivation
+		Eulerf euler321(_state.quat_nominal);
+		const float euler_yaw = euler321(2);
+		const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+		const float initial_sideslip_uncertainty = math::radians(15.0f);
+		const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
+		const float R_yaw = sq(math::radians(10.0f));
+
+		// rotate wind velocity into earth frame aligned with vehicle yaw
+		const float Wx = _state.wind_vel(0) * cosf(euler_yaw) + _state.wind_vel(1) * sinf(euler_yaw);
+		const float Wy = -_state.wind_vel(0) * sinf(euler_yaw) + _state.wind_vel(1) * cosf(euler_yaw);
+
+		// it is safer to remove all existing correlations to other states at this time
 		P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
-		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(22,22) = R_TAS*sq(cosf(euler_yaw)) + R_yaw*sq(-Wx*sinf(euler_yaw) - Wy*cosf(euler_yaw)) + initial_wind_var_body_y*sq(sinf(euler_yaw));
+		P(22,23) = R_TAS*sinf(euler_yaw)*cosf(euler_yaw) + R_yaw*(-Wx*sinf(euler_yaw) - Wy*cosf(euler_yaw))*(Wx*cosf(euler_yaw) - Wy*sinf(euler_yaw)) - initial_wind_var_body_y*sinf(euler_yaw)*cosf(euler_yaw);
 		P(23,22) = P(22,23);
-		P(23,23) = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2;
+		P(23,23) = R_TAS*sq(sinf(euler_yaw)) + R_yaw*sq(Wx*cosf(euler_yaw) - Wy*sinf(euler_yaw)) + initial_wind_var_body_y*sq(cosf(euler_yaw));
 
 		// 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);

EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py

@@ -0,0 +1,20 @@
+import sympy as sp
+Wx, Wy, yaw, R_TAS, initial_wind_var_body_y, R_yaw = sp.symbols('Wx Wy yaw R_TAS initial_wind_var_body_y R_yaw')
+Wn = Wx * sp.cos(yaw) - Wy * sp.sin(yaw)
+We = Wx * sp.sin(yaw) + Wy * sp.cos(yaw)
+
+Wn_Wx = sp.diff(Wn, Wx)
+Wn_Wy = sp.diff(Wn, Wy)
+Wn_yaw = sp.diff(Wn, yaw)
+We_Wx = sp.diff(We, Wx)
+We_Wy = sp.diff(We, Wy)
+We_yaw = sp.diff(We, yaw)
+
+G = sp.Matrix([[Wn_Wx, Wn_Wy, Wn_yaw],[We_Wx, We_Wy, We_yaw]])
+b_wind_cov = sp.Matrix([[R_TAS, 0.0, 0.0], [0.0,initial_wind_var_body_y, 0.0], [0.0, 0.0, R_yaw]])
+i_wind_cov = G * b_wind_cov * G.T
+
+print('P[22][22] = ' + str(i_wind_cov[0,0]))
+print('P[22][23] = ' + str(i_wind_cov[0,1]))
+print('P[23][22] = ' + str(i_wind_cov[1,0]))
+print('P[23][23] = ' + str(i_wind_cov[1,1]))