Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: catch covarience errors and reset filter

Browse Source 
this catches covariance values beyond a reasonable limit and resets
the filter is they happen
This commit is contained in:
Andrew Tridgell 2014-04-21 15:24:16 +10:00
parent 7e5a491f14
commit 610a930612
1 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -95,6 +95,9 @@ extern const AP_HAL::HAL& hal;
#define earthRate 0.000072921f // earth rotation rate (rad/sec) #define earthRate 0.000072921f // earth rotation rate (rad/sec)
// maximum value for any element in the covariance matrix
#define EKF_COVARIENCE_MAX 1.0e8f
// when the wind estimation first starts with no airspeed sensor, // when the wind estimation first starts with no airspeed sensor,
// assume 3m/s to start // assume 3m/s to start
#define STARTUP_WIND_SPEED 3.0f #define STARTUP_WIND_SPEED 3.0f
@ -2641,6 +2644,10 @@ bool NavEKF::getPosNED(Vector3f &pos) const
// return body axis gyro bias estimates in rad/sec // return body axis gyro bias estimates in rad/sec
void NavEKF::getGyroBias(Vector3f &gyroBias) const void NavEKF::getGyroBias(Vector3f &gyroBias) const
{ {
if (dtIMU == 0) {
gyroBias.zero();
return;
}
gyroBias.x = states[10] / dtIMU; gyroBias.x = states[10] / dtIMU;
gyroBias.y = states[11] / dtIMU; gyroBias.y = states[11] / dtIMU;
gyroBias.z = states[12] / dtIMU; gyroBias.z = states[12] / dtIMU;
@ -2650,9 +2657,14 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
void NavEKF::getAccelBias(Vector3f &accelBias) const void NavEKF::getAccelBias(Vector3f &accelBias) const
{ {
accelBias.x = IMU1_weighting; accelBias.x = IMU1_weighting;
if (dtIMU == 0) {
accelBias.y = 0;
accelBias.z = 0;
} else {
accelBias.y = states[22] / dtIMU; accelBias.y = states[22] / dtIMU;
accelBias.z = states[13] / dtIMU; accelBias.z = states[13] / dtIMU;
} }
}
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF::getWind(Vector3f &wind) const void NavEKF::getWind(Vector3f &wind) const
@ -2778,6 +2790,12 @@ void NavEKF::ForceSymmetry()
{ {
for (uint8_t j=0; j<=i-1; j++) for (uint8_t j=0; j<=i-1; j++)
{ {
if (fabsf(P[i][j]) > EKF_COVARIENCE_MAX ||
fabsf(P[j][i]) > EKF_COVARIENCE_MAX) {
// re-initialise the filter from scratch
InitialiseFilterDynamic();
return;
}
float temp = 0.5f*(P[i][j] + P[j][i]); float temp = 0.5f*(P[i][j] + P[j][i]);
P[i][j] = temp; P[i][j] = temp;
P[j][i] = temp; P[j][i] = temp;