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

AP_NavEKF: Improved magnetometer consistency checks

Browse Source 
A magnetometer axis that fails the innovation consistency check will cause
all axes not to be used. If this condition continues for 10 seconds, a
magnetometer timeout condition will be declared. When the timeout has
occurred, if it is not a fly forward vehicle, then individual channels
will be used again, but with a reduced weighting.
This commit is contained in:
priseborough 2014-04-21 16:15:17 +10:00 committed by Andrew Tridgell
parent 01538c5290
commit bd28cdbdcf
2 changed files with 35 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -329,16 +329,17 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
{
AP_Param::setup_object_defaults(this, var_info);
// Tuning parameters
_gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
_gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
_gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
_gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
_gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
_msecHgtDelay = 60; // Height measurement delay (msec)
_msecMagDelay = 40; // Magnetometer measurement delay (msec)
_msecTasDelay = 240; // Airspeed measurement delay (msec)
_gpsRetryTimeUseTAS = 20000; // GPS retry time with airspeed measurements (msec)
_gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec)
_gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec)
_hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec)
_hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec)
_magFailTimeLimit = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
_magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate
_gyroBiasNoiseScaler = 3.0f; // scale factor applied to gyro bias state process variance when on ground
_msecGpsAvg = 200; // average number of msec between GPS measurements
@ -722,6 +723,18 @@ void NavEKF::SelectMagFusion()
// check for and read new magnetometer measurements
readMagData();
// If we are using the compass and the magnetometer has been unhealthy for too long we declare it failed
if (magHealth) {
lastHealthyMagTime_ms = hal.scheduler->millis();
} else {
if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > uint32_t(_magFailTimeLimit) && use_compass()) {
magTimeout = true;
} else {
magTimeout = false;
}
}
// determine if conditions are right to start a new fusion cycle
bool dataReady = statesInitialised && use_compass() && newDataMag;
if (dataReady)
@ -2149,12 +2162,19 @@ void NavEKF::FuseMagnetometer()
innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
// calculate the innovation test ratio
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(_magInnovGate) * varInnovMag[obsIndex]);
// test the ratio before fusing data
if (magTestRatio[obsIndex] < 1.0f)
// check the last values from all componenets and set magnetometer health accordingly
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
// In this case we might as well try using the magnetometer, but with a reduced weighting
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !_ahrs->get_fly_forward() && magTimeout))
{
// correct the state vector
for (uint8_t j= 0; j<=indexLimit; j++)
{
// If we are forced to use a bad compass, we reduce the weighting by a factor of 4
if (!magHealth) {
Kfusion[j] *= 0.25f;
}
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
}
// normalise the quaternion states
@ -2724,8 +2744,8 @@ void NavEKF::OnGroundCheck()
} else {
onGround = true;
}
// force a yaw alignment if exiting onGround without a compass
if (!onGround && prevOnGround && !use_compass()) {
// force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle
if (!onGround && prevOnGround && (!use_compass() || (magTimeout && _ahrs->get_fly_forward()))) {
alignYawGPS();
}
// If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround

12
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -320,6 +320,7 @@ private:
AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec)
AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec)
AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec)
AP_Int16 _magFailTimeLimit; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground
float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
uint16_t _msecGpsAvg; // average number of msec between GPS measurements
@ -330,12 +331,14 @@ private:
// Variables
uint8_t skipCounter; // counter used to skip position and height corrections to achieve _skipRatio
bool statesInitialised; // boolean true when filter states have been initialised
bool velHealth; // boolean true if velocity measurements have failed innovation consistency check
bool posHealth; // boolean true if position measurements have failed innovation consistency check
bool hgtHealth; // boolean true if height measurements have failed innovation consistency check
bool velHealth; // boolean true if velocity measurements have passed innovation consistency check
bool posHealth; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth; // boolean true if height measurements have passed innovation consistency check
bool magHealth; // boolean true if magnetometer has passed innovation consistency check
bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
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
Vector31 Kfusion; // Kalman gain vector
Matrix22 KH; // intermediate result used for covariance updates
@ -422,6 +425,7 @@ private:
uint32_t lastStateStoreTime_ms; // time of last state vector storage
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived
uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator
Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator