mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: improve inertial prediction
This commit is contained in:
parent
262c5fe56d
commit
2f709dfe86
|
@ -260,34 +260,20 @@ void NavEKF2_core::readIMUData()
|
||||||
// Get current time stamp
|
// Get current time stamp
|
||||||
imuDataNew.time_ms = imuSampleTime_ms;
|
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
|
// Accumulate the measurement time interval for the delta velocity and angle data
|
||||||
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
|
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
|
||||||
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
|
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
|
||||||
|
|
||||||
// Rotate quaternon atitude from previous to new and normalise.
|
// Rotate quaternon atitude from previous to new and normalise.
|
||||||
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
|
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
|
||||||
Quaternion deltaQuat;
|
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
|
||||||
deltaQuat.rotate(imuDataNew.delAng);
|
|
||||||
imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat;
|
|
||||||
imuQuatDownSampleNew.normalize();
|
imuQuatDownSampleNew.normalize();
|
||||||
|
|
||||||
// Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle
|
// Rotate the latest delta velocity into the frame of reference at the start of
|
||||||
// This prevents introduction of sculling errors due to downsampling
|
// accumulate the latest delta velocity and apply it to the delta velocity accumulator
|
||||||
Matrix3f deltaRotMat;
|
Matrix3f deltaRotMat;
|
||||||
deltaQuat.inverse().rotation_matrix(deltaRotMat);
|
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
|
||||||
imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel;
|
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
|
||||||
|
|
||||||
// accumulate the latest delta velocity
|
|
||||||
imuDataDownSampledNew.delVel += imuDataNew.delVel;
|
|
||||||
|
|
||||||
// Keep track of the number of IMU frames since the last state prediction
|
// Keep track of the number of IMU frames since the last state prediction
|
||||||
framesSincePredict++;
|
framesSincePredict++;
|
||||||
|
|
|
@ -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
|
// 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;
|
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
|
// don't run filter updates if states have not been initialised
|
||||||
if (!statesInitialised) {
|
if (!statesInitialised) {
|
||||||
return;
|
return;
|
||||||
|
@ -481,29 +477,35 @@ void NavEKF2_core::UpdateFilter(bool predict)
|
||||||
*/
|
*/
|
||||||
void NavEKF2_core::UpdateStrapdownEquationsNED()
|
void NavEKF2_core::UpdateStrapdownEquationsNED()
|
||||||
{
|
{
|
||||||
// apply correction for earths rotation rate
|
// remove gyro scale factor errors
|
||||||
// % * - and + operators have been overloaded
|
imuDataDelayed.delAng.x = imuDataDelayed.delAng.x * stateStruct.gyro_scale.x;
|
||||||
correctedDelAng = imuDataDelayed.delAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT;
|
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
|
// remove sensor bias errors
|
||||||
correctedDelAngQuat.from_axis_angle(correctedDelAng);
|
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
|
// update the quaternion states by rotating from the previous attitude through
|
||||||
// the delta angle rotation quaternion and normalise
|
// the delta angle rotation quaternion and normalise
|
||||||
stateStruct.quat *= correctedDelAngQuat;
|
stateStruct.quat.rotate(imuDataDelayed.delAng);
|
||||||
stateStruct.quat.normalize();
|
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
|
// 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
|
// * and + operators have been overloaded
|
||||||
Vector3f delVelNav; // delta velocity vector in earth axes
|
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;
|
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)
|
// calculate the rate of change of velocity (used for launch detect and other functions)
|
||||||
velDotNED = delVelNav / imuDataDelayed.delVelDT;
|
velDotNED = delVelNav / imuDataDelayed.delVelDT;
|
||||||
|
|
||||||
|
|
|
@ -651,9 +651,6 @@ private:
|
||||||
obs_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
|
obs_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
|
||||||
obs_ring_buffer_t<range_elements> storedRange;
|
obs_ring_buffer_t<range_elements> storedRange;
|
||||||
imu_ring_buffer_t<output_elements> storedOutput;// output state buffer
|
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
|
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 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)
|
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
|
||||||
|
|
Loading…
Reference in New Issue