From e90e1c7e2a25d26e0331af603654fdba70b306fa Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Aug 2021 14:22:03 +0200 Subject: [PATCH] GNSS yaw: use dedicated observation noise --- src/modules/ekf2/EKF/common.h | 3 +++ src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a70bc9bc8c..d2e40a2cfa 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -276,6 +276,9 @@ struct parameters { float mag_yaw_rate_gate{0.25f}; ///< yaw rate threshold used by mode select logic (rad/sec) const float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this + // GNSS heading fusion + float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) + // airspeed fusion float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index a0c66f2ec3..bc741b1037 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -70,7 +70,7 @@ void Ekf::fuseGpsYaw() // using magnetic heading process noise // TODO extend interface to use yaw uncertainty provided by GPS if available - const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); // calculate intermediate variables const float HK0 = sinf(_gps_yaw_offset); @@ -209,7 +209,7 @@ 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.mag_heading_noise, 1.0e-2f)); + 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;