You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
2.1 KiB
67 lines
2.1 KiB
#!/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])) |