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

AP_NavEKF3: separate GPS yaw from ExtNav yaw

Browse Source
This commit is contained in:
Randy Mackay 2020-11-30 21:35:24 +09:00 committed by Andrew Tridgell
parent 6850b48ea1
commit 5891c6ace8
6 changed files with 74 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF/AP_NavEKF_Source.cpp
View File

@ -491,6 +491,9 @@ bool AP_NavEKF_Source::ext_nav_enabled(void) const
if (src.velz == SourceZ::EXTNAV) {
return true;
}
if (src.yaw == SourceYaw::EXTNAV) {
return true;
}
}
return false;
}

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -531,6 +531,9 @@ bool NavEKF3_core::using_external_yaw(void) const
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
}
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
}
return false;
}
@ -595,7 +598,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) {
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) &&
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
// which can make this check fail
Vector3f delAngBiasVarVec = Vector3f(P[10][10],P[11][11],P[12][12]);

52
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -176,10 +176,11 @@ void NavEKF3_core::realignYawGPS()
}
}
void NavEKF3_core::alignYawAngle()
// align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
{
// update quaternion states and covariances
resetQuatStateYawOnly(yawAngDataDelayed.yawAng, sq(MAX(yawAngDataDelayed.yawAngErr, 1.0e-2f)), yawAngDataDelayed.order);
resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2f)), yawAngData.order);
// send yaw alignment information to console
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
@ -222,7 +223,8 @@ void NavEKF3_core::SelectMagFusion()
// flight using the output from the GSF yaw estimator.
if (!use_compass() &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV) {
// because this type of reset event is not as time critical, require a continuous history of valid estimates
if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
@ -255,12 +257,12 @@ void NavEKF3_core::SelectMagFusion()
return;
}
// Handle case where we are using an external yaw sensor instead of a magnetomer
// Handle case where we are using GPS yaw sensor instead of a magnetomer
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
bool have_fused_gps_yaw = false;
if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle();
alignYawAngle(yawAngDataDelayed);
yaw_source_reset = false;
} else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(yawFusionMethod::EXTERNAL);
@ -323,6 +325,34 @@ void NavEKF3_core::SelectMagFusion()
// fall through to magnetometer fusion
}
// Handle case where we are using an external nav for yaw
const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) {
if (extNavYawDataToFuse) {
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(extNavYawAngDataDelayed);
yaw_source_reset = false;
} else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(yawFusionMethod::EXTNAV);
}
last_extnav_yaw_fusion_ms = imuSampleTime_ms;
} else if (tiltAlignComplete && !yawAlignComplete) {
// External yaw sources can take significant time to start providing yaw data so
// while waiting, fuse a 'fake' yaw observation at 7Hz to keep the filter stable
if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
// update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
if (!onGroundNotMoving) {
// prevent uncontrolled yaw variance growth by fusing a zero innovation
fuseEulerYaw(yawFusionMethod::PREDICTED);
} else {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw(yawFusionMethod::STATIC);
}
lastSynthYawTime_ms = imuSampleTime_ms;
}
}
}
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
if (magHealth) {
magTimeout = false;
@ -873,6 +903,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
default:
R_YAW = sq(frontend->_yawNoise);
break;
case yawFusionMethod::EXTNAV:
R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f));
break;
}
// determine if a 321 or 312 Euler sequence is best
@ -893,6 +927,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
// determined automatically
order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
break;
case yawFusionMethod::EXTNAV:
order = extNavYawAngDataDelayed.order;
break;
}
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
@ -1054,6 +1092,10 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
innovYaw = 0.0f;
break;
case yawFusionMethod::EXTNAV:
// both external sensor yaw and last yaw when static are stored in yawAngDataDelayed.yawAng
innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng);
break;
}
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero

13
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1028,10 +1028,12 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
// extract yaw from the attitude
float roll_rad, pitch_rad, yaw_rad;
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
// ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
const float yaw_accuracy_rad = MAX(angErr, radians(5.0f));
writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2);
yaw_elements extNavYawAngDataNew;
extNavYawAngDataNew.yawAng = yaw_rad;
extNavYawAngDataNew.yawAngErr = MAX(angErr, radians(5.0f)); // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
extNavYawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; // Euler rotation order is 321 (ZYX)
extNavYawAngDataNew.time_ms = timeStamp_ms;
storedExtNavYawAng.push(extNavYawAngDataNew);
}
}
@ -1288,7 +1290,8 @@ float NavEKF3_core::MagDeclination(void) const
void NavEKF3_core::updateMovementCheck(void)
{
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass());
const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || !use_compass());
if (!runCheck)
{
onGroundNotMoving = false;

9
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -101,9 +101,6 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
// calculate buffer size for external nav data
const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length);
// buffer size for external yaw
const uint8_t yaw_angle_buffer_length = MAX(obs_buffer_length, extnav_buffer_length);
if(!storedGPS.init(obs_buffer_length)) {
return false;
}
@ -126,7 +123,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
// initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(yaw_angle_buffer_length)) {
if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
return false;
}
// Note: the use of dual range finders potentially doubles the amount of data to be stored
@ -143,6 +140,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if (frontend->sources.ext_nav_enabled() && !storedExtNavVel.init(extnav_buffer_length)) {
return false;
}
if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {
return false;
}
@ -452,6 +452,7 @@ void NavEKF3_core::InitialiseVariablesMag()
magFieldLearned = false;
storedMag.reset();
storedYawAng.reset();
storedExtNavYawAng.reset();
needMagBodyVarReset = false;
needEarthBodyVarReset = false;
}

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -625,6 +625,7 @@ private:
GSF=2,
STATIC=3,
PREDICTED=4,
EXTNAV=5,
};
// update the navigation filter status
@ -747,8 +748,8 @@ private:
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// align the yaw angle for the quaternion states using the external yaw sensor
void alignYawAngle();
// align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
void alignYawAngle(const yaw_elements &yawAngData);
// update mag field states and associated variances using magnetomer and declination data
void resetMagFieldStates();
@ -1344,6 +1345,9 @@ private:
Vector3f extNavVelInnov; // external nav velocity innovations
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
EKF_obs_buffer_t<yaw_elements> storedExtNavYawAng; // external navigation yaw angle buffer
yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon
uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {