Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 17:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix bug in position reset logic

Browse Source
This commit is contained in:
priseborough 2014-04-23 17:37:07 +10:00 committed by Andrew Tridgell
parent b0fd94f18e
commit ecc8e45eda
2 changed files with 21 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -401,8 +401,8 @@ void NavEKF::ResetPosition(void)
// read the GPS // read the GPS
readGpsData(); readGpsData();
// write to state vector // write to state vector
states[7] = posNE[0]; states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x;
states[8] = posNE[1]; states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y;
} }
} }
@ -1600,7 +1600,8 @@ void NavEKF::FuseVelPosNED()
// form the observation vector and zero velocity and horizontal position observations if in static mode // form the observation vector and zero velocity and horizontal position observations if in static mode
if (!staticMode) { if (!staticMode) {
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; observation[3] = gpsPosNE.x + gpsPosGlitchOffsetNE.x;
observation[4] = gpsPosNE.y + gpsPosGlitchOffsetNE.y;
} else { } else {
for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f; for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
} }
@ -1662,14 +1663,13 @@ void NavEKF::FuseVelPosNED()
posHealth = true; posHealth = true;
posFailTime = hal.scheduler->millis(); posFailTime = hal.scheduler->millis();
// if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
// offset is decayed to zero at 1.0 m/s and limited to a maximum value of 100m before it is applied to
// subsequent GPS measurements so we don't have to do any limiting here
if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
posnOffsetNorth += posInnov[0]; gpsPosGlitchOffsetNE.x += posInnov[0];
posnOffsetEast += posInnov[1]; gpsPosGlitchOffsetNE.y += posInnov[1];
// apply the offset to the current GPS measurement // limit the radius of the offset to 100m and decay the offset to zero radially
posNE[0] += posInnov[0]; decayGpsOffset();
posNE[1] += posInnov[1]; // reset the position to the current GPS position which will include the glitch correction offset
ResetPosition();
// don't fuse data on this time step // don't fuse data on this time step
fusePosData = false; fusePosData = false;
} }
@ -3003,12 +3003,9 @@ void NavEKF::readGpsData()
// read latitutde and longitude from GPS and convert to NE position // read latitutde and longitude from GPS and convert to NE position
const struct Location &gpsloc = _ahrs->get_gps().location(); const struct Location &gpsloc = _ahrs->get_gps().location();
Vector2f posdiff = location_diff(_ahrs->get_home(), gpsloc); gpsPosNE = location_diff(_ahrs->get_home(), gpsloc);
// apply a position offset which is used to compensate for GPS jumps // decay and limit the position offset which is applied to NE position wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
// after decaying offset to allow GPS position jumps to be accommodated gradually
decayGpsOffset(); decayGpsOffset();
posNE[0] = posdiff.x + posnOffsetNorth;
posNE[1] = posdiff.y + posnOffsetEast;
} }
} }
@ -3244,8 +3241,7 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f
magVar.y = sqrtf(magTestRatio.y); magVar.y = sqrtf(magTestRatio.y);
magVar.z = sqrtf(magTestRatio.z); magVar.z = sqrtf(magTestRatio.z);
tasVar = sqrtf(tasTestRatio); tasVar = sqrtf(tasTestRatio);
offset.x = posnOffsetNorth; offset = gpsPosGlitchOffsetNE;
offset.y = posnOffsetEast;
} }
// zero stored variables - this needs to be called before a full filter initialisation // zero stored variables - this needs to be called before a full filter initialisation
@ -3254,8 +3250,6 @@ void NavEKF::ZeroVariables()
velTimeout = false; velTimeout = false;
posTimeout = false; posTimeout = false;
hgtTimeout = false; hgtTimeout = false;
posnOffsetNorth = 0;
posnOffsetEast = 0;
lastStateStoreTime_ms = 0; lastStateStoreTime_ms = 0;
lastFixTime_ms = 0; lastFixTime_ms = 0;
secondLastFixTime_ms = 0; secondLastFixTime_ms = 0;
@ -3284,13 +3278,14 @@ void NavEKF::ZeroVariables()
summedDelAng.zero(); summedDelAng.zero();
summedDelVel.zero(); summedDelVel.zero();
velNED.zero(); velNED.zero();
gpsPosGlitchOffsetNE.zero();
gpsPosNE.zero();
prevTnb.zero(); prevTnb.zero();
memset(&P[0][0], 0, sizeof(P)); memset(&P[0][0], 0, sizeof(P));
memset(&nextP[0][0], 0, sizeof(nextP)); memset(&nextP[0][0], 0, sizeof(nextP));
memset(&processNoise[0], 0, sizeof(processNoise)); memset(&processNoise[0], 0, sizeof(processNoise));
memset(&storedStates[0], 0, sizeof(storedStates)); memset(&storedStates[0], 0, sizeof(storedStates));
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
memset(&posNE[0], 0, sizeof(posNE));
} }
// return true if we should use the airspeed sensor // return true if we should use the airspeed sensor
@ -3318,17 +3313,18 @@ bool NavEKF::use_compass(void) const
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s // decay GPS horizontal position offset to close to zero at a rate of 1 m/s
// limit radius to a maximum of 100m // limit radius to a maximum of 100m
// apply glitch offset to GPS measurements
void NavEKF::decayGpsOffset() void NavEKF::decayGpsOffset()
{ {
float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms); float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms);
lastDecayTime_ms = hal.scheduler->millis(); lastDecayTime_ms = hal.scheduler->millis();
float offsetRadius = pythagorous2(posnOffsetNorth, posnOffsetEast); float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
// decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero) // decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero)
if (offsetRadius > (lapsedTime + 0.1f)) { if (offsetRadius > (lapsedTime + 0.1f)) {
// calculate scale factor to be applied to both offset components // calculate scale factor to be applied to both offset components
float scaleFactor = constrain_float((offsetRadius - lapsedTime), 0.0f, 100.0f) / offsetRadius; float scaleFactor = constrain_float((offsetRadius - lapsedTime), 0.0f, 100.0f) / offsetRadius;
posnOffsetNorth *= scaleFactor; gpsPosGlitchOffsetNE.x *= scaleFactor;
posnOffsetEast *= scaleFactor; gpsPosGlitchOffsetNE.y *= scaleFactor;
} }
} }

