AP_NavEKF2: improve inertial prediction

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;
+    imuQuatDownSampleNew.rotate(imuDataNew.delAng);
-    deltaQuat.rotate(imuDataNew.delAng);
-    imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat;
     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;
-    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
     framesSincePredict++;

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
+    // 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
     // 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;
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -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)