AP_NavEKF2: Remove unused code

This method of correcting for the fusion time horizon delay was too computationally expensive for our application and did not smooth fusion noise.

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -482,72 +482,6 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
     ConstrainStates();
 }
 
-// Propagate PVA solution forward from the fusion time horizon to the current time horizon
-// using buffered IMU data
-void  NavEKF2_core::calcOutputStates() {
-
-    // initialise the store access at the fusion time horizon (it will be advanced later)
-    uint8_t imuStoreAccessIndex = fifoIndexDelayed;
-    imu_elements imuData;
-
-    // Counter used to ensure the while loop always exits
-    uint8_t watchdog = 0;
-
-    // initialise to the solution at the fusion time horizon
-    outputDataNew.quat = stateStruct.quat;
-    outputDataNew.velocity = stateStruct.velocity;
-    outputDataNew.position = stateStruct.position;
-
-    // Iterate through the buffered IMU data, using the strapdown equations to wind forward from the fusion time horizon to current time
-    // we stop iterating when we have reached the current imu Data
-    do {
-
-        // If the loop cannot exit, force exit
-        if (watchdog > IMU_BUFFER_LENGTH+2) {
-            return;
-        }
-        watchdog++;
-
-        // advance to the next index
-        imuStoreAccessIndex++;
-
-        // if we have got to the end of the array, return to the start
-        if (imuStoreAccessIndex >= IMU_BUFFER_LENGTH) {
-            imuStoreAccessIndex = 0;
-        }
-        imuData = storedIMU[imuStoreAccessIndex];
-
-        // remove gyro bias errors
-        Vector3f delAng = imuData.delAng - stateStruct.gyro_bias;
-
-        // remove Z accel bias error
-        Vector3f delVel = imuData.delVel;
-        delVel.z -= stateStruct.accel_zbias;
-
-        // convert the rotation vector to its equivalent quaternion
-        Quaternion deltaQuat;
-        deltaQuat.from_axis_angle_fast(delAng);
-
-        // update the quaternion states by rotating from the previous attitude through
-        // the delta angle rotation quaternion and normalise
-        outputDataNew.quat *= deltaQuat;
-        outputDataNew.quat.normalize();
-
-        // calculate the body to nav cosine matrix
-        Matrix3f Tbn_temp;
-        outputDataNew.quat.rotation_matrix(Tbn_temp);
-
-        // transform body delta velocities to delta velocities in the nav frame
-        // * and + operators have been overloaded
-        Vector3f delVelNav  = Tbn_temp*delVel;
-        delVelNav.z += GRAVITY_MSS*imuData.delVelDT;
-
-        // use a simple Euler integration
-        outputDataNew.position += outputDataNew.velocity*imuData.delVelDT;
-    }
-    while (imuStoreAccessIndex != fifoIndexNow);
-}
-
 /*
  * Propagate PVA solution forward from the fusion time horizon to the current time horizon
  * using simple observer which performs two functions:

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -614,10 +614,6 @@ private:
     // Calculate compass heading innovation
     float calcMagHeadingInnov();
 
-    // Propagate PVA solution forward from the fusion time horizon to the current time horizon
-    // using buffered IMU data
-    void calcOutputStates();
-
     // Propagate PVA solution forward from the fusion time horizon to the current time horizon
     // using a simple observer
     void calcOutputStatesFast();