added python script for derivation of initial wind covariance matrix

Signed-off-by: RomanBapst <bapstroman@gmail.com>

EKF/python/wind_cov_init/derivation.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
+"""
+Created on Tue Oct 29 14:11:58 2019
+
+@author: roman
+"""
+
+from sympy import *
+
+################## Here are the variables you can change to see the effects on the cov matrix ###########################
+yaw_init =  0.5
+
+# ground speed in body frame (comes from ekf2)
+groundspeed_body_x_init = 5
+groundspeed_body_y_init = 5
+
+# true airspeed measured by pitot tube
+V_init = 7
+
+# heading variance
+R_yaw_init = rad(15.0)**2
+
+# sideslip variance
+R_beta_init = rad(15.0)**2
+
+# True airspeed measurement variance
+R_tas_init = 1.4**2
+
+#########################################################################################################################
+
+# define symbols: true airspeed, sidslip angle, 
+V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body')
+R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw')
+
+
+# body x/y component of relative wind vector ( V is what the airspeed sensor measures)
+Vx = V * cos(beta)
+Vy = V * sin(beta)
+
+
+# wind in body frame
+wind_body_x = groundspeed_body_x - Vx
+wind_body_y = groundspeed_body_y - Vy
+
+# wind in earth frame
+wind_n = cos(yaw) * wind_body_x - sin(yaw) * wind_body_y
+wind_e = sin(yaw) * wind_body_x + cos(yaw) * wind_body_y
+wind_earth = Matrix([wind_n, wind_e])
+
+# jacobian of earth wind vector with respect to states with known uncertainties
+G = wind_earth.jacobian([V, beta, yaw])
+
+# initial covariance matrix
+P = Matrix([[R_tas, 0, 0], [0, R_beta,0], [0,0,R_yaw]])
+
+# earth wind covariance matrix, assume 0 sideslip angle
+P_wind_earth = (G*P*G.T).subs([(beta, 0)])
+
+P_wind_earth_numeric = P_wind_earth.subs([(V, V_init),(yaw, yaw_init), (R_tas, R_tas_init), (R_yaw, R_yaw_init), (R_beta, R_beta_init)])
+P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspeed_body_x_init), (groundspeed_body_y, groundspeed_body_y_init) ])
+
+
+print('P[22][22] = ' + str(P_wind_earth_numeric[0,0]))
+print('P[22][23] = ' + str(P_wind_earth_numeric[0,1]))
+print('P[23][22] = ' + str(P_wind_earth_numeric[1,0]))
+print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))