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

AP_NavEKF : Improvements to staticMode robustness

This commit is contained in:
Paul Riseborough 2014-02-16 22:32:17 +11:00 committed by Andrew Tridgell
parent 0cbe64bc5e
commit 6fbada26d3
2 changed files with 128 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

200
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -210,7 +210,8 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates TASmsecMax(200), // maximum allowed interval between airspeed measurement updates
fuseMeNow(false), // forces airspeed fusion to occur on the IMU frame that data arrives fuseMeNow(false), // forces airspeed fusion to occur on the IMU frame that data arrives
staticMode(true) // staticMode forces position and velocity fusion with zero values staticMode(true), // staticMode forces position and velocity fusion with zero values
prevStaticMode(true) // staticMode from previous filter update
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -288,6 +289,7 @@ void NavEKF::ResetPosition(void)
states[7] = 0; states[7] = 0;
states[8] = 0; states[8] = 0;
} else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) { } else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
// read the GPS // read the GPS
readGpsData(); readGpsData();
// write to state vector // write to state vector
@ -492,6 +494,7 @@ void NavEKF::UpdateFilter()
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
StoreStatesReset();
//Initialise IMU pre-processing states //Initialise IMU pre-processing states
readIMUData(); readIMUData();
perf_end(_perf_UpdateFilter); perf_end(_perf_UpdateFilter);
@ -508,6 +511,15 @@ void NavEKF::UpdateFilter()
staticMode = false; staticMode = false;
} }
// check to see if static mode has changed and reset states if it has
if (prevStaticMode != staticMode) {
ResetVelocity();
ResetPosition();
ResetHeight();
StoreStatesReset();
prevStaticMode = staticMode;
}
// Run the strapdown INS equations every IMU update // Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED(); UpdateStrapdownEquationsNED();
@ -545,52 +557,65 @@ void NavEKF::SelectVelPosFusion()
// Calculate ratio of VelPos fusion to state prediction setps // Calculate ratio of VelPos fusion to state prediction setps
uint8_t velPosFuseStepRatio = floor(dtVelPos/dtIMU + 0.5f); uint8_t velPosFuseStepRatio = floor(dtVelPos/dtIMU + 0.5f);
// Command fusion of GPS measurements if new ones available or in static mode // Calculate the scale factor to be applied to the measurement variance to account for
readGpsData(); // the fact we repeat fusion of the same measurement to provide a smoother output
if (newDataGps||staticMode) { gpsVarScaler = _msecGpsAvg/(1000.0f*dtVelPos);
fuseVelData = true; // Calculate the scale factor to be applied to the measurement variance to account for
fusePosData = true; // the fact we repeat fusion of the same measurement to provide a smoother output
// Calculate the scale factor to be applied to the measurement variance to account for hgtVarScaler = _msecHgtAvg/(1000.0f*dtVelPos);
// the fact we repeat fusion of the same measurement to provide a smoother output
gpsVarScaler = _msecGpsAvg/(1000.0f*dtVelPos); if (!staticMode) {
// reset the counter used to skip updates so that we always fuse data on the frame data arrives // Command fusion of GPS measurements if new ones available
skipCounter = velPosFuseStepRatio; readGpsData();
// If a long time sinc last GPS update, then reset position and velocity if (newDataGps) {
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) { // reset data arrived flag
if (!staticMode) { newDataGps = false;
ResetPosition(); // enable fusion
ResetVelocity(); fuseVelData = true;
fusePosData = true;
// reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives
skipCounter = velPosFuseStepRatio;
// If a long time since last GPS update, then reset position and velocity and reset stored state history
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) {
ResetPosition();
ResetVelocity();
StoreStatesReset();
} }
} }
}
// Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives // measurement until the next one arrives
if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) { if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
fuseVelData = false;
fusePosData = false;
}
// Command fusion of height measurements if new ones available
if (newDataHgt)
{
// reset data arrived flag
newDataHgt = false;
// enable fusion
fuseHgtData = true;
}
// Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives
readHgtData();
if (hal.scheduler->millis() > lastHgtUpdate + _msecHgtAvg + 40) {
fuseHgtData = false;
}
} else {
// we only fuse position and height in static mode
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = true;
} fusePosData = true;
// Command fusion of height measurements if new ones available or in static mode
readHgtData();
if (newDataHgt||staticMode)
{
fuseHgtData = true;
// Calculate the scale factor to be applied to the measurement variance to account for
// the fact we repeat fusion of the same measurement to provide a smoother output
hgtVarScaler = _msecHgtAvg/(1000.0f*dtVelPos);
}
// Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives
if (hal.scheduler->millis() > lastHgtUpdate + _msecHgtAvg + 40) {
fuseHgtData = false;
} }
// Perform fusion if conditions are met // Perform fusion if conditions are met
if (fuseVelData || fusePosData || fuseHgtData || staticMode) { if (fuseVelData || fusePosData || fuseHgtData) {
// Skip fusion as required to maintain ~dtVelPos time interval between corrections // Skip fusion as required to maintain ~dtVelPos time interval between corrections
if (skipCounter == velPosFuseStepRatio) { if (skipCounter >= velPosFuseStepRatio) {
FuseVelPosNED(); FuseVelPosNED();
// reset counter used to skip update frames // reset counter used to skip update frames
skipCounter = 1; skipCounter = 1;
@ -600,9 +625,6 @@ void NavEKF::SelectVelPosFusion()
} }
} }
// reset data arrived flags
newDataGps = false;
newDataHgt = false;
} }
void NavEKF::SelectMagFusion() void NavEKF::SelectMagFusion()
@ -1085,8 +1107,8 @@ void NavEKF::CovariancePrediction()
nextP[9][13] = P[9][13] + P[6][13]*dt; nextP[9][13] = P[9][13] + P[6][13]*dt;
nextP[9][14] = P[9][14] + P[6][14]*dt; nextP[9][14] = P[9][14] + P[6][14]*dt;
nextP[9][15] = P[9][15] + P[6][15]*dt; nextP[9][15] = P[9][15] + P[6][15]*dt;
nextP[9][16] = P[9][16] + P[6][16]*dt; nextP[9][16] = P[9][16] + P[6][16]*dt;
nextP[9][17] = P[9][17] + P[6][17]*dt; nextP[9][17] = P[9][17] + P[6][17]*dt;
nextP[9][18] = P[9][18] + P[6][18]*dt; nextP[9][18] = P[9][18] + P[6][18]*dt;
nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[9][19] = P[9][19] + P[6][19]*dt;
nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[9][20] = P[9][20] + P[6][20]*dt;
@ -1361,8 +1383,8 @@ void NavEKF::CovariancePrediction()
nextP[i][i] = nextP[i][i] + processNoise[i]; nextP[i][i] = nextP[i][i] + processNoise[i];
} }
// If on ground or no compasss fitted, inhibit magnetic field state covariance growth // If on ground or no compasss fitted or in static mode, inhibit magnetic field state covariance growth
if (onGround || !useCompass) if (onGround || !useCompass || staticMode)
{ {
for (uint8_t i=16; i<=21; i++) { for (uint8_t i=16; i<=21; i++) {
for (uint8_t j=0; j<=21; j++) { for (uint8_t j=0; j<=21; j++) {
@ -1372,9 +1394,9 @@ void NavEKF::CovariancePrediction()
} }
} }
// If on ground or not using airspeed sensing, inhibit wind velocity // If on ground or not using airspeed sensing or in static mode, inhibit wind velocity
// covariance growth. // covariance growth.
if (onGround || !useAirspeed) if (onGround || !useAirspeed || staticMode)
{ {
for (uint8_t i=14; i<=15; i++) { for (uint8_t i=14; i<=15; i++) {
for (uint8_t j=0; j<=21; j++) { for (uint8_t j=0; j<=21; j++) {
@ -1401,14 +1423,23 @@ void NavEKF::CovariancePrediction()
} }
// Copy to output whilst forcing symmetry to prevent ill-conditioning // Copy to output whilst forcing symmetry to prevent ill-conditioning
// of the matrix // of the matrix. If in static or on-ground modes, zero the off-diagonal
for (uint8_t i=0; i<=21; i++) P[i][i] = nextP[i][i]; // terms for state indices 14 and higher to prevent possible long term
// numerical errors during extended ground operation
bool zeroOffDiag = (onGround || staticMode);
for (uint8_t i=0; i<=21; i++) {
P[i][i] = nextP[i][i];
}
for (uint8_t i=1; i<=21; i++) for (uint8_t i=1; i<=21; i++)
{ {
for (uint8_t j=0; j<=i-1; j++) for (uint8_t j=0; j<=i-1; j++)
{ {
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); if (zeroOffDiag && ((i >= 14) || (j >= 14))) {
P[j][i] = P[i][j]; P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
} }
} }
@ -1455,12 +1486,12 @@ void NavEKF::FuseVelPosNED()
if (fuseVelData || fusePosData || fuseHgtData) if (fuseVelData || fusePosData || fuseHgtData)
{ {
// if static mode is active use the current states to perform fusion // if static mode is active use the current states to calculate the predicted
// against the static measurements. We need to do this because there may // measurement. We need to do this because there may be no stored states due
// not be measurements present to store states against // to lack of measurements.
// in static mode, only position and height fusion is used
if (staticMode) { if (staticMode) {
for (uint8_t i=4; i<=9; i++) { for (uint8_t i=7; i<=9; i++) {
statesAtVelTime[i] = states[i];
statesAtPosTime[i] = states[i]; statesAtPosTime[i] = states[i];
statesAtHgtTime[i] = states[i]; statesAtHgtTime[i] = states[i];
} }
@ -1472,12 +1503,12 @@ void NavEKF::FuseVelPosNED()
else gpsRetryTime = _gpsRetryTimeNoTAS; else gpsRetryTime = _gpsRetryTimeNoTAS;
// Form the observation vector // Form the observation vector
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[5] = -hgtMea;
// zero observations if in static mode (used for pre-arm and bench testing) // zero observations if in static mode (used for pre-arm and bench testing)
if (staticMode) { if (~staticMode) {
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[5] = -hgtMea;
} else {
for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f; for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f;
} }
@ -1511,18 +1542,17 @@ void NavEKF::FuseVelPosNED()
// apply an innovation consistency threshold test // apply an innovation consistency threshold test
velHealth = ((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]))); velHealth = ((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2])));
velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime; velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
if (velHealth || velTimeout) if (velHealth || velTimeout || staticMode)
{ {
velHealth = true; velHealth = true;
velFailTime = hal.scheduler->millis(); velFailTime = hal.scheduler->millis();
// if timed out, reset the velocity, but do not fuse data on this time step
if (velTimeout) if (velTimeout)
{ {
if (!staticMode) { ResetVelocity();
ResetVelocity(); StoreStatesReset();
}
fuseVelData = false; fuseVelData = false;
} }
} }
else else
{ {
@ -1539,15 +1569,16 @@ void NavEKF::FuseVelPosNED()
// apply an innovation consistency threshold test // apply an innovation consistency threshold test
posHealth = ((sq(posInnov[0]) + sq(posInnov[1])) < (sq(_gpsPosInnovGate) * (varInnovVelPos[3] + varInnovVelPos[4]))); posHealth = ((sq(posInnov[0]) + sq(posInnov[1])) < (sq(_gpsPosInnovGate) * (varInnovVelPos[3] + varInnovVelPos[4])));
posTimeout = (hal.scheduler->millis() - posFailTime) > gpsRetryTime; posTimeout = (hal.scheduler->millis() - posFailTime) > gpsRetryTime;
if (posHealth || posTimeout) // Fuse position data if healthy or timed out or in static mode
if (posHealth || posTimeout || staticMode)
{ {
posHealth = true; posHealth = true;
posFailTime = hal.scheduler->millis(); posFailTime = hal.scheduler->millis();
// if timed out, reset the position, but do not fuse data on this time step
if (posTimeout) if (posTimeout)
{ {
if (!staticMode) { ResetPosition();
ResetPosition(); StoreStatesReset();
}
fusePosData = false; fusePosData = false;
} }
} }
@ -1569,13 +1600,16 @@ void NavEKF::FuseVelPosNED()
// apply an innovation consistency threshold test // apply an innovation consistency threshold test
hgtHealth = (sq(hgtInnov) < (sq(_hgtInnovGate) * varInnovVelPos[5])); hgtHealth = (sq(hgtInnov) < (sq(_hgtInnovGate) * varInnovVelPos[5]));
hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime; hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime;
if (hgtHealth || hgtTimeout) // Fuse height data if healthy or timed out or in static mode
if (hgtHealth || hgtTimeout || staticMode)
{ {
hgtHealth = true; hgtHealth = true;
hgtFailTime = hal.scheduler->millis(); hgtFailTime = hal.scheduler->millis();
// if timed out, reset the height, but do not fuse data on this time step
if (hgtTimeout) if (hgtTimeout)
{ {
ResetHeight(); ResetHeight();
StoreStatesReset();
fuseHgtData = false; fuseHgtData = false;
} }
} }
@ -1586,13 +1620,13 @@ void NavEKF::FuseVelPosNED()
} }
// Set range for sequential fusion of velocity and position measurements depending // Set range for sequential fusion of velocity and position measurements depending
// on which data is available and its health // on which data is available and its health
if ((fuseVelData && _fusionModeGPS == 0 && velHealth) || staticMode) if (fuseVelData && _fusionModeGPS == 0 && velHealth && !staticMode)
{ {
fuseData[0] = true; fuseData[0] = true;
fuseData[1] = true; fuseData[1] = true;
fuseData[2] = true; fuseData[2] = true;
} }
if (fuseVelData && _fusionModeGPS == 1 && velHealth) if (fuseVelData && _fusionModeGPS == 1 && velHealth && !staticMode)
{ {
fuseData[0] = true; fuseData[0] = true;
fuseData[1] = true; fuseData[1] = true;
@ -2164,6 +2198,19 @@ void NavEKF::StoreStates()
storeIndex = storeIndex + 1; storeIndex = storeIndex + 1;
} }
// Reset the stored state history and store the current state
void NavEKF::StoreStatesReset()
{
// clear stored state history
memset(&storedStates[0][0], 0, sizeof(storedStates));
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
// store current state vector in first column
storeIndex = 0;
for (uint8_t i=0; i<=21; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = hal.scheduler->millis();
storeIndex = storeIndex + 1;
}
// Output the state vector stored at the time that best matches that specified by msec // Output the state vector stored at the time that best matches that specified by msec
void NavEKF::RecallStates(Vector22 &statesForFusion, uint32_t msec) void NavEKF::RecallStates(Vector22 &statesForFusion, uint32_t msec)
{ {
@ -2230,8 +2277,8 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
void NavEKF::getAccelBias(Vector3f &accelBias) const void NavEKF::getAccelBias(Vector3f &accelBias) const
{ {
accelBias.x = 0.0f; accelBias.x = float(staticMode);
accelBias.y = 0.0f; accelBias.y = float(staticModeDemanded);
accelBias.z = states[13] / dtIMU; accelBias.z = states[13] / dtIMU;
} }
@ -2567,6 +2614,7 @@ void NavEKF::ZeroVariables()
dtIMU = 0; dtIMU = 0;
dt = 0; dt = 0;
hgtMea = 0; hgtMea = 0;
storeIndex = 0;
prevDelAng.zero(); prevDelAng.zero();
lastAngRate.zero(); lastAngRate.zero();
lastAccel.zero(); lastAccel.zero();

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -190,6 +190,9 @@ private:
// store states along with system time stamp in msces // store states along with system time stamp in msces
void StoreStates(void); void StoreStates(void);
// Reset the stored state history and store the current state
void StoreStatesReset(void);
// recall state vector stored at closest time to the one specified by msec // recall state vector stored at closest time to the one specified by msec
void RecallStates(Vector22 &statesForFusion, uint32_t msec); void RecallStates(Vector22 &statesForFusion, uint32_t msec);
@ -361,6 +364,7 @@ private:
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
const bool fuseMeNow; // boolean to force fusion whenever data arrives const bool fuseMeNow; // boolean to force fusion whenever data arrives
bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing
bool prevStaticMode; // value of static mode from last update
uint32_t lastMagUpdate; // last time compass was updated uint32_t lastMagUpdate; // last time compass was updated
Vector3f velDotNED; // rate of change of velocity in NED frame Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED Vector3f velDotNEDfilt; // low pass filtered velDotNED