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

AP_NavEKF3: Enable use of yaw fusion before external yaw sensor starts

Browse Source
This commit is contained in:
Paul Riseborough 2020-04-08 20:07:22 +10:00 committed by Andrew Tridgell
parent 83ad1c17a8
commit ebb8bb4f6f
3 changed files with 107 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -273,11 +273,29 @@ void NavEKF3_core::SelectMagFusion()
alignYawAngle();
} else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(false, true);
} else {
fuseEulerYaw(true, true);
}
have_fused_gps_yaw = true;
last_gps_yaw_fusion_ms = now;
} else if (tiltAlignComplete && !yawAlignComplete && (imuSampleTime_ms > lastSynthYawTime_ms + 140)) {
yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
if (onGroundNotMoving) {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw(false, true);
} else {
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
// A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
yawAngDataDelayed.yawAng = atan2f(prevTnb[0][1], prevTnb[0][0]);
yawAngDataDelayed.type = 2;
} else if (yawAngDataDelayed.type == 1) {
// A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
yawAngDataDelayed.yawAng = atan2f(-prevTnb[0][1], prevTnb[1][1]);
yawAngDataDelayed.type = 1;
}
// prevent uncontrolled yaw variance growth by fusing a zero innovation
fuseEulerYaw(true, true);
}
lastSynthYawTime_ms = imuSampleTime_ms;
}
if (magcal == MagCal::EXTERNAL_YAW) {
// no fallback
@ -381,7 +399,7 @@ void NavEKF3_core::SelectMagFusion()
// from becoming badly conditioned. For planes we only do this on-ground because they can align the yaw from GPS when
// airborne. For other platform types we do this all the time.
if (!use_compass()) {
if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms > lastSynthYawTime_ms + 140)) {
fuseEulerYaw(true, false);
magTestRatio.zero();
yawTestRatio = 0.0f;

103
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -393,9 +393,8 @@ void NavEKF3_core::readIMUData()
// update the inactive bias states
learnInactiveBiases();
if (!yawAlignComplete && effective_magCal() == MagCal::EXTERNAL_YAW_FALLBACK && !inFlight) {
updateZGyroBias();
}
// run movement check using IMU data
updateMovementCheck();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(accel_index_active);
@ -1054,35 +1053,77 @@ float NavEKF3_core::MagDeclination(void) const
}
/*
update earth frame yaw gyro bias. This is only used for
MagCal::EXTERNAL_YAW_FALLBACK, when not flying and when we have not
yet done a yaw alignment. It prevents a buildup of a large gyro bias
for gyro axes that can't be corrected using the accelerometers while
waiting for GPS yaw
*/
void NavEKF3_core::updateZGyroBias(void)
Update the on ground and not moving check.
Should be called once per IMU update.
Only updates when on ground and when operating with an external yaw sensor
*/
void NavEKF3_core::updateMovementCheck(void)
{
const float zgyro_learn_limit = radians(3);
const AP_InertialSensor &ins = AP::ins();
/*
get corrected gyro
*/
Vector3f gyro = ins.get_gyro(gyro_index_active) - (stateStruct.gyro_bias / dtEkfAvg);
/*
we want to push the corrected gyro towards zero, but only in the
Z earth axis. Rotate the gyro to earth frame, then zero x,y
components then rotate back
*/
Matrix3f rot;
stateStruct.quat.rotation_matrix(rot);
gyro = rot.transposed() * gyro;
gyro.x = 0;
gyro.y = 0;
gyro = rot * gyro;
if (gyro.length_squared() > sq(zgyro_learn_limit)) {
// more than 3 degrees/second rotation, don't update. We
// assume the vehicle really is moving
MagCal magcal = effective_magCal();
if (!(magcal == MagCal::EXTERNAL_YAW || magcal == MagCal::EXTERNAL_YAW_FALLBACK) && !onGround) {
onGroundNotMoving = false;
return;
}
stateStruct.gyro_bias += gyro * 1.0e-4 * dtEkfAvg;
const float gyro_limit = radians(3.0f);
const float gyro_diff_limit = 0.1;;
const float accel_limit = 1.0f;
const float accel_diff_limit = 1.0f;
const float hysteresis_ratio = 0.7f;
const float dtEkfAvgInv = 1.0f / dtEkfAvg;
// get current bias corrected gyro and accelerometer
const AP_InertialSensor &ins = AP::ins();
Vector3f gyro = ins.get_gyro(gyro_index_active) - stateStruct.gyro_bias * dtEkfAvgInv;
Vector3f accel = ins.get_accel(accel_index_active) - stateStruct.accel_bias * dtEkfAvgInv;
if (!prevOnGround) {
gyro_prev = gyro;
accel_prev = accel;
onGroundNotMoving = false;
gyro_diff = gyro_diff_limit;
accel_diff = accel_diff_limit;
return;
}
// calculate a gyro rate of change metric
Vector3f temp = (gyro - gyro_prev) * dtEkfAvgInv;
gyro_prev = gyro;
gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();
// calculate a acceleration rate of change metric
temp = (accel - accel_prev) * dtEkfAvgInv;
accel_prev = accel;
accel_diff = 0.99f * accel_diff + 0.01f * temp.length();
const float gyro_length = gyro.length();
const float accel_length_error = accel.length() - GRAVITY_MSS;
if (onGroundNotMoving) {
if (gyro_length > gyro_limit ||
fabsf(accel_length_error) > accel_limit ||
gyro_diff > gyro_diff_limit ||
accel_diff > accel_diff_limit) {
onGroundNotMoving = false;
}
} else if (gyro.length() < hysteresis_ratio * gyro_limit &&
fabsf(accel_length_error) < hysteresis_ratio * accel_limit &&
gyro_diff < hysteresis_ratio * gyro_diff_limit &&
accel_diff < hysteresis_ratio * accel_diff_limit) {
onGroundNotMoving = true;
}
if (imuSampleTime_ms - lastMoveCheckLogTime_ms > 100) {
lastMoveCheckLogTime_ms = imuSampleTime_ms;
AP::logger().Write("XKFM",
"TimeUS,OGNM,GL,ALE,GD,AD",
"s-----",
"F-----",
"QBffff",
AP_HAL::micros64(),
uint8_t(onGroundNotMoving),
float(gyro_length),
float(accel_length_error),
float(gyro_diff),
float(accel_diff));
}
}

18
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1239,6 +1239,14 @@ private:
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
// Used by on ground movement check required when operating on ground without a yaw reference
float gyro_diff; // filtered gyro difference (rad/s)
float accel_diff; // filtered acceerometer difference (m/s/s)
Vector3f gyro_prev; // gyro vector from previous time step (rad/s)
Vector3f accel_prev; // accelerometer vector from previous time step (m/s/s)
bool onGroundNotMoving; // true when on the ground and not moving
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {
bool bad_xmag:1;
@ -1340,8 +1348,10 @@ private:
uint8_t gps_yaw_fallback_good_counter;
/*
learn Z gyro bias when not flying and when no yaw alignment has
been done with EXTERNAL_YAW_FALLBACK
*/
void updateZGyroBias(void);
Update the on ground and not moving check.
Should be called once per IMU update.
Only updates when on ground and when operating with an external yaw sensor
*/
void updateMovementCheck(void);
};