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

AP_NavEKF2: Fix plane in-flight yaw reset

Browse Source 
The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected.
The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading.
The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations.
To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
This commit is contained in:
Paul Riseborough 2015-11-16 08:05:08 +11:00 committed by Andrew Tridgell
parent e8706db382
commit 9b82b2200c
5 changed files with 61 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -211,7 +211,7 @@ bool NavEKF2_core::readyToUseGPS(void) const
// return true if we should use the compass
bool NavEKF2_core::use_compass(void) const
{
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex);
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
}
/*

86
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -28,10 +28,11 @@ void NavEKF2_core::controlMagYawReset()
deltaQuat.to_axis_angle(deltaRotVec);
float deltaRot = deltaRotVec.length();
// In-Flight reset for vehicle that cannot use a zero sideslip assumption
// Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
// Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
if (inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f && deltaRot < 0.1745f) {
if (!assume_zero_sideslip() && inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f && deltaRot < 0.1745f) {
firstMagYawInit = true;
// reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
magFuseTiltInhibit_ms = imuSampleTime_ms;
@ -46,10 +47,11 @@ void NavEKF2_core::controlMagYawReset()
StoreQuatRotate(tempQuat);
}
// perform a yaw alignment check against GPS if exiting on-ground mode for fly forward type vehicle (plane)
// In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)
// this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround && assume_zero_sideslip()) {
if (assume_zero_sideslip() && inFlight && !firstMagYawInit) {
alignYawGPS();
firstMagYawInit = true;
}
// inhibit the 3-axis mag fusion from modifying the tilt states for the first few seconds after a mag field reset
@ -66,53 +68,49 @@ void NavEKF2_core::controlMagYawReset()
// vector from GPS. It is used to align the yaw angle after launch or takeoff.
void NavEKF2_core::alignYawGPS()
{
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
float roll;
float pitch;
float oldYaw;
float newYaw;
float yawErr;
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
stateStruct.quat.to_euler(roll, pitch, oldYaw);
// calculate course yaw angle
oldYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
// calculate yaw angle from GPS velocity
newYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
// 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);
float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
// calculate course yaw angle from GPS velocity
float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
// Check the yaw angles for consistency
float yawErr = max(fabsf(wrap_PI(gpsYaw - velYaw)),max(fabsf(wrap_PI(gpsYaw - eulerAngles.z)),fabsf(wrap_PI(velYaw - eulerAngles.z))));
// If the angles disagree by more than 45 degrees and GPS innovations are large, we declare the magnetic yaw as bad
badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f));
// correct yaw angle using GPS ground course compass failed or if not previously aligned
if (badMag || !yawAligned) {
// correct the yaw angle
newYaw = oldYaw + yawErr;
if (badMagYaw) {
// calculate new filter quaternion states from Euler angles
stateStruct.quat.from_euler(roll, pitch, newYaw);
// the yaw angle is now aligned so update its status
yawAligned = true;
// 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);
// velocities - we could have a big error coming out of constant position mode due to GPS lag
P[3][3] = 400.0f;
P[4][4] = P[3][3];
P[5][5] = sq(0.7f);
// positions - we could have a big error coming out of constant position mode due to GPS lag
P[6][6] = 400.0f;
P[7][7] = P[6][6];
P[8][8] = sq(5.0f);
}
// Update magnetic field states if the magnetometer is bad
if (badMag) {
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
// The correlations between attitude errors and positon and velocity errors in the covariance matrix
// are invalid becasue og the changed yaw angle, so reset the corresponding row and columns
zeroCols(P,0,2);
zeroRows(P,0,2);
// Set the initial attitude error covariances
P[1][1] = P[0][0] = sq(radians(5.0f));
P[2][2] = sq(radians(45.0f));
// reset tposition fusion timer to casue the states to be reset to the GPS on the next GPS fusion cycle
lastPosPassTime_ms = 0;
}
}
// reset the magnetometer field states - we could have got bad external interference when initialising on-ground
calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// We shoud retry the primary magnetoemter if previously switched or failed
magSelectIndex = 0;
allMagSensorsFailed = false;
}
/********************************************************

11
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -186,15 +186,22 @@ bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;

14
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -109,8 +109,9 @@ void NavEKF2_core::InitialiseVariables()
gpsNoiseScaler = 1.0f;
hgtTimeout = true;
magTimeout = false;
allMagSensorsFailed = false;
tasTimeout = true;
badMag = false;
badMagYaw = false;
badIMUdata = false;
firstMagYawInit = false;
dtIMUavg = 0.0025f;
@ -148,7 +149,6 @@ void NavEKF2_core::InitialiseVariables()
inFlight = false;
prevInFlight = false;
manoeuvring = false;
yawAligned = false;
inhibitWindStates = true;
inhibitMagStates = true;
gndOffsetValid = false;
@ -1277,10 +1277,10 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
yawAligned = true;
yawAlignComplete = true;
// calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
// otherwise use existing heading
if (!badMag) {
if (!badMagYaw) {
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
@ -1316,11 +1316,11 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
P[20][20] = P[19][19];
P[21][21] = P[19][19];
// clear bad magnetometer status
badMag = false;
// clear bad magnetic yaw status
badMagYaw = false;
} else {
initQuat.from_euler(roll, pitch, 0.0f);
yawAligned = false;
yawAlignComplete = false;
}
// return attitude quaternion

5
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -649,7 +649,7 @@ private:
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 tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badMag; // boolean true if the magnetometer is declared to be producing bad data
bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data
bool badIMUdata; // boolean true if the bad IMU data is detected
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
@ -709,6 +709,7 @@ private:
uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements
uint32_t secondLastGpsTime_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
bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix
@ -716,7 +717,6 @@ private:
Vector5 SG; // intermediate variables used to calculate predicted covariance matrix
Vector8 SQ; // intermediate variables used to calculate predicted covariance matrix
Vector23 SPP; // intermediate variables used to calculate predicted covariance matrix
bool yawAligned; // true when the yaw angle has been aligned
Vector2f lastKnownPositionNE; // last known position
uint32_t lastDecayTime_ms; // time of last decay of GPS position offset
float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
@ -727,7 +727,6 @@ private:
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 firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has 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