fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Convert magnetic field observation methods to use SymPy generated code (#879)

Browse Source 
* EKF: Add comparison test for mag field fusion generated code

* EKF: convert mag field fusion to use SymPy generated code

* EKF: Add test comparison program for yaw fusion equations

* Stop setting 0 to 0

* Reduce if/else statement to only if

* EKF: more accurate implementation for sequential fusion of magnetometer data

* test: update change indicator

* Use matrix::SparseVector<float, 24, ...> for observation jacobian

* Adapt the auto code generation to allow for different bracket styles

* Add auto generated code for mag fusion

* Add generic computation of KHP

* Apply generic computation of KHP to mag fusion

* tests: update change indicator

* tests: update change indicator

Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
2020-08-11 18:57:22 +10:00
committed by GitHub
co-authored by kamilritz
parent ec93490890
commit 21cc46edd7
12 changed files with 2574 additions and 782 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/main.py
+53 -14
View File
@@ -48,14 +48,28 @@ def create_symmetric_cov_matrix():
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):
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]
HK_simple = cse(Matrix([H.transpose(), K]), symbols("HK0:1000"), optimizations='basic')
extension="0:1000"
var_string = varname+extension
HK_simple = cse(Matrix([H.transpose(), K]), symbols(var_string), optimizations='basic')
return HK_simple
@@ -86,18 +100,18 @@ def write_equations_to_file(equations,code_generator_id,n_obs):
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")
code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False, ".at<", ">()")
code_generator_id.print_string("Kalman gains")
code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
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")
code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion", False, ".at<", ">()")
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")
code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion", False, "(", ")")
return
@@ -157,8 +171,8 @@ def body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz):
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")
vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion")
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()
@@ -267,11 +281,11 @@ def yaw_observation(P,state,R_to_earth):
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")
yaw_code_generator.write_matrix(Matrix(H_YAW321_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
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")
yaw_code_generator.write_matrix(Matrix(H_YAW321_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
# Derive observation Jacobian for fusion of 312 sequence yaw measurement
# Calculate the yaw (first rotation) angle from an Euler 312 sequence
@@ -286,16 +300,31 @@ def yaw_observation(P,state,R_to_earth):
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")
yaw_code_generator.write_matrix(Matrix(H_YAW312_A_simple[1]).T, "H_YAW", False, ".at<", ">()")
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")
yaw_code_generator.write_matrix(Matrix(H_YAW312_B_simple[1]).T, "H_YAW", False, ".at<", ">()")
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
@@ -306,8 +335,17 @@ def mag_observation(P,state,R_to_body,i,ib):
mag_code_generator = CodeGenerator("./generated/3Dmag_generated.cpp")
axes = [0,1,2]
label="HK"
for index in axes:
equations = generate_observation_equations(P,state,m_mag[index],obs_var)
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)
@@ -459,7 +497,7 @@ def generate_code():
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.write_matrix(Matrix(P_new_simple[1]), "nextP", True, "(", ")")
cov_code_generator.close()
@@ -469,6 +507,7 @@ def generate_code():
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)