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

ekf2-drag: do not generate Kalman gain to save flash

Browse Source
This commit is contained in:
bresch
2024-01-31 09:46:07 +01:00
committed by Mathieu Bresciani
parent 1efb08375a
commit 17d55dddd6
6 changed files with 355 additions and 492 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+6 -8
View File
@@ -567,7 +567,7 @@ def predict_drag(
return bluff_body_drag + momentum_drag
def compute_drag_x_innov_var_and_k(
def compute_drag_x_innov_var_and_h(
state: VState,
P: MTangent,
rho: sf.Scalar,
@@ -581,11 +581,10 @@ def compute_drag_x_innov_var_and_k(
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
Hx = sf.V1(meas_pred[0]).jacobian(state)
innov_var = (Hx * P * Hx.T + R)[0,0]
K = P * Hx.T / sf.Max(innov_var, epsilon)
return (innov_var, K)
return (innov_var, Hx.T)
def compute_drag_y_innov_var_and_k(
def compute_drag_y_innov_var_and_h(
state: VState,
P: MTangent,
rho: sf.Scalar,
@@ -599,9 +598,8 @@ def compute_drag_y_innov_var_and_k(
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
Hy = sf.V1(meas_pred[1]).jacobian(state)
innov_var = (Hy * P * Hy.T + R)[0,0]
K = P * Hy.T / sf.Max(innov_var, epsilon)
return (innov_var, K)
return (innov_var, Hy.T)
def predict_gravity_direction(state: State):
# get transform from earth to body frame
@@ -674,8 +672,8 @@ if not args.disable_mag:
if not args.disable_wind:
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])