AP_NavEKF2: Don't correct for zero IMU position offset

The IMU offset correction involves a significant number of floating point operations and most users will leave the offset parameter at zero.

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -606,6 +606,11 @@ void NavEKF2_core::calcOutputStates()
     // apply a trapezoidal integration to velocities to calculate position
     outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
 
+    // If the IMU accelerometer is offset from the body frame origin, then calculate corrections
+    // that can be added to the EKF velocity and position outputs so that they represent the velocity
+    // and position of the body frame origin.
+    // Note the * operator has been overloaded to operate as a dot product
+    if (!accelPosOffset.is_zero()) {
         // calculate the average angular rate across the last IMU update
         // note delAngDT is prevented from being zero in readIMUData()
         Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
@@ -617,6 +622,10 @@ void NavEKF2_core::calcOutputStates()
 
         // calculate the earth frame position of the body frame origin relative to the IMU
         posOffsetNED = Tbn_temp * (- accelPosOffset);
+    } else {
+        velOffsetNED.zero();
+        posOffsetNED.zero();
+    }
 
     // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
     if (runUpdates) {