AP_NavEKF2: improve inertial prediction

This commit is contained in:
Jonathan Challinger 2016-05-31 14:26:34 -07:00 committed by Andrew Tridgell
parent 262c5fe56d
commit 2f709dfe86
3 changed files with 23 additions and 38 deletions

View File

@ -260,34 +260,20 @@ void NavEKF2_core::readIMUData()
// Get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// remove gyro scale factor errors
imuDataNew.delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x;
imuDataNew.delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y;
imuDataNew.delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z;
// remove sensor bias errors
imuDataNew.delAng -= stateStruct.gyro_bias * (imuDataNew.delAngDT / dtEkfAvg);
imuDataNew.delVel.z -= stateStruct.accel_zbias * (imuDataNew.delVelDT / dtEkfAvg);
// Accumulate the measurement time interval for the delta velocity and angle data
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
// Rotate quaternon atitude from previous to new and normalise.
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
Quaternion deltaQuat;
deltaQuat.rotate(imuDataNew.delAng);
imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat;
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
imuQuatDownSampleNew.normalize();
// Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle
// This prevents introduction of sculling errors due to downsampling
// Rotate the latest delta velocity into the frame of reference at the start of
// accumulate the latest delta velocity and apply it to the delta velocity accumulator
Matrix3f deltaRotMat;
deltaQuat.inverse().rotation_matrix(deltaRotMat);
imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel;
// accumulate the latest delta velocity
imuDataDownSampledNew.delVel += imuDataNew.delVel;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict++;

View File

@ -412,10 +412,6 @@ void NavEKF2_core::UpdateFilter(bool predict)
// Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
startPredictEnabled = predict;
// zero the delta quaternion used by the strapdown navigation because it is published
// and we need to return a zero rotation of the INS fails to update it
correctedDelAngQuat.initialise();
// don't run filter updates if states have not been initialised
if (!statesInitialised) {
return;
@ -481,29 +477,35 @@ void NavEKF2_core::UpdateFilter(bool predict)
*/
void NavEKF2_core::UpdateStrapdownEquationsNED()
{
// apply correction for earths rotation rate
// % * - and + operators have been overloaded
correctedDelAng = imuDataDelayed.delAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT;
// remove gyro scale factor errors
imuDataDelayed.delAng.x = imuDataDelayed.delAng.x * stateStruct.gyro_scale.x;
imuDataDelayed.delAng.y = imuDataDelayed.delAng.y * stateStruct.gyro_scale.y;
imuDataDelayed.delAng.z = imuDataDelayed.delAng.z * stateStruct.gyro_scale.z;
// convert the rotation vector to its equivalent quaternion
correctedDelAngQuat.from_axis_angle(correctedDelAng);
// remove sensor bias errors
imuDataDelayed.delAng -= stateStruct.gyro_bias * (imuDataDelayed.delAngDT / dtEkfAvg);
imuDataDelayed.delVel.z -= stateStruct.accel_zbias * (imuDataDelayed.delVelDT / dtEkfAvg);
// apply correction for earth's rotation rate
// % * - and + operators have been overloaded
imuDataDelayed.delAng -= prevTnb * earthRateNED*imuDataDelayed.delAngDT;
// update the quaternion states by rotating from the previous attitude through
// the delta angle rotation quaternion and normalise
stateStruct.quat *= correctedDelAngQuat;
stateStruct.quat.rotate(imuDataDelayed.delAng);
stateStruct.quat.normalize();
// calculate the body to nav cosine matrix
Matrix3f Tbn_temp;
stateStruct.quat.rotation_matrix(Tbn_temp);
prevTnb = Tbn_temp.transposed();
// transform body delta velocities to delta velocities in the nav frame
// use the nav frame from previous time step as the delta velocities
// have been rotated into that frame
// * and + operators have been overloaded
Vector3f delVelNav; // delta velocity vector in earth axes
delVelNav = Tbn_temp*imuDataDelayed.delVel;
delVelNav = prevTnb.mul_transpose(imuDataDelayed.delVel);
delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
// calculate the body to nav cosine matrix
stateStruct.quat.inverse().rotation_matrix(prevTnb);
// calculate the rate of change of velocity (used for launch detect and other functions)
velDotNED = delVelNav / imuDataDelayed.delVelDT;

View File

@ -651,9 +651,6 @@ private:
obs_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
obs_ring_buffer_t<range_elements> storedRange;
imu_ring_buffer_t<output_elements> storedOutput;// output state buffer
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)