Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Improved Magnetometer Error Handling 

(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

235
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
yawAligned(false), // set true when heading or yaw angle has been aligned
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
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -495,7 +497,7 @@ void NavEKF::InitialiseFilterDynamic(void)
ResetVelocity();
ResetPosition();
ResetHeight();
state.body_magfield = magBias;
state.body_magfield.zero();
// set to true now that states have be initialised
statesInitialised = true;
@ -568,7 +570,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
state.wind_vel.zero();
state.body_magfield = magBias;
state.body_magfield.zero();
// set to true now we have intialised the states
statesInitialised = true;
@ -628,9 +630,6 @@ void NavEKF::UpdateFilter()
ResetPosition();
ResetHeight();
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);
prevStaticMode = staticMode;
}
@ -751,51 +750,42 @@ void NavEKF::SelectVelPosFusion()
// select fusion of magnetometer data
void NavEKF::SelectMagFusion()
{
if(!magFailed) {
// 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 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) {
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;
}
}
// determine if conditions are right to start a new fusion cycle
bool dataReady = statesInitialised && use_compass() && newDataMag;
if (dataReady)
{
fuseMagData = true;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
magUpdateCount = 0;
}
else
{
fuseMagData = false;
}
// call the function that performs fusion of magnetometer data
FuseMagnetometer();
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
if (magUpdateCount < magUpdateCountMax) {
magUpdateCount ++;
for (uint8_t i = 0; i <= 9; i++) {
states[i] += magIncrStateDelta[i];
}
}
// 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 a timeout
if (magHealth) {
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
bool dataReady = statesInitialised && use_compass() && newDataMag;
if (dataReady)
{
fuseMagData = true;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
magUpdateCount = 0;
}
else
{
fuseMagData = false;
}
// call the function that performs fusion of magnetometer data
FuseMagnetometer();
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
if (magUpdateCount < magUpdateCountMax) {
magUpdateCount ++;
for (uint8_t i = 0; i <= 9; i++) {
states[i] += magIncrStateDelta[i];
}
}
}
// 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
void NavEKF::SetFlightAndFusionModes()
{
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
uint8_t highAirSpd = (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f);
float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
uint8_t highGndSpdStage1 = (uint8_t)(gndSpdSq > 9.0f);
uint8_t highGndSpdStage2 = (uint8_t)(gndSpdSq > 36.0f);
uint8_t highGndSpdStage3 = (uint8_t)(gndSpdSq > 81.0f);
uint8_t largeHgt = (uint8_t)(fabsf(hgtMea) > 15.0f);
uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt;
// if magnetometer calibration mode 1 is selected, use a manoeuvre threshold test
// otherwise use a height and velocity test
if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
onGround = false;
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f){
manoeuvring = true;
} else {
// detect on-ground to in-air transition
// if we are already on the ground then 3 or more out of 5 criteria are required
// if we are in the air then only 2 or more are required
// this prevents rapid tansitions
if ((onGround && (inAirSum >= 3)) || (!onGround && (inAirSum >= 2))) {
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();
if (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f) highAirSpd = 1;
float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
if (fabsf(_baro.get_climb_rate()) > 0.5f) heightVarying = 1; // this will trigger during change in baro height
if (gndSpdSq > 9.0f) highGndSpdStage1 = 1; // trigger at 3 m/s GPS velocity
if (gndSpdSq > 36.0f) highGndSpdStage2 = 1; // trigger at 6 m/s GPS velocity
if (gndSpdSq > 81.0f) highGndSpdStage3 = 1; // trigger at 9 m/s GPS velocity
if (fabsf(hgtMea) > 15.0f) largeHgt = 1; // trigger if more than 15m away from initial height
if (accNavMag > 0.5f) accelerating = 1; // this will trigger due to air turbulence during flight
uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt + heightVarying + accelerating;
// 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;
} else {
}
// if is possible we are in flight, set the time this condition was last detected
if (inAirSum >= 1) {
airborneDetectTime_ms = imuSampleTime_ms;
}
// after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
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
if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) {
// perform a yaw alignment check against GPS if exiting on-ground mode
// this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround) {
alignYawGPS();
}
// If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
// we set the wind velocity to the reciprocal of the velocity vector and scale states so that the
// wind speed is equal to the 6m/s. This prevents gains being too high at the start
// of flight if launching into a headwind until the first turn when the EKF can form a wind speed
// estimate
if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
// If we aren't using an airspeed sensor we set the wind velocity to the reciprocal
// of the velocity vector and scale states so that the wind speed is equal to 3m/s. This helps prevent gains
// being too high at the start of flight if launching into a headwind until the first turn when the EKF can form
// a wind speed estimate and also corrects bad initial wind estimates due to heading errors
if (!onGround && prevOnGround && !useAirspeed()) {
setWindVelStates();
}
}
@ -2917,8 +2924,12 @@ void NavEKF::SetFlightAndFusionModes()
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
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
inhibitMagStates = ((_magCal == 2) || !use_compass() || onGround || staticMode);
// request mag calibration for both in-air and manoeuvre threshold options
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
@ -3153,11 +3164,9 @@ void NavEKF::readMagData()
// store time of last measurement update
lastMagUpdate = _ahrs->get_compass()->last_update;
// read compass data and assign to bias and uncorrected measurement
// body fixed magnetic bias is opposite sign to APM compass offsets
// we scale compass data to improve numerical conditioning
magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
// Read compass data
// We scale compass data to improve numerical conditioning
magData = _ahrs->get_compass()->get_field() * 0.001f;
// get states stored at time closest to measurement time after allowance for measurement delay
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
// 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)
{
// declare local variables required to calculate initial orientation and magnetic field
@ -3216,7 +3225,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
readMagData();
// rotate the magnetic field into NED axes
initMagNED = Tbn*(magData - magBias);
initMagNED = Tbn * magData;
// calculate heading of mag field rel to body heading
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;
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
yawAligned = true;
// calculate initial filter quaternion states
initQuat.from_euler(roll, pitch, yaw);
if (!badMag) {
// if mag healthy use declination and mag measurements to calculate yaw
yaw = magDecAng - magHeading;
yawAligned = true;
// calculate initial filter quaternion states
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
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
initMagNED = Tbn * (magData - magBias);
initMagNED = Tbn* magData;
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;
// clear bad magnetometer status
badMag = false;
} else {
initQuat.from_euler(roll, pitch, 0.0f);
yawAligned = false;
@ -3247,11 +3264,11 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
return initQuat;
}
// this function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity
// vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer.
// 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.
void NavEKF::alignYawGPS()
{
if ((sq(velNED[0]) + sq(velNED[1])) > 16.0f) {
if ((sq(velNED[0]) + sq(velNED[1])) > 25.0f) {
float roll;
float pitch;
float oldYaw;
@ -3259,21 +3276,26 @@ void NavEKF::alignYawGPS()
float yawErr;
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
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
newYaw = atan2f(velNED[1],velNED[0]);
// modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned
yawErr = fabsf(newYaw - oldYaw);
if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) {
// estimate the yaw error
yawErr = wrap_PI(newYaw - oldYaw);
// 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
state.quat.from_euler(roll, pitch, newYaw);
// the yaw angle is now aligned so update its status
yawAligned = true;
// set the velocity states
if (_fusionModeGPS < 2) {
state.velocity.x = velNED.x;
state.velocity.y = velNED.y;
}
// reinitialise the quaternion, velocity and position covariances
// reset the position and velocity states
ResetPosition();
ResetVelocity();
// reset the covariance for the quaternion, velocity and position states
// zero the matrix entries
zeroRows(P,0,9);
zeroCols(P,0,9);
@ -3291,6 +3313,10 @@ void NavEKF::alignYawGPS()
P[8][8] = P[7][7];
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;
secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms;
airborneDetectTime_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f;
velTimeout = false;
posTimeout = false;
hgtTimeout = false;
magTimeout = false;
magFailed = false;
badMag = false;
storeIndex = 0;
dtIMU = 0;
dt = 0;
@ -3427,7 +3454,7 @@ bool NavEKF::static_mode_demanded(void) const
// return true if we should use the compass
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

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 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 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
Vector31 Kfusion; // Kalman gain vector
@ -393,6 +393,8 @@ private:
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 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 varInnovVelPos; // innovation variance output for a group of measurements
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
float VtasMeas; // true airspeed measurement (m/s)
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 covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed