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

AP_NavEKF : Improved Magnetometer Error Handling

Browse Source 
(Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector.
Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight.
For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
This commit is contained in:
priseborough 2014-10-30 18:27:35 +11:00 committed by Andrew Tridgell
parent a55db1c25d
commit 5359da9c68
2 changed files with 134 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

163
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -325,7 +325,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
prevStaticMode(true), // staticMode from previous filter update prevStaticMode(true), // staticMode from previous filter update
yawAligned(false), // set true when heading or yaw angle has been aligned yawAligned(false), // set true when heading or yaw angle has been aligned
inhibitWindStates(true), // inhibit wind state updates on startup inhibitWindStates(true), // inhibit wind state updates on startup
inhibitMagStates(true) // inhibit magnetometer state updates on startup inhibitMagStates(true), // inhibit magnetometer state updates on startup
onGround(true), // initialise assuming we are on ground
manoeuvring(false) // initialise assuming we are not maneouvring
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -495,7 +497,7 @@ void NavEKF::InitialiseFilterDynamic(void)
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
state.body_magfield = magBias; state.body_magfield.zero();
// set to true now that states have be initialised // set to true now that states have be initialised
statesInitialised = true; statesInitialised = true;
@ -568,7 +570,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
state.accel_zbias1 = 0; state.accel_zbias1 = 0;
state.accel_zbias2 = 0; state.accel_zbias2 = 0;
state.wind_vel.zero(); state.wind_vel.zero();
state.body_magfield = magBias; state.body_magfield.zero();
// set to true now we have intialised the states // set to true now we have intialised the states
statesInitialised = true; statesInitialised = true;
@ -628,9 +630,6 @@ void NavEKF::UpdateFilter()
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
StoreStatesReset(); StoreStatesReset();
// clear the magnetometer failed status as the failure may have been
// caused by external field disturbances associated with pre-flight activities
magFailed = false;
calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
prevStaticMode = staticMode; prevStaticMode = staticMode;
} }
@ -751,23 +750,15 @@ void NavEKF::SelectVelPosFusion()
// select fusion of magnetometer data // select fusion of magnetometer data
void NavEKF::SelectMagFusion() void NavEKF::SelectMagFusion()
{ {
if(!magFailed) {
// check for and read new magnetometer measurements // check for and read new magnetometer measurements
readMagData(); readMagData();
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
// If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset
if (magHealth) { if (magHealth) {
lastHealthyMagTime_ms = imuSampleTime_ms;
} else {
if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
magTimeout = true;
if (assume_zero_sideslip()) {
magFailed = true;
}
} else {
magTimeout = false; magTimeout = false;
} lastHealthyMagTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
magTimeout = true;
} }
// determine if conditions are right to start a new fusion cycle // determine if conditions are right to start a new fusion cycle
@ -795,7 +786,6 @@ void NavEKF::SelectMagFusion()
} }
} }
}
} }
// select fusion of true airspeed measurements // select fusion of true airspeed measurements
@ -2878,38 +2868,55 @@ bool NavEKF::getLLH(struct Location &loc) const
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void NavEKF::SetFlightAndFusionModes() void NavEKF::SetFlightAndFusionModes()
{ {
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f){
manoeuvring = true;
} else {
manoeuvring = false;
}
// if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
if (assume_zero_sideslip()) {
// Evaluate a numerical score that defines the likelihood we are in the air
uint8_t highAirSpd = 0;
uint8_t heightVarying = 0;
uint8_t highGndSpdStage1 = 0;
uint8_t highGndSpdStage2 = 0;
uint8_t highGndSpdStage3 = 0;
uint8_t largeHgt = 0;
uint8_t accelerating = 0;
const AP_Airspeed *airspeed = _ahrs->get_airspeed(); const AP_Airspeed *airspeed = _ahrs->get_airspeed();
uint8_t highAirSpd = (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f); if (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f) highAirSpd = 1;
float gndSpdSq = sq(velNED[0]) + sq(velNED[1]); float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
uint8_t highGndSpdStage1 = (uint8_t)(gndSpdSq > 9.0f); if (fabsf(_baro.get_climb_rate()) > 0.5f) heightVarying = 1; // this will trigger during change in baro height
uint8_t highGndSpdStage2 = (uint8_t)(gndSpdSq > 36.0f); if (gndSpdSq > 9.0f) highGndSpdStage1 = 1; // trigger at 3 m/s GPS velocity
uint8_t highGndSpdStage3 = (uint8_t)(gndSpdSq > 81.0f); if (gndSpdSq > 36.0f) highGndSpdStage2 = 1; // trigger at 6 m/s GPS velocity
uint8_t largeHgt = (uint8_t)(fabsf(hgtMea) > 15.0f); if (gndSpdSq > 81.0f) highGndSpdStage3 = 1; // trigger at 9 m/s GPS velocity
uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt; if (fabsf(hgtMea) > 15.0f) largeHgt = 1; // trigger if more than 15m away from initial height
// if magnetometer calibration mode 1 is selected, use a manoeuvre threshold test if (accNavMag > 0.5f) accelerating = 1; // this will trigger due to air turbulence during flight
// otherwise use a height and velocity test uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt + heightVarying + accelerating;
if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) { // if we on-ground then 4 or more out of 7 criteria are required to transition to the
// in-air mode and we also need enough GPS velocity to be able to calculate a reliable ground track heading
if (onGround && (inAirSum >= 4) && highGndSpdStage2) {
onGround = false; onGround = false;
} else { }
// detect on-ground to in-air transition // if is possible we are in flight, set the time this condition was last detected
// if we are already on the ground then 3 or more out of 5 criteria are required if (inAirSum >= 1) {
// if we are in the air then only 2 or more are required airborneDetectTime_ms = imuSampleTime_ms;
// this prevents rapid tansitions }
if ((onGround && (inAirSum >= 3)) || (!onGround && (inAirSum >= 2))) { // after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
onGround = false; if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
} else {
onGround = true; onGround = true;
} }
// force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle // perform a yaw alignment check against GPS if exiting on-ground mode
if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) { // this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround) {
alignYawGPS(); alignYawGPS();
} }
// If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround // If we aren't using an airspeed sensor we set the wind velocity to the reciprocal
// we set the wind velocity to the reciprocal of the velocity vector and scale states so that the // of the velocity vector and scale states so that the wind speed is equal to 3m/s. This helps prevent gains
// wind speed is equal to the 6m/s. This prevents gains being too high at the start // being too high at the start of flight if launching into a headwind until the first turn when the EKF can form
// of flight if launching into a headwind until the first turn when the EKF can form a wind speed // a wind speed estimate and also corrects bad initial wind estimates due to heading errors
// estimate if (!onGround && prevOnGround && !useAirspeed()) {
if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
setWindVelStates(); setWindVelStates();
} }
} }
@ -2917,8 +2924,12 @@ void NavEKF::SetFlightAndFusionModes()
prevOnGround = onGround; prevOnGround = onGround;
// If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states // If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode); inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode);
// If magnetometer calibration mode is turned off by the user or we are on ground or in static mode, then inhibit magnetometer states // request mag calibration for both in-air and manoeuvre threshold options
inhibitMagStates = ((_magCal == 2) || !use_compass() || onGround || staticMode); bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring);
// deny mag calibration request if we aren't using the compass, are in the pre-arm static mode or it has been inhibited by the user
bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2));
// inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
} }
// initialise the covariance matrix // initialise the covariance matrix
@ -3153,11 +3164,9 @@ void NavEKF::readMagData()
// store time of last measurement update // store time of last measurement update
lastMagUpdate = _ahrs->get_compass()->last_update; lastMagUpdate = _ahrs->get_compass()->last_update;
// read compass data and assign to bias and uncorrected measurement // Read compass data
// body fixed magnetic bias is opposite sign to APM compass offsets // We scale compass data to improve numerical conditioning
// we scale compass data to improve numerical conditioning magData = _ahrs->get_compass()->get_field() * 0.001f;
magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
// get states stored at time closest to measurement time after allowance for measurement delay // get states stored at time closest to measurement time after allowance for measurement delay
RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay)); RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay));
@ -3199,7 +3208,7 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
// initialise the earth magnetic field states using declination, suppled roll/pitch // initialise the earth magnetic field states using declination, suppled roll/pitch
// and magnetometer measurements and return initial attitude quaternion // and magnetometer measurements and return initial attitude quaternion
// if no magnetometer data, do not update amgentic field states and assume zero yaw angle // if no magnetometer data, do not update magnetic field states and assume zero yaw angle
Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch) Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
{ {
// declare local variables required to calculate initial orientation and magnetic field // declare local variables required to calculate initial orientation and magnetic field
@ -3216,7 +3225,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
readMagData(); readMagData();
// rotate the magnetic field into NED axes // rotate the magnetic field into NED axes
initMagNED = Tbn*(magData - magBias); initMagNED = Tbn * magData;
// calculate heading of mag field rel to body heading // calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x); float magHeading = atan2f(initMagNED.y, initMagNED.x);
@ -3225,19 +3234,27 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// calculate yaw angle rel to true north // calculate yaw angle rel to true north
if (!badMag) {
// if mag healthy use declination and mag measurements to calculate yaw
yaw = magDecAng - magHeading; yaw = magDecAng - magHeading;
yawAligned = true; yawAligned = true;
// calculate initial filter quaternion states // calculate initial filter quaternion states
initQuat.from_euler(roll, pitch, yaw); initQuat.from_euler(roll, pitch, yaw);
} else {
// if mag failed keep current yaw
initQuat = state.quat;
}
// calculate initial Tbn matrix and rotate Mag measurements into NED // calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states // to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn); initQuat.rotation_matrix(Tbn);
initMagNED = Tbn * (magData - magBias); initMagNED = Tbn* magData;
// write to earth magnetic field state vector // write to earth magnetic field state vector
state.earth_magfield = initMagNED; state.earth_magfield = initMagNED;
// clear bad magnetometer status
badMag = false;
} else { } else {
initQuat.from_euler(roll, pitch, 0.0f); initQuat.from_euler(roll, pitch, 0.0f);
yawAligned = false; yawAligned = false;
@ -3247,11 +3264,11 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
return initQuat; return initQuat;
} }
// this function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity // this function is used to do a forced alignment of the yaw angle to align with the horizontal velocity
// vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer. // vector from GPS. It is used to align the yaw angle after launch or takeoff.
void NavEKF::alignYawGPS() void NavEKF::alignYawGPS()
{ {
if ((sq(velNED[0]) + sq(velNED[1])) > 16.0f) { if ((sq(velNED[0]) + sq(velNED[1])) > 25.0f) {
float roll; float roll;
float pitch; float pitch;
float oldYaw; float oldYaw;
@ -3259,21 +3276,26 @@ void NavEKF::alignYawGPS()
float yawErr; float yawErr;
// get quaternion from existing filter states and calculate roll, pitch and yaw angles // get quaternion from existing filter states and calculate roll, pitch and yaw angles
state.quat.to_euler(roll, pitch, oldYaw); state.quat.to_euler(roll, pitch, oldYaw);
// calculate course yaw angle
oldYaw = atan2f(state.velocity.y,state.velocity.x);
// calculate yaw angle from GPS velocity // calculate yaw angle from GPS velocity
newYaw = atan2f(velNED[1],velNED[0]); newYaw = atan2f(velNED[1],velNED[0]);
// modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned // estimate the yaw error
yawErr = fabsf(newYaw - oldYaw); yawErr = wrap_PI(newYaw - oldYaw);
if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) { // If the inertial course angle disagrees with the GPS by more than 45 degrees, we declare the compass as bad
badMag = (fabsf(yawErr) > 0.7854f);
// correct yaw angle using GPS ground course compass failed or if not previously aligned
if (badMag || !yawAligned) {
// correct the yaw angle
newYaw = oldYaw + yawErr;
// calculate new filter quaternion states from Euler angles // calculate new filter quaternion states from Euler angles
state.quat.from_euler(roll, pitch, newYaw); state.quat.from_euler(roll, pitch, newYaw);
// the yaw angle is now aligned so update its status // the yaw angle is now aligned so update its status
yawAligned = true; yawAligned = true;
// set the velocity states // reset the position and velocity states
if (_fusionModeGPS < 2) { ResetPosition();
state.velocity.x = velNED.x; ResetVelocity();
state.velocity.y = velNED.y; // reset the covariance for the quaternion, velocity and position states
}
// reinitialise the quaternion, velocity and position covariances
// zero the matrix entries // zero the matrix entries
zeroRows(P,0,9); zeroRows(P,0,9);
zeroCols(P,0,9); zeroCols(P,0,9);
@ -3291,6 +3313,10 @@ void NavEKF::alignYawGPS()
P[8][8] = P[7][7]; P[8][8] = P[7][7];
P[9][9] = sq(5.0f); P[9][9] = sq(5.0f);
} }
// Update magnetic field states if the magnetometer is bad
if (badMag) {
calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
}
} }
} }
@ -3372,13 +3398,14 @@ void NavEKF::ZeroVariables()
lastFixTime_ms = imuSampleTime_ms; lastFixTime_ms = imuSampleTime_ms;
secondLastFixTime_ms = imuSampleTime_ms; secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms; lastDecayTime_ms = imuSampleTime_ms;
airborneDetectTime_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f; gpsNoiseScaler = 1.0f;
velTimeout = false; velTimeout = false;
posTimeout = false; posTimeout = false;
hgtTimeout = false; hgtTimeout = false;
magTimeout = false; magTimeout = false;
magFailed = false; badMag = false;
storeIndex = 0; storeIndex = 0;
dtIMU = 0; dtIMU = 0;
dt = 0; dt = 0;
@ -3427,7 +3454,7 @@ bool NavEKF::static_mode_demanded(void) const
// return true if we should use the compass // return true if we should use the compass
bool NavEKF::use_compass(void) const bool NavEKF::use_compass(void) const
{ {
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw() && !magFailed; return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
} }
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s // decay GPS horizontal position offset to close to zero at a rate of 1 m/s

5
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -364,7 +364,7 @@ private:
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 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 magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
bool magFailed; // boolean true if the magnetometer has failed bool badMag; // boolean true if the magnetometer is declared to be producing bad data
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector31 Kfusion; // Kalman gain vector Vector31 Kfusion; // Kalman gain vector
@ -393,6 +393,8 @@ private:
ftype hgtRate; // state for rate of change of height filter ftype hgtRate; // state for rate of change of height filter
bool onGround; // boolean true when the flight vehicle is on the ground (not flying) bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
bool prevOnGround; // value of onGround from previous update bool prevOnGround; // value of onGround from previous update
bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
uint32_t airborneDetectTime_ms; // last time flight movement was detected
Vector6 innovVelPos; // innovation output for a group of measurements Vector6 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements
bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fuseVelData; // this boolean causes the velNED measurements to be fused
@ -414,7 +416,6 @@ private:
bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
state_elements statesAtVtasMeasTime; // filter states at the effective measurement time state_elements statesAtVtasMeasTime; // filter states at the effective measurement time
Vector3f magBias; // magnetometer bias vector in XYZ body axes
const ftype covTimeStepMax; // maximum time allowed between covariance predictions const ftype covTimeStepMax; // maximum time allowed between covariance predictions
const ftype covDelAngMax; // maximum delta angle between covariance predictions const ftype covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool covPredStep; // boolean set to true when a covariance prediction step has been performed