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

ekf2: decompose drag vector into x-y components

Browse Source 
The drag force is a vector.
drag_x = drag.norm() * vx / v.norm()
This commit is contained in:
bresch
2022-12-30 17:43:30 +01:00
committed by Daniel Agar
parent 1a9c358858
commit 04f76c932e
2 changed files with 12 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py
+4 -2
View File
@@ -132,11 +132,13 @@ def run(logfile):
rho = 1.15 # air densitiy
rho15 = 1.225 # air density at 15 degC
rel_wind_speed = np.sqrt(v_body[0]**2 + v_body[1]**2 + v_body[2]**2)
# x[0]: momentum drag, scales with v
# x[1]: inverse of ballistic coefficient (X body axis), scales with v^2
# x[2]: inverse of ballistic coefficient (Y body axis), scales with v^2
predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0]**2 * np.sign(v_body[0]) * x[1]
predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1]**2 * np.sign(v_body[1]) * x[2]
predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0] * rel_wind_speed * x[1]
predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1] * rel_wind_speed * x[2]
J = lambda x: np.sum(np.power(abs(a_body[0]-predict_acc_x(x)), 2.0) + np.power(abs(a_body[1]-predict_acc_y(x)), 2.0)) # cost function