Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: integrate gravity over correct time period

This commit is contained in:
Jonathan Challinger 2015-04-16 16:51:54 -07:00 committed by Randy Mackay
parent cb0c424da1
commit b6b55bf6f2
1 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1112,7 +1112,6 @@ void NavEKF::UpdateStrapdownEquationsNED()
float rotScaler; // scaling variable used to calculate delta quaternion from last to current time step 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 qUpdated; // quaternion at current time step after application of delta quaternion
Quaternion deltaQuat; // quaternion from last to current time step 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 // remove sensor bias errors
correctedDelAng = dAngIMU - state.gyro_bias; correctedDelAng = dAngIMU - state.gyro_bias;
@ -1162,16 +1161,26 @@ void NavEKF::UpdateStrapdownEquationsNED()
state.quat.rotation_matrix(Tbn_temp); state.quat.rotation_matrix(Tbn_temp);
prevTnb = Tbn_temp.transposed(); 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 // transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded // * and + operators have been overloaded
// blended IMU calc // blended IMU calc
delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMUactual; delVelNav = Tbn_temp*correctedDelVel12;
delVelNav.z += delVelGravity_z;
// single IMU calcs // single IMU calcs
delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMUactual; delVelNav1 = Tbn_temp*correctedDelVel1;
delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMUactual; 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) // 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 // apply a first order lowpass filter
velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f; velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;