From 5891c6ace87febf25c37b88475e25e9f0605fc81 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 30 Nov 2020 21:35:24 +0900 Subject: [PATCH] AP_NavEKF3: separate GPS yaw from ExtNav yaw --- libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 3 ++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 6 ++- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 52 +++++++++++++++++-- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 13 +++-- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 9 ++-- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 ++- 6 files changed, 74 insertions(+), 17 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp index 6e206f245c..84b267923e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp @@ -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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 6436ae3dc7..4874f1483f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -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]); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 2392b81ced..3ff3df46e2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index fb6d6e4772..2ba1d994ba 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index e04f8fa9f9..b6c06a3be0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f8f2cd1cbd..4851d7a57f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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 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 {