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

AP_NavEKF2: Improve ground based magnetic anomaly protection for copter 

The toilet bowling check during early flight has been removed. This check caused problems where bad compass calibration was the cause of the toilet bowling and resetting to the compass was a bad option. The handling of simultaneous failed mag and velocity innovations is already handled outside the EKF by the failsafe.
A check for yaw errors due to a ground based magnetic anomaly has been introduced.
The logic for in-flight yaw and magnetic field resets has been cleaned up and variable names improved.
This commit is contained in:
Paul Riseborough 2016-06-10 09:19:28 +10:00 committed by Andrew Tridgell
parent 703f56908f
commit dc6836988c
6 changed files with 95 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -98,7 +98,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
bool magCalRequested =
((magCal == 0) && inFlight) || // when flying
((magCal == 1) && manoeuvring) || // when manoeuvring
((magCal == 3) && firstInflightYawInit && firstInflightMagInit) || // when initial in-air yaw and mag field reset is complete
((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
(magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
@ -116,7 +116,7 @@ void NavEKF2_core::setWindMagStateLearningMode()
P[index][index] = sq(frontend->_magNoise);
}
// request a reset of the yaw and magnetic field states if not done before
if (!magStateInitComplete || (!firstInflightMagInit && inFlight)) {
if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
magYawResetRequest = true;
}
}
@ -124,8 +124,8 @@ void NavEKF2_core::setWindMagStateLearningMode()
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if (onGround) {
firstInflightYawInit = false;
firstInflightMagInit = false;
finalInflightYawInit = false;
finalInflightMagInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
@ -304,7 +304,7 @@ void NavEKF2_core::recordYawReset()
{
yawAlignComplete = true;
if (inFlight) {
firstInflightYawInit = true;
finalInflightYawInit = true;
}
}

90
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -20,42 +20,68 @@ extern const AP_HAL::HAL& hal;
// Control reset of yaw and magnetic field states
void NavEKF2_core::controlMagYawReset()
{
// Use a quaternion division to calcualte the delta quaternion between the rotation at the current and last time
Quaternion deltaQuat = stateStruct.quat / prevQuatMagReset;
prevQuatMagReset = stateStruct.quat;
// Quaternion and delta rotation vector that are re-used for different calculations
Vector3f deltaRotVecTemp;
Quaternion deltaQuatTemp;
// convert the quaternion to a rotation vector and find its length
Vector3f deltaRotVec;
deltaQuat.to_axis_angle(deltaRotVec);
bool flightResetAllowed = false;
bool initialResetAllowed = false;
if (!finalInflightYawInit) {
// Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time
deltaQuatTemp = stateStruct.quat / prevQuatMagReset;
prevQuatMagReset = stateStruct.quat;
// check if the spin rate is OK - high spin rates can cause angular alignment errors
bool angRateOK = deltaRotVec.length() < 0.1745f;
// convert the quaternion to a rotation vector and find its length
deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
// a flight reset is allowed if we are not spinning too fast and are in flight
bool flightResetAllowed = angRateOK && inFlight;
// check if the spin rate is OK - high spin rates can cause angular alignment errors
bool angRateOK = deltaRotVecTemp.length() < 0.1745f;
initialResetAllowed = angRateOK;
flightResetAllowed = angRateOK && !onGround;
}
// check that we have reached a height where ground magnetic interference effects are insignificant
bool hgtCheckPassed = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
// and can perform a final reset of the yaw and field states
bool finalResetRequest = false;
if (flightResetAllowed && !assume_zero_sideslip()) {
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
}
// check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
// this can occur if there is severe magnetic interference on the ground
bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
// Check for bad yaw casued by ground based magnetic anomaly
bool interimResetRequest = false;
if (flightResetAllowed && !assume_zero_sideslip()) {
// check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset);
// calculate if an in flight reset is required
bool flightResetRequired = (!firstInflightYawInit && (hgtCheckPassed || toiletBowling));
// check for increasing yaw innovations
bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
// check that the yaw innovations haven't been caused by a large change in attitude
deltaQuatTemp = quatAtLastMagReset / stateStruct.quat;
deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
bool largeAngleChange = deltaRotVecTemp.length() > yawInnovIncrease;
// if yaw innovations and height have increased and we haven't rotated much
// then we are climbing away from a ground based magnetic anomaly and need to reset
interimResetRequest = hgtIncreasing && yawInnovIncreasing && !largeAngleChange;
}
// an initial reset is required if we have not yet aligned the yaw angle
bool initialResetRequired = !yawAlignComplete;
bool initialResetRequest = initialResetAllowed && !yawAlignComplete;
// a combined yaw angle and magnetic field reset can be initiated by:
magYawResetRequest = magYawResetRequest || // an external request
(initialResetRequired && angRateOK) || // we need to do an initial alignment and aren't rotating too fast
(flightResetRequired && flightResetAllowed && !assume_zero_sideslip()); // conditions are right to do the in-flight re-alignment
initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer
interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly
finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
if (magYawResetRequest || magStateResetRequest) {
// gett he eeruler angles from the current state estimate
// get the euler angles from the current state estimate
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
@ -80,9 +106,11 @@ void NavEKF2_core::controlMagYawReset()
hal.console->printf("EKF2 IMU%u initial yaw alignment complete\n",(unsigned)imu_index);
}
// send initial in-flight yaw alignment status to console
if (!firstInflightYawInit && inFlight) {
// send in-flight yaw alignment status to console
if (finalResetRequest) {
hal.console->printf("EKF2 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index);
} else if (interimResetRequest) {
hal.console->printf("EKF2 IMU%u ground mag anomaly, yaw re-aligned\n",(unsigned)imu_index);
}
// update the yaw reset completed status
@ -90,13 +118,20 @@ void NavEKF2_core::controlMagYawReset()
// clear the yaw reset request flag
magYawResetRequest = false;
// clear the complete flags if an interim reset has been performed to allow subsequent
// and final reset to occur
if (interimResetRequest) {
finalInflightYawInit = false;
finalInflightMagInit = false;
}
}
}
}
// Request an in-flight check of heading against GPS and reset if necessary
// this can only be used by vehicles that can use a zero sideslip assumption (Planes)
// this allows recovery for heading alignment errors due to compass faults
if (!firstInflightYawInit && inFlight && assume_zero_sideslip()) {
if (!finalInflightYawInit && inFlight && assume_zero_sideslip()) {
gpsYawResetRequest = true;
}
@ -1044,8 +1079,13 @@ void NavEKF2_core::recordMagReset()
{
magStateInitComplete = true;
if (inFlight) {
firstInflightMagInit = true;
finalInflightMagInit = true;
}
// take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference
// for post alignment checks
posDownAtLastMagReset = stateStruct.position.z;
quatAtLastMagReset = stateStruct.quat;
yawInnovAtLastMagReset = innovYaw;
}

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -350,7 +350,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (mag_idx == magSelectIndex && firstInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
if (mag_idx == magSelectIndex && finalInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {

9
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -184,7 +184,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f || yawTestRatio > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
@ -365,6 +365,13 @@ void NavEKF2_core::detectFlight()
posDownAtTakeoff = stateStruct.position.z;
// store the range finder measurement which will be used as a reference to detect when we have taken off
rngAtStartOfFlight = rangeDataNew.rng;
// if the magnetic field states have been set, then continue to update the vertical position
// quaternion and yaw innovation snapshots to use as a reference when we start to fly.
if (magStateInitComplete) {
posDownAtLastMagReset = stateStruct.position.z;
quatAtLastMagReset = stateStruct.quat;
yawInnovAtLastMagReset = innovYaw;
}
}
}

8
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -153,8 +153,8 @@ void NavEKF2_core::InitialiseVariables()
tasTimeout = true;
badMagYaw = false;
badIMUdata = false;
firstInflightYawInit = false;
firstInflightMagInit = false;
finalInflightYawInit = false;
finalInflightMagInit = false;
dtIMUavg = 0.0025f;
dtEkfAvg = 0.01f;
dt = 0;
@ -258,6 +258,9 @@ void NavEKF2_core::InitialiseVariables()
magStateInitComplete = false;
magYawResetRequest = false;
gpsYawResetRequest = false;
posDownAtLastMagReset = stateStruct.position.z;
yawInnovAtLastMagReset = 0.0f;
quatAtLastMagReset = stateStruct.quat;
// zero data buffers
storedIMU.reset();
@ -1387,6 +1390,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
// record the fact we have initialised the magnetic field states
recordMagReset();
} else {
// this function should not be called if there is no compass data but if is is, return the
// current attitude

15
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -715,8 +715,6 @@ 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
bool firstInflightYawInit; // true when the first post takeoff initialisation of yaw angle has been performed
bool firstInflightMagInit; // true when the first post takeoff initialisation of magnetic field states been performed
bool gpsNotAvailable; // bool true when valid GPS data is not available
bool isAiding; // true when the filter is fusing position, velocity or flow measurements
bool prevIsAiding; // isAiding from previous frame
@ -789,9 +787,6 @@ private:
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
// variables used to calculate a vertical velocity that is kinematically consistent with the verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
@ -887,6 +882,16 @@ private:
uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
float meaHgtAtTakeOff; // height measured at commencement of takeoff
// control of post takeoff magentic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed
bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {
bool bad_xmag:1;