From 2b13020c192ebdeb20bc6927e9afed22afefd9f5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 19 Oct 2015 14:01:59 +1100 Subject: [PATCH] AP_NavEKF2: Fix bug in vertical position derivative calculation --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 1cdeca10a4..e282bef90a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -628,10 +628,8 @@ void NavEKF2_core::calcOutputStatesFast() { // 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 * (outputDataNew.position.z - posDown); - posDown += (posDownDerivative + lastPosDownDerivative) * (imuDataNew.delVelDT*0.5f) + posDownDerivativeCorrection * imuDataNew.delVelDT; - + posDownDerivative = 2.0f * (outputDataNew.position.z - posDown); + posDown += (posDownDerivative + lastPosDownDerivative + 2.0f*delVelNav.z) * (imuDataNew.delVelDT*0.5f); } /*