diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 57677191bc..1a9b878f38 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/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; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index a85c4b35f0..2e2543c296 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index d256dc2281..145ee5c43e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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.