diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index e4f2203f25..a4935f124a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -817,4 +817,24 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAng) return core->getLastYawResetAngle(yawAng); } +// return the amount of NE position change due to the last position reset in metres +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &pos) +{ + if (!core) { + return false; + } + return core->getLastPosNorthEastReset(pos); +} + +// return the amount of NE velocity change due to the last velocity reset in metres/sec +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) +{ + if (!core) { + return false; + } + return core->getLastVelNorthEastReset(vel); +} + #endif //HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index fdd108c6a3..39c9e0ac24 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -227,6 +227,14 @@ public: // returns the time of the last yaw angle reset or 0 if no reset has ever occurred uint32_t getLastYawResetAngle(float &yawAng); + // return the amount of NE position change due to the last position reset in metres + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosNorthEastReset(Vector2f &pos); + + // return the amount of NE velocity change due to the last velocity reset in metres/sec + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastVelNorthEastReset(Vector2f &vel); + // allow the enable flag to be set by Replay void set_enable(bool enable) { _enable.set(enable); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 936b933105..c2d1e0e9d3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -54,8 +54,8 @@ void NavEKF2_core::FuseAirspeed() vwn = stateStruct.wind_vel.x; vwe = stateStruct.wind_vel.y; - // calculate the predicted airspeed, compensating for bias in GPS velocity when we are pulling a glitch offset back in - VtasPred = pythagorous3((ve - gpsVelGlitchOffset.y - vwe) , (vn - gpsVelGlitchOffset.x - vwn) , vd); + // calculate the predicted airspeed + VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd); // perform fusion of True Airspeed measurement if (VtasPred > 1.0f) { @@ -330,9 +330,9 @@ void NavEKF2_core::FuseSideslip() vwn = stateStruct.wind_vel.x; vwe = stateStruct.wind_vel.y; - // calculate predicted wind relative velocity in NED, compensating for offset in velcity when we are pulling a GPS glitch offset back in - vel_rel_wind.x = vn - vwn - gpsVelGlitchOffset.x; - vel_rel_wind.y = ve - vwe - gpsVelGlitchOffset.y; + // calculate predicted wind relative velocity in NED + vel_rel_wind.x = vn - vwn; + vel_rel_wind.y = ve - vwe; vel_rel_wind.z = vd; // rotate into body axes diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index acf6823175..e2d7c54070 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -531,9 +531,6 @@ void NavEKF2_core::readGpsData() } } - // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually - decayGpsOffset(); - // save measurement to buffer to be fused later StoreGPS(); @@ -584,10 +581,6 @@ void NavEKF2_core::readGpsData() // put the filter into constant position mode PV_AidingMode = AID_NONE; - // reset all glitch states - gpsPosGlitchOffsetNE.zero(); - gpsVelGlitchOffset.zero(); - // Reset the velocity and position states ResetVelocity(); ResetPosition(); @@ -654,34 +647,6 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { } -// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes -// limit radius to a maximum of 50m -void NavEKF2_core::decayGpsOffset() -{ - float offsetDecaySpd; - if (assume_zero_sideslip()) { - offsetDecaySpd = 5.0f; - } else { - offsetDecaySpd = 1.0f; - } - float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms); - lastDecayTime_ms = imuSampleTime_ms; - float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y); - // decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero) - if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) { - // Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in. - gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius; - // calculate scale factor to be applied to both offset components - float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius; - gpsPosGlitchOffsetNE.x *= scaleFactor; - gpsPosGlitchOffsetNE.y *= scaleFactor; - } else { - gpsVelGlitchOffset.zero(); - gpsPosGlitchOffsetNE.zero(); - } -} - - /******************************************************** * Height Measurements * ********************************************************/ diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 131b7c02f0..18595250b2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -183,7 +183,6 @@ void NavEKF2_core::EstimateTerrainOffset() if (fuseOptFlowData) { - Vector3f vel; // velocity of sensor relative to ground in NED axes Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes float losPred; // predicted optical flow angular rate measurement float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time @@ -193,11 +192,6 @@ void NavEKF2_core::EstimateTerrainOffset() float K_OPT; float H_OPT; - // Correct velocities for GPS glitch recovery offset - vel.x = stateStruct.velocity[0] - gpsVelGlitchOffset.x; - vel.y = stateStruct.velocity[1] - gpsVelGlitchOffset.y; - vel.z = stateStruct.velocity[2]; - // predict range to centre of image float flowRngPred = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; @@ -205,7 +199,7 @@ void NavEKF2_core::EstimateTerrainOffset() terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); // calculate relative velocity in sensor frame - relVelSensor = Tnb_flow*vel; + relVelSensor = Tnb_flow*stateStruct.velocity; // divide velocity by range, subtract body rates and apply scale factor to // get predicted sensed angular optical rates relative to X and Y sensor axes @@ -222,25 +216,25 @@ void NavEKF2_core::EstimateTerrainOffset() float t10 = q0*q3*2.0f; float t11 = q1*q2*2.0f; float t14 = t3+t4-t5-t6; - float t15 = t14*vel.x; + float t15 = t14*stateStruct.velocity.x; float t16 = t10+t11; - float t17 = t16*vel.y; + float t17 = t16*stateStruct.velocity.y; float t18 = q0*q2*2.0f; float t19 = q1*q3*2.0f; float t20 = t18-t19; - float t21 = t20*vel.z; + float t21 = t20*stateStruct.velocity.z; float t2 = t15+t17-t21; float t7 = t3-t4-t5+t6; float t8 = stateStruct.position[2]-terrainState; float t9 = 1.0f/sq(t8); float t24 = t3-t4+t5-t6; - float t25 = t24*vel.y; + float t25 = t24*stateStruct.velocity.y; float t26 = t10-t11; - float t27 = t26*vel.x; + float t27 = t26*stateStruct.velocity.x; float t28 = q0*q1*2.0f; float t29 = q2*q3*2.0f; float t30 = t28+t29; - float t31 = t30*vel.z; + float t31 = t30*stateStruct.velocity.z; float t12 = t25-t27+t31; float t13 = sq(t7); float t22 = sq(t2); @@ -288,7 +282,6 @@ void NavEKF2_core::EstimateTerrainOffset() void NavEKF2_core::FuseOptFlow() { Vector24 H_LOS; - Vector3f velNED_local; Vector3f relVelSensor; Vector14 SH_LOS; Vector2 losPred; @@ -303,11 +296,6 @@ void NavEKF2_core::FuseOptFlow() float vd = stateStruct.velocity.z; float pd = stateStruct.position.z; - // Correct velocities for GPS glitch recovery offset - velNED_local.x = vn - gpsVelGlitchOffset.x; - velNED_local.y = ve - gpsVelGlitchOffset.y; - velNED_local.z = vd; - // constrain height above ground to be above range measured on ground float heightAboveGndEst = max((terrainState - pd), rngOnGnd); float ptd = pd + heightAboveGndEst; @@ -334,7 +322,7 @@ void NavEKF2_core::FuseOptFlow() float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f); // calculate relative velocity in sensor frame - relVelSensor = Tnb_flow*velNED_local; + relVelSensor = Tnb_flow*stateStruct.velocity; // divide velocity by range to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 5fb72d8ef9..bff7490775 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -337,7 +337,7 @@ void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // this indicates the amount of margin available when tuning the various error traps -// also return the current offsets applied to the GPS position measurements +// also return the delta in position due to the last position reset void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { velVar = sqrtf(velTestRatio); @@ -347,7 +347,7 @@ void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve magVar.y = sqrtf(magTestRatio.y); magVar.z = sqrtf(magTestRatio.z); tasVar = sqrtf(tasTestRatio); - offset = gpsPosGlitchOffsetNE; + offset = posResetNE; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 1530039859..3ac3ae3c0e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -21,12 +21,16 @@ extern const AP_HAL::HAL& hal; // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift void NavEKF2_core::ResetVelocity(void) { + // Store the position before the reset so that we can record the reset delta + velResetNE.x = stateStruct.velocity.x; + velResetNE.y = stateStruct.velocity.y; + if (PV_AidingMode != AID_ABSOLUTE) { stateStruct.velocity.zero(); } else if (!gpsNotAvailable) { - // reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero. - stateStruct.velocity.x = gpsDataNew.vel.x + gpsVelGlitchOffset.x; // north velocity from blended accel data - stateStruct.velocity.y = gpsDataNew.vel.y + gpsVelGlitchOffset.y; // east velocity from blended accel data + // reset horizontal velocity states to the GPS velocity + stateStruct.velocity.x = gpsDataNew.vel.x; // north velocity from blended accel data + stateStruct.velocity.y = gpsDataNew.vel.y; // east velocity from blended accel data } for (uint8_t i=0; i sq(float(frontend._gpsGlitchRadiusMax)))) { - gpsPosGlitchOffsetNE.x += innovVelPos[3]; - gpsPosGlitchOffsetNE.y += innovVelPos[4]; - // limit the radius of the offset and decay the offset to zero radially - decayGpsOffset(); - // reset the position to the current GPS position which will include the glitch correction offset + // reset the position to the current GPS position ResetPosition(); // reset the velocity to the GPS velocity ResetVelocity(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index da2639d2cf..616b3b8c40 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -91,6 +91,8 @@ void NavEKF2_core::InitialiseVariables() timeTasReceived_ms = 0; magYawResetTimer_ms = imuSampleTime_ms; lastPreAlignGpsCheckTime_ms = imuSampleTime_ms; + lastPosReset_ms = 0; + lastVelReset_ms = 0; // initialise other variables gpsNoiseScaler = 1.0f; @@ -106,7 +108,6 @@ void NavEKF2_core::InitialiseVariables() velDotNEDfilt.zero(); summedDelAng.zero(); summedDelVel.zero(); - gpsPosGlitchOffsetNE.zero(); lastKnownPositionNE.zero(); prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); @@ -127,7 +128,6 @@ void NavEKF2_core::InitialiseVariables() PV_AidingMode = AID_NONE; posTimeout = true; velTimeout = true; - gpsVelGlitchOffset.zero(); isAiding = false; prevIsAiding = false; memset(&faultStatus, 0, sizeof(faultStatus)); @@ -195,6 +195,8 @@ void NavEKF2_core::InitialiseVariables() airSpdFusionDelayed = false; sideSlipFusionDelayed = false; magFuseTiltInhibit = false; + posResetNE.zero(); + velResetNE.zero(); } // Initialise the states from accelerometer and magnetometer data (if present) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 3ec75a5687..de2b44c5f2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -531,10 +531,6 @@ private: // return true if the vehicle code has requested the filter to be ready for flight bool readyToUseGPS(void) const; - // decay GPS horizontal position offset to close to zero at a rate of 1 m/s - // this allows large GPS position jumps to be accomodated gradually - void decayGpsOffset(void); - // Check for filter divergence void checkDivergence(void); @@ -708,7 +704,6 @@ private: Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix bool yawAligned; // true when the yaw angle has been aligned - Vector2f gpsPosGlitchOffsetNE; // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution Vector2f lastKnownPositionNE; // last known position uint32_t lastDecayTime_ms; // time of last decay of GPS position offset float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold @@ -720,7 +715,6 @@ private: bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has been performed - Vector2f gpsVelGlitchOffset; // Offset applied to the GPS velocity when the gltch radius is being decayed back to zero bool gpsNotAvailable; // bool true when valid GPS data is not available bool isAiding; // true when the filter is fusing position, velocity or flow measurements bool prevIsAiding; // isAiding from previous frame