Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Remove GPS glitch offset logic 

Correction for steps in position and velocity caused by resets following GPS glitches and other events are now handled by the control loops.
This commit is contained in:
Paul Riseborough 2015-10-29 16:06:24 +11:00 committed by Randy Mackay
parent 4fdec67546
commit 74da4d8e57
9 changed files with 80 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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

8
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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); }

10
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -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

35
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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 *
********************************************************/

28
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -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;

4
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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;
}

48
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -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<IMU_BUFFER_LENGTH; i++) {
storedOutput[i].velocity.x = stateStruct.velocity.x;
@ -36,19 +40,30 @@ void NavEKF2_core::ResetVelocity(void)
outputDataNew.velocity.y = stateStruct.velocity.y;
outputDataDelayed.velocity.x = stateStruct.velocity.x;
outputDataDelayed.velocity.y = stateStruct.velocity.y;
// Calculate the position jump due to the reset
velResetNE.x = stateStruct.velocity.x - velResetNE.x;
velResetNE.y = stateStruct.velocity.y - velResetNE.y;
// store the time of the reset
lastVelReset_ms = imuSampleTime_ms;
}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF2_core::ResetPosition(void)
{
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;
if (PV_AidingMode != AID_ABSOLUTE) {
// reset all position state history to the last known position
stateStruct.position.x = lastKnownPositionNE.x;
stateStruct.position.y = lastKnownPositionNE.y;
} else if (!gpsNotAvailable) {
// write to state vector and compensate for offset between last GPs measurement and the EKF time horizon
stateStruct.position.x = gpsDataNew.pos.x + gpsPosGlitchOffsetNE.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
stateStruct.position.y = gpsDataNew.pos.y + gpsPosGlitchOffsetNE.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
}
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
storedOutput[i].position.x = stateStruct.position.x;
@ -58,6 +73,13 @@ void NavEKF2_core::ResetPosition(void)
outputDataNew.position.y = stateStruct.position.y;
outputDataDelayed.position.x = stateStruct.position.x;
outputDataDelayed.position.y = stateStruct.position.y;
// Calculate the position jump due to the reset
posResetNE.x = stateStruct.position.x - posResetNE.x;
posResetNE.y = stateStruct.position.y - posResetNE.y;
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
}
// reset the vertical position state using the last height measurement
@ -218,11 +240,11 @@ void NavEKF2_core::FuseVelPosNED()
else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
// form the observation vector
observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
observation[0] = gpsDataDelayed.vel.x;
observation[1] = gpsDataDelayed.vel.y;
observation[2] = gpsDataDelayed.vel.z;
observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
observation[3] = gpsDataDelayed.pos.x;
observation[4] = gpsDataDelayed.pos.y;
observation[5] = -baroDataDelayed.hgt;
// calculate additional error in GPS position caused by manoeuvring
@ -304,13 +326,9 @@ void NavEKF2_core::FuseVelPosNED()
// only reset the failed time and do glitch timeout checks if we are doing full aiding
if (PV_AidingMode == AID_ABSOLUTE) {
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
// if timed out or outside the specified uncertainty radius, reset to the GPS
if (posTimeout || ((P[6][6] + P[7][7]) > 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();

6
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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)

6
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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