diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 84804ea21b..72c4919678 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -222,9 +222,11 @@ public: bool isYawFinalAlignComplete() const { const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); + const bool is_mag_alignment_in_flight_complete = is_using_mag && _control_status.flags.mag_aligned_in_flight && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6); + return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); } @@ -836,7 +838,9 @@ private: void controlGpsFusion(); bool shouldResetGpsFusion() const; bool hasHorizontalAidingTimedOut() const; - bool isYawFailure() const; + + bool isYawError(float error_threshold = math::radians(10.f)) const; + bool isYawFailure() const { return isYawError(math::radians(25.f)); } void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); @@ -862,6 +866,8 @@ private: void runMagAndMagDeclFusions(const Vector3f &mag); void run3DMagAndDeclFusions(const Vector3f &mag); + bool resetMagStates(); + // control fusion of range finder observations void controlRangeFinderFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 7011116fb5..366c67b697 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -356,22 +356,22 @@ bool Ekf::realignYawGPS(bool force) } // check for excessive horizontal GPS velocity innovations - const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps; + const bool bad_vel_innov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps; // calculate GPS course over ground angle - const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); + const float gps_cog = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); // calculate course yaw angle - const float ekfCOG = atan2f(_state.vel(1), _state.vel(0)); + const float ekf_cog = atan2f(_state.vel(1), _state.vel(0)); // Check the EKF and GPS course over ground for consistency - const float courseYawError = wrap_pi(gpsCOG - ekfCOG); + const float course_yaw_error = wrap_pi(gps_cog - ekf_cog); // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad - const bool badYawErr = fabsf(courseYawError) > math::radians(25.f); - const bool badMagYaw = (badYawErr && badVelInnov); + const bool bad_yaw_error = fabsf(course_yaw_error) > math::radians(25.f); + const bool bad_mag_yaw = (bad_yaw_error && bad_vel_innov); - if (badMagYaw) { + if (bad_mag_yaw) { _num_bad_flight_yaw_events++; _warning_events.flags.bad_yaw_using_gps_course = true; @@ -389,7 +389,9 @@ bool Ekf::realignYawGPS(bool force) } // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned - if (badMagYaw || !_control_status.flags.yaw_align || force) { + if (bad_mag_yaw || !_control_status.flags.yaw_align || force) { + + ECL_INFO("realign yaw GPS resetting yaw"); // calculate new yaw estimate float yaw_new; @@ -398,7 +400,7 @@ bool Ekf::realignYawGPS(bool force) // This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error const float current_yaw = getEulerYaw(_R_to_earth); - yaw_new = current_yaw + courseYawError; + yaw_new = current_yaw + course_yaw_error; _control_status.flags.mag_aligned_in_flight = true; } else if (_control_status.flags.wind) { @@ -409,7 +411,7 @@ bool Ekf::realignYawGPS(bool force) } else { // we don't have wind estimates, so align yaw to the GPS velocity vector - yaw_new = gpsCOG; + yaw_new = gps_cog; } // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment @@ -419,37 +421,20 @@ bool Ekf::realignYawGPS(bool force) // Apply updated yaw and yaw variance to states and covariances resetQuatStateYaw(yaw_new, yaw_variance_new, true); - - // Use the last magnetometer measurements to reset the field states - _state.mag_B.zero(); - _state.mag_I = _R_to_earth * _mag_lpf.getState(); - - resetMagCov(); - - // record the start time for the magnetic field alignment - _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _control_status.flags.yaw_align = true; // If heading was bad, then we also need to reset the velocity and position states - if (badMagYaw) { + if (bad_mag_yaw && _control_status.flags.gps) { resetVelocityToGps(_gps_sample_delayed); resetHorizontalPositionToGps(_gps_sample_delayed); } - return true; - - } else { - // align mag states only - - // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * _mag_lpf.getState(); - - resetMagCov(); - - // record the start time for the magnetic field alignment - _flt_mag_align_start_time = _imu_sample_delayed.time_us; + resetMagStates(); return true; } + + return false; } // Reset heading and magnetic field states @@ -472,6 +457,8 @@ bool Ekf::resetMagHeading() if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) { + ECL_INFO("reset mag heading resetting yaw"); + const Vector3f mag_init = _mag_lpf.getState(); // rotate the magnetometer measurements into earth frame using a zero yaw angle @@ -1523,6 +1510,9 @@ void Ekf::saveMagCovData() void Ekf::loadMagCovData() { + P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); + P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); + // re-instate variances for the XYZ body axis field P(19, 19) = _saved_mag_bf_variance(0); P(20, 20) = _saved_mag_bf_variance(1); @@ -1556,6 +1546,48 @@ void Ekf::stopAirspeedFusion() void Ekf::startGpsFusion() { if (!_control_status.flags.gps) { + + bool yaw_reset_needed = false; + + if (_control_status.flags.ev_yaw) { + // Stop the vision for yaw fusion and do not allow it to start again + stopEvYawFusion(); + + yaw_reset_needed = true; + } + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + if (_mag_inhibit_yaw_reset_req || _mag_yaw_reset_req) { + yaw_reset_needed = true; + } + } + + if (isYawEmergencyEstimateAvailable() && isYawError(math::radians(10.f))) { + yaw_reset_needed = true; + } + + // Do not use external vision for yaw if using GPS because yaw needs to be + // defined relative to an NED reference frame + if (yaw_reset_needed) { + if (resetYawToEKFGSF()) { + ECL_INFO("starting GPS, reset yaw using yaw estimator"); + + } else if (resetYawToGps()) { + ECL_INFO("starting GPS, reset yaw to GPS"); + + } else if (realignYawGPS(true)) { + ECL_INFO("starting GPS, reset yaw using GPS course"); + + } else if (resetMagHeading()) { + ECL_INFO("starting GPS, reset yaw using mag"); + + } else { + // all failed + ECL_WARN("starting GPS, yaw reset failed"); + _mag_yaw_reset_req = true; + } + } + resetHorizontalPositionToGps(_gps_sample_delayed); // when already using another velocity source velocity reset is not necessary @@ -1744,30 +1776,25 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) // Returns true if the reset was successful bool Ekf::resetYawToEKFGSF() { - if (!isYawEmergencyEstimateAvailable()) { + // The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time + // to improve its estimate if the previous reset was not successful. + if (!isYawEmergencyEstimateAvailable() + && isTimedOut(_ekfgsf_yaw_reset_time, 5000000) + && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) { + return false; } resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true); - // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight - _flt_mag_align_start_time = _imu_sample_delayed.time_us; _control_status.flags.yaw_align = true; - - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around - // and cause another navigation failure - _control_status.flags.mag_fault = true; - _warning_events.flags.emergency_yaw_reset_mag_stopped = true; - - } else if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; - } + _information_events.flags.yaw_aligned_to_imu_gps = true; _ekfgsf_yaw_reset_time = _time_last_imu; _ekfgsf_yaw_reset_count++; + resetMagStates(); + return true; } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index bbb0a923ed..fd2c72bfd4 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -53,6 +53,15 @@ void Ekf::controlGpsFusion() controlGpsYawFusion(gps_checks_passing, gps_checks_failing); + if (!_control_status.flags.yaw_align && _control_status.flags.tilt_align + && gps_checks_passing && !gps_checks_failing + && isYawEmergencyEstimateAvailable()) { + + if (resetYawToEKFGSF()) { + ECL_INFO("Yaw aligned using IMU and GPS"); + } + } + // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently const bool mandatory_conditions_passing = _control_status.flags.tilt_align @@ -79,23 +88,33 @@ void Ekf::controlGpsFusion() if (isYawFailure() && _control_status.flags.in_air && !was_gps_signal_lost - && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit - && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) { - // The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time - // to improve its estimate if the previous reset was not successful. - if (resetYawToEKFGSF()) { - ECL_WARN("GPS emergency yaw reset"); + && resetYawToEKFGSF()) { + + ECL_WARN("GPS emergency yaw reset"); + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around + // and cause another navigation failure + _control_status.flags.mag_fault = true; + _warning_events.flags.emergency_yaw_reset_mag_stopped = true; + + } else if (_control_status.flags.gps_yaw) { + _control_status.flags.gps_yaw_fault = true; + _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; + + } else if (_control_status.flags.ev_yaw) { + stopEvYawFusion(); } } else { + _warning_events.flags.gps_fusion_timout = true; + ECL_WARN("GPS fusion timeout - resetting"); + // use GPS velocity data to check and correct yaw angle if a FW vehicle if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); } - - _warning_events.flags.gps_fusion_timout = true; - ECL_WARN("GPS fusion timeout - resetting"); } resetVelocityToGps(_gps_sample_delayed); @@ -120,29 +139,7 @@ void Ekf::controlGpsFusion() } else { if (starting_conditions_passing) { - // Do not use external vision for yaw if using GPS because yaw needs to be - // defined relative to an NED reference frame - if (_control_status.flags.ev_yaw - || _mag_inhibit_yaw_reset_req - || _mag_yaw_reset_req) { - - _mag_yaw_reset_req = true; - - // Stop the vision for yaw fusion and do not allow it to start again - stopEvYawFusion(); - - } else { - startGpsFusion(); - } - - } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) { - // If no mag is used, align using the yaw estimator (if available) - if (resetYawToEKFGSF()) { - _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO("Yaw aligned using IMU and GPS"); - resetVelocityToGps(_gps_sample_delayed); - resetHorizontalPositionToGps(_gps_sample_delayed); - } + startGpsFusion(); } } @@ -192,7 +189,7 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure); } -bool Ekf::isYawFailure() const +bool Ekf::isYawError(float error_threshold) const { if (!isYawEmergencyEstimateAvailable()) { return false; @@ -201,5 +198,5 @@ bool Ekf::isYawFailure() const const float euler_yaw = getEulerYaw(_R_to_earth); const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw()); - return fabsf(yaw_error) > math::radians(25.f); + return fabsf(yaw_error) > error_threshold; } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 709deeeec6..efc0bed755 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -206,11 +206,17 @@ bool Ekf::resetYawToGps() // GPS yaw measurement is alreday compensated for antenna offset in the driver const float measured_yaw = _gps_sample_delayed.yaw; - const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance, true); + if (PX4_ISFINITE(measured_yaw) && !_control_status.flags.gps_yaw_fault) { + const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + resetQuatStateYaw(measured_yaw, yaw_variance, true); - _time_last_gps_yaw_fuse = _time_last_imu; - _yaw_signed_test_ratio_lpf.reset(0.f); + _time_last_gps_yaw_fuse = _time_last_imu; + _yaw_signed_test_ratio_lpf.reset(0.f); - return true; + resetMagStates(); + + return true; + } + + return false; } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index bc0899e7b8..264fac9717 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -362,3 +362,63 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) } } } + +bool Ekf::resetMagStates() +{ + bool reset = false; + + // reinit mag states + const bool mag_available = (_mag_counter != 0) && isRecent(_time_last_mag, 500000); + + // if world magnetic model (inclination, declination, strength) available then use it to reset mag states + if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { + // use predicted earth field to reset states + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + _state.mag_I = mag_earth_pred; + + ECL_DEBUG("resetting mag I to [%.3f, %.3f, %.3f]", (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)); + + if (mag_available) { + const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); + _state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred); + + ECL_DEBUG("resetting mag B to [%.3f, %.3f, %.3f]", (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)); + + } else { + _state.mag_B.zero(); + } + + reset = true; + + } else if (mag_available && !magFieldStrengthDisturbed(_mag_lpf.getState())) { + // Use the last magnetometer measurements to reset the field states + + // calculate initial earth magnetic field states + _state.mag_I = _R_to_earth * _mag_lpf.getState(); + _state.mag_B.zero(); + + ECL_DEBUG("resetting mag I to [%.3f, %.3f, %.3f]", (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)); + + reset = true; + } + + if (reset) { + resetMagCov(); + + // clear any pending resets + _mag_yaw_reset_req = false; + _mag_inhibit_yaw_reset_req = false; + + if (mag_available) { + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _control_status.flags.mag_aligned_in_flight = true; + _time_last_mag_heading_fuse = _time_last_imu; + _time_last_mag_3d_fuse = _time_last_imu; + } + + return true; + } + + return false; +} diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 518f02a233..187786574c 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -106,10 +106,10 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 2 resets: 1 after IMU+GNSS yaw alignment and 1 when starting GNSS aiding + // 1 reset after IMU+GNSS yaw alignment reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(2)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); EXPECT_TRUE(_ekf->local_position_is_valid()); EXPECT_TRUE(_ekf->global_position_is_valid());