aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Improved divergence detection and reset

Browse Source 
Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.

Don't reset filterDiverged status immediately during reset

Set filterDiverged true if covariance blows up

Add fault status reporting
This commit is contained in:
priseborough 2014-05-12 17:35:22 +10:00 committed by Andrew Tridgell
parent 5d2d6e0063
commit ce8e1daa16
2 changed files with 77 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

File diff suppressed because one or more lines are too long

17
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -135,6 +135,20 @@ public:
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
/*
return the filter fault status as a bitmasked integer
0 = filter divergence detected via gyro bias growth
1 = filter divergence detected by large covariances
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
4 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion
7 = unassigned
return normalised delta gyro bias length used for divergence test
*/
void getFilterFaults(uint8_t &faults, float &deltaGyroBias) const;
static const struct AP_Param::GroupInfo var_info[];
private:
@ -358,6 +372,7 @@ private:
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
Vector3f prevDelAng; // previous delta angle use for INS coning error compensation
Vector3f lastGyroBias; // previous gyro bias vector used by filter divergence check
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
@ -451,6 +466,8 @@ private:
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
uint8_t faultStatus; // filter status masked integer
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