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

AP_NavEKF2: Don't correct for zero IMU position offset 

The IMU offset correction involves a significant number of floating point operations and most users will leave the offset parameter at zero.
This commit is contained in:
priseborough 2016-10-15 09:21:05 +11:00 committed by Andrew Tridgell
parent 1890bbdafa
commit c93c3d54f3
1 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -606,17 +606,26 @@ void NavEKF2_core::calcOutputStates()
// apply a trapezoidal integration to velocities to calculate position
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
// calculate the average angular rate across the last IMU update
// note delAngDT is prevented from being zero in readIMUData()
Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
// If the IMU accelerometer is offset from the body frame origin, then calculate corrections
// that can be added to the EKF velocity and position outputs so that they represent the velocity
// and position of the body frame origin.
// Note the * operator has been overloaded to operate as a dot product
if (!accelPosOffset.is_zero()) {
// calculate the average angular rate across the last IMU update
// note delAngDT is prevented from being zero in readIMUData()
Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
// Calculate the velocity of the body frame origin relative to the IMU in body frame
// and rotate into earth frame. Note % operator has been overloaded to perform a cross product
Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
velOffsetNED = Tbn_temp * velBodyRelIMU;
// Calculate the velocity of the body frame origin relative to the IMU in body frame
// and rotate into earth frame. Note % operator has been overloaded to perform a cross product
Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
velOffsetNED = Tbn_temp * velBodyRelIMU;
// calculate the earth frame position of the body frame origin relative to the IMU
posOffsetNED = Tbn_temp * (- accelPosOffset);
// calculate the earth frame position of the body frame origin relative to the IMU
posOffsetNED = Tbn_temp * (- accelPosOffset);
} else {
velOffsetNED.zero();
posOffsetNED.zero();
}
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
if (runUpdates) {