From cb0b4c596db5da729a28e1ff13a71d3739db4214 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 9 Jan 2022 20:54:48 +1100 Subject: [PATCH] ekf2: add terrain state to derivation --- .../ekf2/EKF/python/ekf_derivation/main.py | 42 ++++++++++--------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/main.py b/src/modules/ekf2/EKF/python/ekf_derivation/main.py index 2bb0d3b719..69ef5450d7 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/main.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/main.py @@ -59,10 +59,10 @@ def quat_mult(p,q): def create_symmetric_cov_matrix(): # define a symbolic covariance matrix - P = Matrix(24,24,create_cov_matrix) + P = Matrix(25,25,create_cov_matrix) - for index in range(24): - for j in range(24): + for index in range(25): + for j in range(25): if index > j: P[index,j] = P[j,index] @@ -96,16 +96,16 @@ def generate_observation_equations(P,state,observation,variance,varname="HK"): # 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) + K = zeros(25,n_obs) H = observation.jacobian(state) - HK = zeros(n_obs*48,1) + HK = zeros(n_obs*50,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[index*50:(index+1)*50,0] = Matrix([H[index,:].transpose(), K[:,index]]) HK_simple = cse(HK, symbols("HK0:1000"), optimizations='basic') @@ -120,18 +120,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", False, ".at<", ">()") + code_generator_id.write_matrix(Matrix(equations[1][0][0:25]), "Hfusion", False, ".at<", ">()") code_generator_id.print_string("Kalman gains") - code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")") + code_generator_id.write_matrix(Matrix(equations[1][0][25:]), "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 + start_index = axis_index*50 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, ".at<", ">()") + code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+25]), "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", False, "(", ")") + code_generator_id.write_matrix(Matrix(equations[1][0][start_index+25:start_index+50]), "Kfusion", False, "(", ")") return @@ -185,14 +185,14 @@ def body_frame_velocity_observation(P,state,R_to_body,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) + K_gain = zeros(25,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.write_matrix(Matrix(equations[1][0][0:25]), "H_VEL", False, "(", ")") + vel_bf_code_generator.write_matrix(Matrix(equations[1][0][25:]), "Kfusion", False, "(", ")") vel_bf_code_generator.close() @@ -270,7 +270,7 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy): acc_bf_code_generator = CodeGenerator("./generated/acc_bf_generated.cpp") H = observation.jacobian(state) - K = zeros(24,2) + K = zeros(25,2) axes = [0,1] for index in axes: equations = generate_observation_equations(P,state,observation[index],obs_var) @@ -569,8 +569,11 @@ def generate_code(): wx, wy = symbols("vwn, vwe", real=True) w = Matrix([wx,wy]) + # vertical position of terrain underneath vehicle + ptz = symbols("ptd", real=True) + # state vector at arbitrary time t - state = Matrix([q,v,p,d_ang_b,d_vel_b,i,ib,w]) + state = Matrix([q, v, p, d_ang_b, d_vel_b, i, ib, w, ptz]) print('Defining state propagation ...') 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]])) @@ -582,9 +585,10 @@ def generate_code(): i_new = i ib_new = ib w_new = w + ptz_new = ptz # 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]) + state_new = Matrix([q_new, v_new, p_new, d_ang_b_new, d_vel_b_new, i_new, ib_new, w_new, ptz_new]) print('Computing state propagation jacobian ...') A = state_new.jacobian(state) @@ -595,8 +599,8 @@ def generate_code(): 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): + for index in range(25): + for j in range(25): if index > j: P_new[index,j] = 0