diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index c8ac440438..6bbc0be68c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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++; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 2c837df5e6..9c4eca4626 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 66e997c9b7..8962473d00 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -651,9 +651,6 @@ private: obs_ring_buffer_t storedTAS; // TAS data buffer obs_ring_buffer_t storedRange; imu_ring_buffer_t 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)