mirror of https://github.com/ArduPilot/ardupilot
702 lines
29 KiB
Python
702 lines
29 KiB
Python
# 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 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()
|