Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug in vertical position derivative calculation

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-19 14:01:59 +11:00 committed by Andrew Tridgell
parent f6ad79688e
commit 2b13020c19
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -628,10 +628,8 @@ void NavEKF2_core::calcOutputStatesFast() {
// update vertical velocity and position states used to provide a vertical position derivative output // update vertical velocity and position states used to provide a vertical position derivative output
// using a simple complementary filter // using a simple complementary filter
float lastPosDownDerivative = posDownDerivative; float lastPosDownDerivative = posDownDerivative;
posDownDerivative += delVelNav.z; posDownDerivative = 2.0f * (outputDataNew.position.z - posDown);
float posDownDerivativeCorrection = 0.2f * (outputDataNew.position.z - posDown); posDown += (posDownDerivative + lastPosDownDerivative + 2.0f*delVelNav.z) * (imuDataNew.delVelDT*0.5f);
posDown += (posDownDerivative + lastPosDownDerivative) * (imuDataNew.delVelDT*0.5f) + posDownDerivativeCorrection * imuDataNew.delVelDT;
} }
/* /*