AP_NavEKF2: Correct velocity and position outputs for IMU offset

This can improve position hold performance where it is not practical to have the IMU located at the centroid.
Although this enables the effect of IMU position offsets to be corrected, users will still need to be instructed to place the IMU as close to the vehicle c.g. as practical as correcting for large offsets makes the velocity estimates noisy.

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -177,18 +177,20 @@ void NavEKF2_core::getWind(Vector3f &wind) const
 }
 
 
-// return NED velocity in m/s
+// return the NED velocity of the body frame origin in m/s
 //
 void NavEKF2_core::getVelNED(Vector3f &vel) const
 {
-    vel = outputDataNew.velocity;
+    // correct for the IMU position offset (EKF calculations are at the IMU)
+    vel = outputDataNew.velocity + velOffsetNED;
 }
 
-// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
+// Return the rate of change of vertical position in the down diection (dPosD/dt) of the body frame origin in m/s
 float NavEKF2_core::getPosDownDerivative(void) const
 {
-    // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration
-    return posDownDerivative;
+    // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
+    // correct for the IMU offset (EKF calculations are at the IMU)
+    return posDownDerivative + velOffsetNED.z;
 }
 
 // This returns the specific forces in the NED frame
@@ -206,16 +208,18 @@ void NavEKF2_core::getAccelZBias(float &zbias) const {
     }
 }
 
-// Write the last estimated NE position relative to the reference point (m).
+// Write the last estimated NE position of the body frame origin relative to the reference point (m).
 // Return true if the estimate is valid
 bool NavEKF2_core::getPosNE(Vector2f &posNE) const
 {
     // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
     if (PV_AidingMode != AID_NONE) {
         // This is the normal mode of operation where we can use the EKF position states
-        posNE.x = outputDataNew.position.x;
-        posNE.y = outputDataNew.position.y;
+        // correct for the IMU offset (EKF calculations are at the IMU)
+        posNE.x = outputDataNew.position.x + posOffsetNED.x;
+        posNE.y = outputDataNew.position.y + posOffsetNED.y;
         return true;
+
     } else {
         // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
         if(validOrigin) {
@@ -242,27 +246,28 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
     return false;
 }
 
-// Write the last calculated D position relative to the reference point (m).
-// Return true if the estimte is valid
+// Write the last calculated D position of the body frame origin relative to the reference point (m).
+// Return true if the estimate is valid
 bool NavEKF2_core::getPosD(float &posD) const
 {
     // The EKF always has a height estimate regardless of mode of operation
-    posD = outputDataNew.position.z;
+    // correct for the IMU offset (EKF calculations are at the IMU)
+    posD = outputDataNew.position.z + posOffsetNED.z;
 
     // Return the current height solution status
     return filterStatus.flags.vert_pos;
 
 }
-// return the estimated height above ground level
+// return the estimated height of body frame origin above ground level
 bool NavEKF2_core::getHAGL(float &HAGL) const
 {
-    HAGL = terrainState - outputDataNew.position.z;
+    HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
     // If we know the terrain offset and altitude, then we have a valid height above ground estimate
     return !hgtTimeout && gndOffsetValid && healthy();
 }
 
 
-// Return the last calculated latitude, longitude and height in WGS-84
+// Return the last calculated latitude, longitude and height of the body frame origin in WGS-84
 // If a calculated location isn't available, return a raw GPS measurement
 // The status will return true if a calculation or raw measurement is available
 // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
@@ -278,7 +283,8 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
         if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
             loc.lat = EKF_origin.lat;
             loc.lng = EKF_origin.lng;
-            location_offset(loc, outputDataNew.position.x, outputDataNew.position.y);
+            // correct for IMU offset (EKF calculations are at the IMU position)
+            location_offset(loc, (outputDataNew.position.x + posOffsetNED.x), (outputDataNew.position.y + posOffsetNED.y));
             return true;
         } else {
             // we could be in constant position mode  because the vehicle has taken off without GPS, or has lost GPS
@@ -291,7 +297,8 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
                 return true;
             } else {
                 // if no GPS fix, provide last known position before entering the mode
-                location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
+                // correct for IMU offset (EKF calculations are at the IMU position)
+                location_offset(loc, (lastKnownPositionNE.x + posOffsetNED.x), (lastKnownPositionNE.y + posOffsetNED.y));
                 return false;
             }
         }

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -267,6 +267,8 @@ void NavEKF2_core::InitialiseVariables()
     memset(&storedRngMeas, 0, sizeof(storedRngMeas));
     terrainHgtStable = true;
     ekfOriginHgtVar = 0.0f;
+    velOffsetNED.zero();
+    posOffsetNED.zero();
 
     // zero data buffers
     storedIMU.reset();
@@ -604,6 +606,18 @@ void NavEKF2_core::calcOutputStates()
     // apply a trapezoidal integration to velocities to calculate position
     outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
 
+    // 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);
+
+    // Calculate the velocity of the body frame origin relative to the IMU in body frame
+    // and rotate into earth frame. Note % operator has been overloaded to perform a cross product
+    Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
+    velOffsetNED = Tbn_temp * velBodyRelIMU;
+
+    // calculate the earth frame position of the body frame origin relative to the IMU
+    posOffsetNED = Tbn_temp * (- accelPosOffset);
+
     // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
     if (runUpdates) {
         // store the states at the output time horizon

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -833,6 +833,8 @@ private:
     float ekfOriginHgtVar;          // Variance of the the EKF WGS-84 origin height estimate (m^2)
     uint32_t lastOriginHgtTime_ms;  // last time the ekf's WGS-84 origin height was corrected
     Vector3f outputTrackError;      // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
+    Vector3f velOffsetNED;          // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
+    Vector3f posOffsetNED;          // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
 
     // variables used to calculate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.