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

EKF: Extend drag fusion to include propeller momnetum drag

Browse Source
This commit is contained in:
Paul Riseborough
2021-06-09 07:58:21 +10:00
committed by Paul Riseborough
parent 91808f7c9e
commit 73d1e514d0
3 changed files with 65 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/main.py
+3 -3
View File
@@ -247,9 +247,9 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
vrel = R_to_body*Matrix([vx-wx,vy-wy,vz]) # predicted wind relative velocity
# Use this nonlinear model for the prediction in the implementation only
# It uses a ballistic coefficient for each axis
# accXpred = -0.5*rho*vrel[0]*vrel[0]*BCXinv # predicted acceleration measured along X body axis
# accYpred = -0.5*rho*vrel[1]*vrel[1]*BCYinv # predicted acceleration measured along Y body axis
# It uses a ballistic coefficient for each axis and a propeller momentum drag coefficient
# accXpred = -sign(vrel[0]) * 0.5*rho*vrel[0]*(vrel[0]/BCoefX + MCoef) # predicted acceleration measured along X body axis
# accYpred = -sign(vrel[1]) * 0.5*rho*vrel[1]*(vrel[1]/BCoefY + MCoef) # predicted acceleration measured along Y body axis
# Use a simple viscous drag model for the linear estimator equations
# Use the the derivative from speed to acceleration averaged across the