fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 03:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: ev-vel refactoring (#23577)

Browse Source
This commit is contained in:
Marco Hauswirth
2024-09-24 14:09:11 +02:00
committed by GitHub
parent 21a2350eca
commit 454ded1b25
12 changed files with 674 additions and 430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+32 -19
View File
@@ -383,32 +383,45 @@ def predict_vel_body(
R_to_body = state["quat_nominal"].inverse()
return R_to_body * vel
def compute_ev_body_vel_hx(
def compute_body_vel_innov_var_h(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.V3,
) -> (sf.V3, VTangent, VTangent, VTangent):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)
Hx = jacobian_chain_rule(meas_pred[0], state)
return (Hx.T)
Hy = jacobian_chain_rule(meas_pred[1], state)
Hz = jacobian_chain_rule(meas_pred[2], state)
innov_var = sf.V3((Hx * P * Hx.T + R[0])[0,0],
(Hy * P * Hy.T + R[1])[0,0],
(Hz * P * Hz.T + R[2])[0,0])
def compute_ev_body_vel_hy(
return (innov_var, Hx.T, Hy.T, Hz.T)
def compute_body_vel_y_innov_var(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.Scalar
) -> (sf.Scalar):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)[1]
Hy = jacobian_chain_rule(meas_pred, state)
return (Hy.T)
meas_pred = predict_vel_body(state)
Hy = jacobian_chain_rule(meas_pred[1], state)
innov_var = (Hy * P * Hy.T + R)[0,0]
def compute_ev_body_vel_hz(
return (innov_var)
def compute_body_vel_z_innov_var(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.Scalar
) -> (sf.Scalar):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)[2]
Hz = jacobian_chain_rule(meas_pred, state)
return (Hz.T)
meas_pred = predict_vel_body(state)
Hz = jacobian_chain_rule(meas_pred[2], state)
innov_var = (Hz * P * Hz.T + R)[0,0]
return (innov_var)
def predict_mag_body(state) -> sf.V3:
mag_field_earth = state["mag_I"]
@@ -746,8 +759,8 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas
generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])
generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
generate_px4_state(State, tangent_idx)