From b6b55bf6f2487fddaaf71171e3c0affa9c087e7a Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 16 Apr 2015 16:51:54 -0700 Subject: [PATCH] AP_NavEKF: integrate gravity over correct time period --- libraries/AP_NavEKF/AP_NavEKF.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5828150315..8cb0e65b28 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1112,7 +1112,6 @@ void NavEKF::UpdateStrapdownEquationsNED() float rotScaler; // scaling variable used to calculate delta quaternion from last to current time step Quaternion qUpdated; // quaternion at current time step after application of delta quaternion Quaternion deltaQuat; // quaternion from last to current time step - const Vector3f gravityNED(0, 0, GRAVITY_MSS); // NED gravity vector m/s^2 // remove sensor bias errors correctedDelAng = dAngIMU - state.gyro_bias; @@ -1162,16 +1161,26 @@ void NavEKF::UpdateStrapdownEquationsNED() state.quat.rotation_matrix(Tbn_temp); prevTnb = Tbn_temp.transposed(); + float delVelGravity1_z = GRAVITY_MSS*dtDelVel1; + float delVelGravity2_z = GRAVITY_MSS*dtDelVel2; + float delVelGravity_z = delVelGravity1_z * IMU1_weighting + delVelGravity2_z * (1.0f - IMU1_weighting); + // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded + // blended IMU calc - delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMUactual; + delVelNav = Tbn_temp*correctedDelVel12; + delVelNav.z += delVelGravity_z; + // single IMU calcs - delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMUactual; - delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMUactual; + delVelNav1 = Tbn_temp*correctedDelVel1; + delVelNav1.z += delVelGravity1_z; + + delVelNav2 = Tbn_temp*correctedDelVel2; + delVelNav2.z += delVelGravity2_z; // calculate the rate of change of velocity (used for launch detect and other functions) - velDotNED = delVelNav / dtIMUactual ; + velDotNED = delVelNav / dtIMUactual; // apply a first order lowpass filter velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;