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

EKF: Fix derivation comments for drag fusion

Browse Source
This commit is contained in:
Paul Riseborough 2021-06-15 20:26:42 +10:00 committed by Paul Riseborough
parent 58d376e2be
commit 97b5ae1e96
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/python/ekf_derivation/main.py
View File

@ -248,8 +248,12 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
# Use this nonlinear model for the prediction in the implementation only
# 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
#
# accXpred = -sign(vrel[0]) * vrel[0]*(0.5*rho*vrel[0]/BCoefX + MCoef) # predicted acceleration measured along X body axis
# accYpred = -sign(vrel[1]) * vrel[1]*(0.5*rho*vrel[1]/BCoefY + MCoef) # predicted acceleration measured along Y body axis
#
# BcoefX and BcoefY have units of Kg/m^2
# Mcoef has units of 1/s
# Use a simple viscous drag model for the linear estimator equations
# Use the the derivative from speed to acceleration averaged across the