5
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -371,7 +371,7 @@ private:
bool fusePosData; // this boolean causes the posNE measurements to be fused bool fusePosData; // this boolean causes the posNE measurements to be fused
bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
Vector3f velNED; // North, East, Down velocity measurements (m/s) Vector3f velNED; // North, East, Down velocity measurements (m/s)
Vector2 posNE; // North, East position measurements (m) Vector2f gpsPosNE; // North, East position measurements (m)
ftype hgtMea; // height measurement relative to reference point (m) ftype hgtMea; // height measurement relative to reference point (m)
state_elements statesAtVelTime; // States at the effective time of velNED measurements state_elements statesAtVelTime; // States at the effective time of velNED measurements
state_elements statesAtPosTime; // States at the effective time of posNE measurements state_elements statesAtPosTime; // States at the effective time of posNE measurements
@ -437,8 +437,7 @@ private:
Vector8 SPP; // intermediate variables used to calculate predicted covariance matrix Vector8 SPP; // intermediate variables used to calculate predicted covariance matrix
float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1. float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1.
bool yawAligned; // true when the yaw angle has been aligned bool yawAligned; // true when the yaw angle has been aligned
float posnOffsetNorth; // offset applied to GPS data in the north direction to compensate for rapid changes in GPS solution Vector2f gpsPosGlitchOffsetNE; // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
float posnOffsetEast; // offset applied to GPS data in the north direction to compensate for rapid changes in GPS solution
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset 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 float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold