#!/usr/bin/env python3
# Copied from https://github.com/PX4/ecl/commit/264c8c4e8681704e4719d0a03b848df8617c0863
# and modified for ArduPilot
from sympy import *
from code_gen import *
import numpy as np
# q: quaternion describing rotation from frame 1 to frame 2
# returns a rotation matrix derived form q which describes the same
# rotation
def quat2Rot(q):
q0 = q[0]
q1 = q[1]
q2 = q[2]
q3 = q[3]
# This form is the one normally used in flight dynamics and inertial navigation texts, eg
# Aircraft Control and Simulation, Stevens,B.L, Lewis,F.L, Johnson,E.N, Third Edition, eqn 1.8-18
# It does produce second order terms in the covariance prediction that can be problematic
# with single precision processing.
# It requires the quternion to be unit length.
# Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
# [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
# [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
# This form removes q1 from the 0,0, q2 from the 1,1 and q3 from the 2,2 entry and results
# in a covariance prediction that is better conditioned.
# It requires the quaternion to be unit length and is mathematically identical
# to the alternate form when q0**2 + q1**2 + q2**2 + q3**2 = 1
# See https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
Rot = Matrix([[1 - 2*(q2**2 + q3**2), 2*(q1*q2 - q0*q3) , 2*(q1*q3 + q0*q2) ],
[2*(q1*q2 + q0*q3) , 1 - 2*(q1**2 + q3**2), 2*(q2*q3 - q0*q1) ],
[2*(q1*q3-q0*q2) , 2*(q2*q3 + q0*q1) , 1 - 2*(q1**2 + q2**2)]])
return Rot
def create_cov_matrix(i, j):
if j >= i:
# return Symbol("P(" + str(i) + "," + str(j) + ")", real=True)
# legacy array format
return Symbol("P[" + str(i) + "][" + str(j) + "]", real=True)
else:
return 0
def create_yaw_estimator_cov_matrix():
# define a symbolic covariance matrix
P = Matrix(3,3,create_cov_matrix)
for index in range(3):
for j in range(3):
if index > j:
P[index,j] = P[j,index]
return P
def create_Tbs_matrix(i, j):
# return Symbol("Tbs(" + str(i) + "," + str(j) + ")", real=True)
# legacy array format
return Symbol("Tbs[" + str(i) + "][" + str(j) + "]", real=True)
def quat_mult(p,q):
r = Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3],
p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2],
p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1],
p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]])
return r
def create_symmetric_cov_matrix(n):
# define a symbolic covariance matrix
P = Matrix(n,n,create_cov_matrix)
for index in range(n):
for j in range(n):
if index > j:
P[index,j] = P[j,index]
return P
# generate equations for observation vector innovation variances
def generate_observation_vector_innovation_variances(P,state,observation,variance,n_obs):
H = observation.jacobian(state)
innovation_variance = zeros(n_obs,1)
for index in range(n_obs):
H[index,:] = Matrix([observation[index]]).jacobian(state)
innovation_variance[index] = H[index,:] * P * H[index,:].T + Matrix([variance])
IV_simple = cse(innovation_variance, symbols("IV0:1000"), optimizations='basic')
return IV_simple
# generate equations for observation Jacobian and Kalman gain
def generate_observation_equations(P,state,observation,variance,varname="HK"):
H = Matrix([observation]).jacobian(state)
innov_var = H * P * H.T + Matrix([variance])
assert(innov_var.shape[0] == 1)
assert(innov_var.shape[1] == 1)
K = P * H.T / innov_var[0,0]
extension="0:1000"
var_string = varname+extension
HK_simple = cse(Matrix([H.transpose(), K]), symbols(var_string), optimizations='basic')
return HK_simple
# generate equations for observation vector Jacobian and Kalman gain
# n_obs is the vector dimension and must be >= 2
def generate_observation_vector_equations(P,state,observation,variance,n_obs):
K = zeros(24,n_obs)
H = observation.jacobian(state)
HK = zeros(n_obs*48,1)
for index in range(n_obs):
H[index,:] = Matrix([observation[index]]).jacobian(state)
innov_var = H[index,:] * P * H[index,:].T + Matrix([variance])
assert(innov_var.shape[0] == 1)
assert(innov_var.shape[1] == 1)
K[:,index] = P * H[index,:].T / innov_var[0,0]
HK[index*48:(index+1)*48,0] = Matrix([H[index,:].transpose(), K[:,index]])
HK_simple = cse(HK, symbols("HK0:1000"), optimizations='basic')
return HK_simple
# write single observation equations to file
def write_equations_to_file(equations,code_generator_id,n_obs):
if (n_obs < 1):
return
if (n_obs == 1):
code_generator_id.print_string("Sub Expressions")
code_generator_id.write_subexpressions(equations[0])
code_generator_id.print_string("Observation Jacobians")
code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False)
code_generator_id.print_string("Kalman gains")
code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False)
else:
code_generator_id.print_string("Sub Expressions")
code_generator_id.write_subexpressions(equations[0])
for axis_index in range(n_obs):
start_index = axis_index*48
code_generator_id.print_string("Observation Jacobians - axis %i" % axis_index)
code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion", False)
code_generator_id.print_string("Kalman gains - axis %i" % axis_index)
code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion", False)
return
# derive equations for sequential fusion of optical flow measurements
def optical_flow_observation(P,state,R_to_body,vx,vy,vz):
flow_code_generator = CodeGenerator("./generated/flow_generated.cpp")
range = symbols("range", real=True) # range from camera focal point to ground along sensor Z axis
obs_var = symbols("R_LOS", real=True) # optical flow line of sight rate measurement noise variance
# Define rotation matrix from body to sensor frame
Tbs = Matrix(3,3,create_Tbs_matrix)
# Calculate earth relative velocity in a non-rotating sensor frame
relVelSensor = Tbs * R_to_body * Matrix([vx,vy,vz])
# Divide by range to get predicted angular LOS rates relative to X and Y
# axes. Note these are rates in a non-rotating sensor frame
losRateSensorX = +relVelSensor[1]/range
losRateSensorY = -relVelSensor[0]/range
# calculate the observation Jacobian and Kalman gains for the X axis
equations = generate_observation_equations(P,state,losRateSensorX,obs_var)
flow_code_generator.print_string("X Axis Equations")
write_equations_to_file(equations,flow_code_generator,1)
# calculate the observation Jacobian and Kalman gains for the Y axis
equations = generate_observation_equations(P,state,losRateSensorY,obs_var)
flow_code_generator.print_string("Y Axis Equations")
write_equations_to_file(equations,flow_code_generator,1)
flow_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
observation = Matrix([relVelSensor[1]/range,-relVelSensor[0]/range])
equations = generate_observation_vector_equations(P,state,observation,obs_var,2)
flow_code_generator_alt = CodeGenerator("./generated/flow_generated_alt.cpp")
write_equations_to_file(equations,flow_code_generator_alt,2)
flow_code_generator_alt.close()
return
# Derive equations for sequential fusion of body frame velocity measurements
def body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz):
obs_var = symbols("R_VEL", real=True) # measurement noise variance
# Calculate earth relative velocity in a non-rotating sensor frame
vel_bf = R_to_body * Matrix([vx,vy,vz])
vel_bf_code_generator = CodeGenerator("./generated/vel_bf_generated.cpp")
axes = [0,1,2]
H_obs = vel_bf.jacobian(state) # observation Jacobians
K_gain = zeros(24,3)
for index in axes:
equations = generate_observation_equations(P,state,vel_bf[index],obs_var)
vel_bf_code_generator.print_string("axis %i" % index)
vel_bf_code_generator.write_subexpressions(equations[0])
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:24]), "H_VEL", False)
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False)
vel_bf_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
equations = generate_observation_vector_equations(P,state,vel_bf,obs_var,3)
vel_bf_code_generator_alt = CodeGenerator("./generated/vel_bf_generated_alt.cpp")
write_equations_to_file(equations,vel_bf_code_generator_alt,3)
vel_bf_code_generator_alt.close()
# derive equations for fusion of dual antenna yaw measurement
def gps_yaw_observation(P,state,R_to_body):
obs_var = symbols("R_YAW", real=True) # measurement noise variance
ant_yaw = symbols("ant_yaw", real=True) # yaw angle of antenna array axis wrt X body axis
# define antenna vector in body frame
ant_vec_bf = Matrix([cos(ant_yaw),sin(ant_yaw),0])
# rotate into earth frame
ant_vec_ef = R_to_body.T * ant_vec_bf
# Calculate the yaw angle from the projection
observation = atan(ant_vec_ef[1]/ant_vec_ef[0])
equations = generate_observation_equations(P,state,observation,obs_var)
gps_yaw_code_generator = CodeGenerator("./generated/gps_yaw_generated.cpp")
write_equations_to_file(equations,gps_yaw_code_generator,1)
gps_yaw_code_generator.close()
return
# derive equations for fusion of declination
def declination_observation(P,state,ix,iy):
obs_var = symbols("R_DECL", real=True) # measurement noise variance
# the predicted measurement is the angle wrt magnetic north of the horizontal
# component of the measured field
observation = atan(iy/ix)
equations = generate_observation_equations(P,state,observation,obs_var)
mag_decl_code_generator = CodeGenerator("./generated/mag_decl_generated.cpp")
write_equations_to_file(equations,mag_decl_code_generator,1)
mag_decl_code_generator.close()
return
# derive equations for fusion of lateral body acceleration (multirotors only)
def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
obs_var = symbols("R_ACC", real=True) # measurement noise variance
Kaccx = symbols("Kaccx", real=True) # measurement noise variance
Kaccy = symbols("Kaccy", real=True) # measurement noise variance
# use relationship between airspeed along the X and Y body axis and the
# drag to predict the lateral acceleration for a multirotor vehicle type
# where propulsion forces are generated primarily along the Z body axis
vrel = R_to_body*Matrix([vx-wx,vy-wy,vz]) # predicted wind relative velocity
# Use this nonlinear model for the prediction in the implementation only
# It uses a ballistic coefficient for each axis
# accXpred = -0.5*rho*vrel[0]*vrel[0]*BCXinv # predicted acceleration measured along X body axis
# accYpred = -0.5*rho*vrel[1]*vrel[1]*BCYinv # predicted acceleration measured along Y body axis
# Use a simple viscous drag model for the linear estimator equations
# Use the derivative from speed to acceleration averaged across the
# speed range. This avoids the generation of a dirac function in the derivation
# The nonlinear equation will be used to calculate the predicted measurement in implementation
observation = Matrix([-Kaccx*vrel[0],-Kaccy*vrel[1]])
acc_bf_code_generator = CodeGenerator("./generated/acc_bf_generated.cpp")
H = observation.jacobian(state)
K = zeros(24,2)
axes = [0,1]
for index in axes:
equations = generate_observation_equations(P,state,observation[index],obs_var)
acc_bf_code_generator.print_string("Axis %i equations" % index)
write_equations_to_file(equations,acc_bf_code_generator,1)
acc_bf_code_generator.close()
# calculate a combined result for a possible reduction in operations, but will use more stack
equations = generate_observation_vector_equations(P,state,observation,obs_var,2)
acc_bf_code_generator_alt = CodeGenerator("./generated/acc_bf_generated_alt.cpp")
write_equations_to_file(equations,acc_bf_code_generator_alt,3)
acc_bf_code_generator_alt.close()
return
# yaw fusion
def yaw_observation(P,state,R_to_earth):
yaw_code_generator = CodeGenerator("./generated/yaw_generated.cpp")
# Derive observation Jacobian for fusion of 321 sequence yaw measurement
# Calculate the yaw (first rotation) angle from the 321 rotation sequence
# Provide alternative angle that avoids singularity at +-pi/2 yaw
angMeasA = atan(R_to_earth[1,0]/R_to_earth[0,0])
H_YAW321_A = Matrix([angMeasA]).jacobian(state)
H_YAW321_A_simple = cse(H_YAW321_A, symbols('SA0:200'))
angMeasB = pi/2 - atan(R_to_earth[0,0]/R_to_earth[1,0])
H_YAW321_B = Matrix([angMeasB]).jacobian(state)
H_YAW321_B_simple = cse(H_YAW321_B, symbols('SB0:200'))
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option A")
yaw_code_generator.write_subexpressions(H_YAW321_A_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW321_A_simple[1]).T, "H_YAW", False)
yaw_code_generator.print_string("calculate 321 yaw observation matrix - option B")
yaw_code_generator.write_subexpressions(H_YAW321_B_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW321_B_simple[1]).T, "H_YAW", False)
# Derive observation Jacobian for fusion of 312 sequence yaw measurement
# Calculate the yaw (first rotation) angle from an Euler 312 sequence
# Provide alternative angle that avoids singularity at +-pi/2 yaw
angMeasA = atan(-R_to_earth[0,1]/R_to_earth[1,1])
H_YAW312_A = Matrix([angMeasA]).jacobian(state)
H_YAW312_A_simple = cse(H_YAW312_A, symbols('SA0:200'))
angMeasB = pi/2 - atan(-R_to_earth[1,1]/R_to_earth[0,1])
H_YAW312_B = Matrix([angMeasB]).jacobian(state)
H_YAW312_B_simple = cse(H_YAW312_B, symbols('SB0:200'))
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option A")
yaw_code_generator.write_subexpressions(H_YAW312_A_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW312_A_simple[1]).T, "H_YAW", False)
yaw_code_generator.print_string("calculate 312 yaw observation matrix - option B")
yaw_code_generator.write_subexpressions(H_YAW312_B_simple[0])
yaw_code_generator.write_matrix(Matrix(H_YAW312_B_simple[1]).T, "H_YAW", False)
yaw_code_generator.close()
return
# 3D magnetometer fusion
def mag_observation_variance(P,state,R_to_body,i,ib):
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
m_mag = R_to_body * i + ib
# separate calculation of innovation variance equations for the y and z axes
m_mag[0]=0
innov_var_equations = generate_observation_vector_innovation_variances(P,state,m_mag,obs_var,3)
mag_innov_var_code_generator = CodeGenerator("./generated/3Dmag_innov_var_generated.cpp")
write_equations_to_file(innov_var_equations,mag_innov_var_code_generator,3)
mag_innov_var_code_generator.close()
return
# 3D magnetometer fusion
def mag_observation(P,state,R_to_body,i,ib):
obs_var = symbols("R_MAG", real=True) # magnetometer measurement noise variance
m_mag = R_to_body * i + ib
# calculate a separate set of equations for each axis
mag_code_generator = CodeGenerator("./generated/3Dmag_generated.cpp")
axes = [0,1,2]
label="HK"
for index in axes:
if (index==0):
label="HKX"
elif (index==1):
label="HKY"
elif (index==2):
label="HKZ"
else:
return
equations = generate_observation_equations(P,state,m_mag[index],obs_var,varname=label)
mag_code_generator.print_string("Axis %i equations" % index)
write_equations_to_file(equations,mag_code_generator,1)
mag_code_generator.close()
# calculate a combined set of equations for a possible reduction in operations, but will use slighlty more stack
equations = generate_observation_vector_equations(P,state,m_mag,obs_var,3)
mag_code_generator_alt = CodeGenerator("./generated/3Dmag_generated_alt.cpp")
write_equations_to_file(equations,mag_code_generator_alt,3)
mag_code_generator_alt.close()
return
# airspeed fusion
def tas_observation(P,state,vx,vy,vz,wx,wy):
obs_var = symbols("R_TAS", real=True) # true airspeed measurement noise variance
observation = sqrt((vx-wx)*(vx-wx)+(vy-wy)*(vy-wy)+vz*vz)
equations = generate_observation_equations(P,state,observation,obs_var)
tas_code_generator = CodeGenerator("./generated/tas_generated.cpp")
write_equations_to_file(equations,tas_code_generator,1)
tas_code_generator.close()
return
# sideslip fusion
def beta_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
obs_var = symbols("R_BETA", real=True) # sideslip measurement noise variance
v_rel_ef = Matrix([vx-wx,vy-wy,vz])
v_rel_bf = R_to_body * v_rel_ef
observation = v_rel_bf[1]/v_rel_bf[0]
equations = generate_observation_equations(P,state,observation,obs_var)
beta_code_generator = CodeGenerator("./generated/beta_generated.cpp")
write_equations_to_file(equations,beta_code_generator,1)
beta_code_generator.close()
return
# yaw estimator prediction and observation code
def yaw_estimator():
dt = symbols("dt", real=True) # dt (sec)
psi = symbols("psi", real=True) # yaw angle of body frame wrt earth frame
vn, ve = symbols("vn ve", real=True) # velocity in world frame (north/east) - m/sec
daz = symbols("daz", real=True) # IMU z axis delta angle measurement in body axes - rad
dazVar = symbols("dazVar", real=True) # IMU Z axis delta angle measurement variance (rad^2)
dvx, dvy = symbols("dvx dvy", real=True) # IMU x and y axis delta velocity measurement in body axes - m/sec
dvxVar, dvyVar = symbols("dvxVar dvyVar", real=True) # IMU x and y axis delta velocity measurement variance (m/s)^2
# derive the body to nav direction transformation matrix
Tbn = Matrix([[cos(psi) , -sin(psi)],
[sin(psi) , cos(psi)]])
# attitude update equation
psiNew = psi + daz
# velocity update equations
velNew = Matrix([vn,ve]) + Tbn*Matrix([dvx,dvy])
# Define the state vectors
stateVector = Matrix([vn,ve,psi])
# Define vector of process equations
newStateVector = Matrix([velNew,psiNew])
# Calculate state transition matrix
F = newStateVector.jacobian(stateVector)
# Derive the covariance prediction equations
# Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
# velocities, after bias effects have been removed.
# derive the control(disturbance) influence matrix from IMU noise to state noise
G = newStateVector.jacobian(Matrix([dvx,dvy,daz]))
# derive the state error matrix
distMatrix = Matrix([[dvxVar , 0 , 0],
[0 , dvyVar , 0],
[0 , 0 , dazVar]])
Q = G * distMatrix * G.T
# propagate covariance matrix
P = create_yaw_estimator_cov_matrix()
P_new = F * P * F.T + Q
P_new_simple = cse(P_new, symbols("S0:1000"), optimizations='basic')
yaw_estimator_covariance_generator = CodeGenerator("./generated/yaw_estimator_covariance_prediction_generated.cpp")
yaw_estimator_covariance_generator.print_string("Equations for covariance matrix prediction")
yaw_estimator_covariance_generator.write_subexpressions(P_new_simple[0])
yaw_estimator_covariance_generator.write_matrix(Matrix(P_new_simple[1]), "_ekf_gsf[model_index].P", True)
yaw_estimator_covariance_generator.close()
# derive the covariance update equation for a NE velocity observation
velObsVar = symbols("velObsVar", real=True) # velocity observation variance (m/s)^2
H = Matrix([[1,0,0],
[0,1,0]])
R = Matrix([[velObsVar , 0],
[0 , velObsVar]])
S = H * P * H.T + R
S_det_inv = 1 / S.det()
S_inv = S.inv()
K = (P * H.T) * S_inv
P_new = P - K * S * K.T
# optimize code
t, [S_det_inv_s, S_inv_s, K_s, P_new_s] = cse([S_det_inv, S_inv, K, P_new], symbols("t0:1000"), optimizations='basic')
yaw_estimator_observation_generator = CodeGenerator("./generated/yaw_estimator_measurement_update_generated.cpp")
yaw_estimator_observation_generator.print_string("Intermediate variables")
yaw_estimator_observation_generator.write_subexpressions(t)
yaw_estimator_observation_generator.print_string("Equations for NE velocity innovation variance's determinante inverse")
yaw_estimator_observation_generator.write_matrix(Matrix([[S_det_inv_s]]), "_ekf_gsf[model_index].S_det_inverse", False)
yaw_estimator_observation_generator.print_string("Equations for NE velocity innovation variance inverse")
yaw_estimator_observation_generator.write_matrix(Matrix(S_inv_s), "_ekf_gsf[model_index].S_inverse", True)
yaw_estimator_observation_generator.print_string("Equations for NE velocity Kalman gain")
yaw_estimator_observation_generator.write_matrix(Matrix(K_s), "K", False)
yaw_estimator_observation_generator.print_string("Equations for covariance matrix update")
yaw_estimator_observation_generator.write_matrix(Matrix(P_new_s), "_ekf_gsf[model_index].P", True)
yaw_estimator_observation_generator.close()
def quaternion_error_propagation():
# define quaternion state vector
q0, q1, q2, q3 = symbols("q0 q1 q2 q3", real=True)
q = Matrix([q0, q1, q2, q3])
# define truth gravity unit vector in body frame
R_to_earth = quat2Rot(q)
R_to_body = R_to_earth.T
gravity_ef = Matrix([0,0,1])
gravity_bf = R_to_body * gravity_ef
# define perturbations to quaternion state vector q
dq0, dq1, dq2, dq3 = symbols("dq0 dq1 dq2 dq3", real=True)
q_delta = Matrix([dq0, dq1, dq2, dq3])
# apply perturbations
q_perturbed = q + q_delta
# gravity unit vector in body frame after quaternion perturbation
R_to_earth_perturbed = quat2Rot(q_perturbed)
R_to_body_perturbed = R_to_earth_perturbed.T
gravity_bf_perturbed = R_to_body_perturbed * gravity_ef
# calculate the angular difference between the perturbed and unperturbed body frame gravity unit vectors
# assuming small angles
tilt_error_bf = gravity_bf.cross(gravity_bf_perturbed)
# calculate the derivative of the perturbation rotation vector wrt the quaternion perturbations
J = tilt_error_bf.jacobian(q_delta)
# remove second order terms
# we don't want the error deltas to appear in the final result
J.subs(dq0,0)
J.subs(dq1,0)
J.subs(dq2,0)
J.subs(dq3,0)
# define covaraince matrix for quaternion states
P = create_symmetric_cov_matrix(4)
# discard off diagonals
P_diag = diag(P[0,0],P[1,1],P[2,2],P[3,3])
# rotate quaternion covariances into rotation vector state space
P_rot_vec = J * P_diag * J.transpose()
P_rot_vec_simple = cse(P_rot_vec, symbols("PS0:400"), optimizations='basic')
quat_code_generator = CodeGenerator("./generated/tilt_error_cov_mat_generated.cpp")
quat_code_generator.write_subexpressions(P_rot_vec_simple[0])
quat_code_generator.write_matrix(Matrix(P_rot_vec_simple[1]), "tiltErrCovMat", False, "[", "]")
quat_code_generator.close()
def generate_code():
print('Starting code generation:')
print('Creating symbolic variables ...')
dt = symbols("dt", real=True) # dt
g = symbols("g", real=True) # gravity constant
r_hor_vel = symbols("R_hor_vel", real=True) # horizontal velocity noise variance
r_ver_vel = symbols("R_vert_vel", real=True) # vertical velocity noise variance
r_hor_pos = symbols("R_hor_pos", real=True) # horizontal position noise variance
# inputs, integrated gyro measurements
# delta angle x y z
d_ang_x, d_ang_y, d_ang_z = symbols("dax day daz", real=True) # delta angle x
d_ang = Matrix([d_ang_x, d_ang_y, d_ang_z])
# inputs, integrated accelerometer measurements
# delta velocity x y z
d_v_x, d_v_y, d_v_z = symbols("dvx dvy dvz", real=True)
d_v = Matrix([d_v_x, d_v_y,d_v_z])
u = Matrix([d_ang, d_v])
# input noise
d_ang_x_var, d_ang_y_var, d_ang_z_var = symbols("daxVar dayVar dazVar", real=True)
d_v_x_var, d_v_y_var, d_v_z_var = symbols("dvxVar dvyVar dvzVar", real=True)
var_u = Matrix.diag(d_ang_x_var, d_ang_y_var, d_ang_z_var, d_v_x_var, d_v_y_var, d_v_z_var)
# define state vector
# attitude quaternion
qw, qx, qy, qz = symbols("q0 q1 q2 q3", real=True)
q = Matrix([qw,qx,qy,qz])
R_to_earth = quat2Rot(q)
R_to_body = R_to_earth.T
# velocity in NED local frame (north, east, down)
vx, vy, vz = symbols("vn ve vd", real=True)
v = Matrix([vx,vy,vz])
# position in NED local frame (north, east, down)
px, py, pz = symbols("pn pe pd", real=True)
p = Matrix([px,py,pz])
# delta angle bias x y z
d_ang_bx, d_ang_by, d_ang_bz = symbols("dax_b day_b daz_b", real=True)
d_ang_b = Matrix([d_ang_bx, d_ang_by, d_ang_bz])
d_ang_true = d_ang - d_ang_b
# delta velocity bias x y z
d_vel_bx, d_vel_by, d_vel_bz = symbols("dvx_b dvy_b dvz_b", real=True)
d_vel_b = Matrix([d_vel_bx, d_vel_by, d_vel_bz])
d_vel_true = d_v - d_vel_b
# earth magnetic field vector x y z
ix, iy, iz = symbols("magN magE magD", real=True)
i = Matrix([ix,iy,iz])
# earth magnetic field bias in body frame
ibx, iby, ibz = symbols("ibx iby ibz", real=True)
ib = Matrix([ibx,iby,ibz])
# wind in local NE frame (north, east)
wx, wy = symbols("vwn, vwe", real=True)
w = Matrix([wx,wy])
# state vector at arbitrary time t
state = Matrix([q, v, p, d_ang_b, d_vel_b, i, ib, w])
print('Defining state propagation ...')
# kinematic processes driven by IMU 'control inputs'
q_new = quat_mult(q, Matrix([1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]]))
v_new = v + R_to_earth * d_vel_true + Matrix([0,0,g]) * dt
p_new = p + v * dt
# static processes
d_ang_b_new = d_ang_b
d_vel_b_new = d_vel_b
i_new = i
ib_new = ib
w_new = w
# predicted state vector at time t + dt
state_new = Matrix([q_new, v_new, p_new, d_ang_b_new, d_vel_b_new, i_new, ib_new, w_new])
print('Computing state propagation jacobian ...')
A = state_new.jacobian(state)
G = state_new.jacobian(u)
P = create_symmetric_cov_matrix(24)
print('Computing covariance propagation ...')
P_new = A * P * A.T + G * var_u * G.T
for index in range(24):
for j in range(24):
if index > j:
P_new[index,j] = 0
print('Simplifying covariance propagation ...')
P_new_simple = cse(P_new, symbols("PS0:400"), optimizations='basic')
print('Writing covariance propagation to file ...')
cov_code_generator = CodeGenerator("./generated/covariance_generated.cpp")
cov_code_generator.print_string("Equations for covariance matrix prediction, without process noise!")
cov_code_generator.write_subexpressions(P_new_simple[0])
cov_code_generator.write_matrix(Matrix(P_new_simple[1]), "nextP", True, "[", "]")
cov_code_generator.close()
# derive autocode for other methods
print('Computing tilt error covariance matrix ...')
quaternion_error_propagation()
print('Generating heading observation code ...')
yaw_observation(P,state,R_to_earth)
print('Generating gps heading observation code ...')
gps_yaw_observation(P,state,R_to_body)
print('Generating mag observation code ...')
mag_observation_variance(P,state,R_to_body,i,ib)
mag_observation(P,state,R_to_body,i,ib)
print('Generating declination observation code ...')
declination_observation(P,state,ix,iy)
print('Generating airspeed observation code ...')
tas_observation(P,state,vx,vy,vz,wx,wy)
print('Generating sideslip observation code ...')
beta_observation(P,state,R_to_body,vx,vy,vz,wx,wy)
print('Generating optical flow observation code ...')
optical_flow_observation(P,state,R_to_body,vx,vy,vz)
print('Generating body frame velocity observation code ...')
body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz)
print('Generating body frame acceleration observation code ...')
body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy)
print('Generating yaw estimator code ...')
yaw_estimator()
print('Code generation finished!')
if __name__ == "__main__":
generate_code()