From 3c6790f5d5ccad8d3e9ff1617e354e9db650b8d1 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 22 Jun 2020 13:15:32 +0200 Subject: [PATCH] GPS Yaw: Always reset Yaw when GPS Yaw fusion is starting Also avoid fusing fake mag data when an other source of yaw aiding is active, even if in MAG_TYPE_NONE mode. --- EKF/control.cpp | 10 ++-------- EKF/ekf.h | 3 ++- EKF/ekf_helper.cpp | 9 +++++++++ EKF/gps_yaw_fusion.cpp | 13 ++----------- EKF/mag_control.cpp | 20 +++++++++++--------- test/test_EKF_gps_yaw.cpp | 32 +++++++++++++++++++------------- 6 files changed, 45 insertions(+), 42 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 4e8c4b3d578b..a23ffe241477 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -522,20 +522,14 @@ void Ekf::controlGpsFusion() if ((_params.fusion_mode & MASK_USE_GPSYAW) && ISFINITE(_gps_sample_delayed.yaw) && _control_status.flags.tilt_align - && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) + && !_control_status.flags.gps_yaw && !_gps_hgt_intermittent) { if (resetGpsAntYaw()) { // flag the yaw as aligned _control_status.flags.yaw_align = true; - // turn on fusion of external vision yaw measurements and disable all other yaw fusion - _control_status.flags.gps_yaw = true; - _control_status.flags.ev_yaw = false; - _control_status.flags.mag_dec = false; - - stopMagHdgFusion(); - stopMag3DFusion(); + startGpsYawFusion(); ECL_INFO_TIMESTAMPED("starting GPS yaw fusion"); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 5e5182034768..bc4b6ba71ee7 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -693,7 +693,7 @@ class Ekf : public EstimatorInterface void controlMagFusion(); void updateMagFilter(); - bool canRunMagFusion() const; + bool noOtherYawAidingThanMag() const; void checkHaglYawResetReq(); float getTerrainVPos() const; @@ -842,6 +842,7 @@ class Ekf : public EstimatorInterface void stopGpsVelFusion(); + void startGpsYawFusion(); void stopGpsYawFusion(); void stopEvFusion(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a37e4e76fe03..3684fcee33aa 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1634,6 +1634,15 @@ void Ekf::stopGpsVelFusion() _gps_vel_test_ratio.setZero(); } +void Ekf::startGpsYawFusion() +{ + _control_status.flags.mag_dec = false; + stopEvYawFusion(); + stopMagHdgFusion(); + stopMag3DFusion(); + _control_status.flags.gps_yaw = true; +} + void Ekf::stopGpsYawFusion() { _control_status.flags.gps_yaw = false; diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 176569312db2..ca713ccdb68c 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -303,20 +303,11 @@ bool Ekf::resetGpsAntYaw() return false; } - const float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0)); - // get measurement and correct for antenna array yaw offset const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset; - // calculate the amount the yaw needs to be rotated by - const float yaw_delta = wrap_pi(measured_yaw - predicted_yaw); - - // update the quaternion state estimates and corresponding covariances only if - // the change in angle has been large or the yaw is not yet aligned - if(fabsf(yaw_delta) > math::radians(15.0f) || !_control_status.flags.yaw_align) { - const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance, true); - } + const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + resetQuatStateYaw(measured_yaw, yaw_variance, true); return true; } diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index af750677fca3..b6230768b318 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -46,12 +46,15 @@ void Ekf::controlMagFusion() return; } - // When operating without a magnetometer, yaw fusion is run selectively to prevent - // enable yaw gyro bias learning hen stationary on ground and to prevent uncontrolled - // yaw variance growth + // When operating without a magnetometer and no other source of yaw aiding is active, + // yaw fusion is run selectively to enable yaw gyro bias learning when stationary on + // ground and to prevent uncontrolled yaw variance growth if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { - _yaw_use_inhibit = true; - fuseHeading(); + if (noOtherYawAidingThanMag()) + { + _yaw_use_inhibit = true; + fuseHeading(); + } return; } @@ -70,7 +73,7 @@ void Ekf::controlMagFusion() return; } - if (canRunMagFusion()) { + if (noOtherYawAidingThanMag() && _mag_data_ready) { if (_control_status.flags.in_air) { checkHaglYawResetReq(); runInAirYawReset(); @@ -116,11 +119,10 @@ void Ekf::updateMagFilter() } } -bool Ekf::canRunMagFusion() const +bool Ekf::noOtherYawAidingThanMag() const { - // check for new magnetometer data that has fallen behind the fusion time horizon // If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used - return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready; + return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw; } void Ekf::checkHaglYawResetReq() diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index 86b227873cf2..b44073e03d5b 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -88,26 +88,32 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset) _sensor_simulator.runSeconds(11); // THEN: after a while the fusion should be stopped - // TODO: THIS IS NOT HAPPENING EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); } -TEST_F(EkfGpsHeadingTest, fusionStartWithoutReset) +TEST_F(EkfGpsHeadingTest, yawConvergence) { - // GIVEN:EKF that fuses GPS - - // WHEN: enabling GPS heading fusion and heading difference is smaller than 15 degrees - const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f); + // GIVEN: an initial GPS yaw, not aligned with the current one + float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f); _sensor_simulator._gps.setYaw(gps_heading); + + // WHEN: the GPS yaw fusion is activated _ekf_wrapper.enableGpsHeadingFusion(); - const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); - _sensor_simulator.runSeconds(0.2); + _sensor_simulator.runSeconds(5); - // THEN: GPS heading fusion should have started; - EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + // THEN: the estimate is reset and stays close to the measurement + float yaw_est = _ekf_wrapper.getYawAngle(); + EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.05f)) + << "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading); - // AND: no reset to GPS heading should be performed - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + // AND WHEN: the the measurement changes + gps_heading += math::radians(2.f); + _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator.runSeconds(6); - // TODO: investigate why heading is not converging exactly to GPS heading + // THEN: the estimate slowly converges to the new measurement + // Note that the process is slow, because the gyro did not detect any motion + yaw_est = _ekf_wrapper.getYawAngle(); + EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f)) + << "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading); }