mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
AP_NavEKF : Remove ineffective numerical divergence checks
This commit is contained in:
priseborough
Andrew Tridgell
parent
638d63aa90
commit
5e51c09978
@ -95,9 +95,6 @@ extern const AP_HAL::HAL& hal;
|
||||
|
||||
#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,
|
||||
// assume 3m/s to start
|
||||
#define STARTUP_WIND_SPEED 3.0f
|
||||
@ -372,9 +369,6 @@ bool NavEKF::healthy(void) const
|
||||
if (state.velocity.is_nan()) {
|
||||
return false;
|
||||
}
|
||||
if (filterDiverged || (imuSampleTime_ms - lastDivergeTime_ms < 10000)) {
|
||||
return false;
|
||||
}
|
||||
// If measurements have failed innovation consistency checks for long enough to time-out
|
||||
// and force fusion then the nav solution can be conidered to be unhealthy
|
||||
// This will only be set as a transient
|
||||
@ -596,13 +590,6 @@ void NavEKF::UpdateFilter()
|
||||
// read IMU data and convert to delta angles and velocities
|
||||
readIMUData();
|
||||
|
||||
// detect if filter has diverged and do a dynamic reset using the DCM solution
|
||||
checkDivergence();
|
||||
if (filterDiverged) {
|
||||
InitialiseFilterDynamic();
|
||||
return;
|
||||
}
|
||||
|
||||
// detect if the filter update has been delayed for too long
|
||||
if (dtIMU > 0.2f) {
|
||||
// we have stalled for too long - reset states
|
||||
@ -2973,15 +2960,6 @@ void NavEKF::ForceSymmetry()
|
||||
{
|
||||
for (uint8_t j=0; j<=i-1; j++)
|
||||
{
|
||||
if (fabsf(P[i][j]) > EKF_COVARIENCE_MAX ||
|
||||
fabsf(P[j][i]) > EKF_COVARIENCE_MAX) {
|
||||
// set the filter status as diverged and re-initialise the filter
|
||||
filterDiverged = true;
|
||||
faultStatus.diverged = true;
|
||||
lastDivergeTime_ms = imuSampleTime_ms;
|
||||
InitialiseFilterDynamic();
|
||||
return;
|
||||
}
|
||||
float temp = 0.5f*(P[i][j] + P[j][i]);
|
||||
P[i][j] = temp;
|
||||
P[j][i] = temp;
|
||||
@ -3389,7 +3367,6 @@ void NavEKF::ZeroVariables()
|
||||
velTimeout = false;
|
||||
posTimeout = false;
|
||||
hgtTimeout = false;
|
||||
filterDiverged = false;
|
||||
magTimeout = false;
|
||||
magFailed = false;
|
||||
storeIndex = 0;
|
||||
@ -3471,36 +3448,10 @@ bool NavEKF::assume_zero_sideslip(void) const
|
||||
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
|
||||
}
|
||||
|
||||
// Check for filter divergence
|
||||
void NavEKF::checkDivergence()
|
||||
{
|
||||
// If filter is diverging, then fail for 10 seconds
|
||||
// delay checking to allow bias estimate to settle after reset
|
||||
// filter divergence is detected by looking for rapid changes in gyro bias
|
||||
Vector3f tempVec = state.gyro_bias - lastGyroBias;
|
||||
float tempLength = tempVec.length();
|
||||
if (tempLength != 0.0f) {
|
||||
float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f);
|
||||
scaledDeltaGyrBiasLgth = (5e-8f / temp) * tempVec.length() / dtIMU;
|
||||
}
|
||||
bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
|
||||
lastGyroBias = state.gyro_bias;
|
||||
if (imuSampleTime_ms - lastDivergeTime_ms > 10000) {
|
||||
if (divergenceDetected) {
|
||||
filterDiverged = true;
|
||||
faultStatus.diverged = true;
|
||||
lastDivergeTime_ms = imuSampleTime_ms;
|
||||
} else {
|
||||
filterDiverged = false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
return the filter fault status as a bitmasked integer
|
||||
0 = filter divergence detected via gyro bias growth
|
||||
1 = filter divergence detected by large covariances
|
||||
0 = unassigned
|
||||
1 = unassigned
|
||||
2 = badly conditioned X magnetometer fusion
|
||||
3 = badly conditioned Y magnetometer fusion
|
||||
4 = badly conditioned Z magnetometer fusion
|
||||
@ -3509,16 +3460,13 @@ return the filter fault status as a bitmasked integer
|
||||
7 = unassigned
|
||||
return normalised delta gyro bias length used for divergence test
|
||||
*/
|
||||
void NavEKF::getFilterFaults(uint8_t &faults, float &deltaGyroBias) const
|
||||
void NavEKF::getFilterFaults(uint8_t &faults) const
|
||||
{
|
||||
faults = (faultStatus.diverged<<0 |
|
||||
faultStatus.large_covarience<<1 |
|
||||
faultStatus.bad_xmag<<2 |
|
||||
faults = (faultStatus.bad_xmag<<2 |
|
||||
faultStatus.bad_ymag<<3 |
|
||||
faultStatus.bad_zmag<<4 |
|
||||
faultStatus.bad_airspeed<<5 |
|
||||
faultStatus.bad_sideslip<<6);
|
||||
deltaGyroBias = scaledDeltaGyrBiasLgth;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -144,8 +144,8 @@ public:
|
||||
|
||||
/*
|
||||
return the filter fault status as a bitmasked integer
|
||||
0 = filter divergence detected via gyro bias growth
|
||||
1 = filter divergence detected by large covariances
|
||||
0 = unassigned
|
||||
1 = unassigned
|
||||
2 = badly conditioned X magnetometer fusion
|
||||
3 = badly conditioned Y magnetometer fusion
|
||||
4 = badly conditioned Z magnetometer fusion
|
||||
@ -154,7 +154,7 @@ public:
|
||||
7 = unassigned
|
||||
return normalised delta gyro bias length used for divergence test
|
||||
*/
|
||||
void getFilterFaults(uint8_t &faults, float &deltaGyroBias) const;
|
||||
void getFilterFaults(uint8_t &faults) const;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
@ -303,9 +303,6 @@ private:
|
||||
// this allows large GPS position jumps to be accomodated gradually
|
||||
void decayGpsOffset(void);
|
||||
|
||||
// Check for filter divergence
|
||||
void checkDivergence(void);
|
||||
|
||||
// EKF Mavlink Tuneable Parameters
|
||||
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
|
||||
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
|
||||
@ -364,7 +361,6 @@ private:
|
||||
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
|
||||
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
|
||||
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
|
||||
bool filterDiverged; // boolean true if the filter has diverged
|
||||
bool magFailed; // boolean true if the magnetometer has failed
|
||||
|
||||
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
|
||||
@ -487,15 +483,12 @@ private:
|
||||
float magUpdateCountMaxInv; // floating point inverse of magFilterCountMax
|
||||
|
||||
struct {
|
||||
bool diverged:1;
|
||||
bool large_covarience:1;
|
||||
bool bad_xmag:1;
|
||||
bool bad_ymag:1;
|
||||
bool bad_zmag:1;
|
||||
bool bad_airspeed:1;
|
||||
bool bad_sideslip:1;
|
||||
} faultStatus;
|
||||
float scaledDeltaGyrBiasLgth; // scaled delta gyro bias vector length used to test for filter divergence
|
||||
|
||||
// states held by magnetomter fusion across time steps
|
||||
// magnetometer X,Y,Z measurements are fused across three time steps
|
||||
|
||||
Loading…
Reference in New Issue
Block a user