Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix bug in vertical position derivative calculation

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

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1234,13 +1234,6 @@ void NavEKF::UpdateStrapdownEquationsNED()
state.vel1 += delVelNav1;
state.vel2 += delVelNav2;
// update vertical velocity and position states used to provide a vertical position derivative output
// using a simple complementary filter
float lastPosDownDerivative = posDownDerivative;
posDownDerivative += delVelNav.z;
float posDownDerivativeCorrection = 0.2f * (state.position.z - posDown);
posDown += (posDownDerivative + lastPosDownDerivative) * (dtIMUactual*0.5f) + posDownDerivativeCorrection * dtIMUactual;
// apply a trapezoidal integration to velocities to calculate position
state.position += (state.velocity + lastVelocity) * (dtIMUactual*0.5f);
state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMUactual*0.5f);
@ -1261,6 +1254,12 @@ void NavEKF::UpdateStrapdownEquationsNED()
// limit states to protect against divergence
ConstrainStates();
// update vertical velocity and position states used to provide a vertical position derivative output
// using a simple complementary filter
float lastPosDownDerivative = posDownDerivative;
posDownDerivative = 2.0f * (state.position.z - posDown);
posDown += (posDownDerivative + lastPosDownDerivative + 2.0f*delVelNav.z) * (dtIMUactual*0.5f);
}
// calculate the predicted state covariance matrix