From 33a2bd0292888a83c57e5b228a6310a450b6a96c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 29 Oct 2019 15:40:58 +0100 Subject: [PATCH] added python script for derivation of initial wind covariance matrix Signed-off-by: RomanBapst --- EKF/python/wind_cov_init/derivation.py | 67 ++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 EKF/python/wind_cov_init/derivation.py diff --git a/EKF/python/wind_cov_init/derivation.py b/EKF/python/wind_cov_init/derivation.py new file mode 100644 index 0000000000..0b2708d214 --- /dev/null +++ b/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])) \ No newline at end of file