From 7f4fedde6af1bb67eac46e2f818ad9313e199dea Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 30 Jun 2020 17:12:04 +0200 Subject: [PATCH] GPS Yaw: fix heading initialisation and add unit tests When the antennas are not parallel to the x body axis, the GPS message contains the angular offset but the data is already corrected in the driver. EKF2 should then not add this offset during the initialisation. --- EKF/gps_yaw_fusion.cpp | 4 +- test/sensor_simulator/gps.cpp | 4 ++ test/sensor_simulator/gps.h | 1 + test/test_EKF_gps_yaw.cpp | 71 +++++++++++++++++++++++++++++++---- 4 files changed, 71 insertions(+), 9 deletions(-) diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index ebcdea2de5..d0b8ba5a8f 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -273,8 +273,8 @@ bool Ekf::resetGpsAntYaw() return false; } - // get measurement and correct for antenna array yaw offset - const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset; + // 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)); resetQuatStateYaw(measured_yaw, yaw_variance, true); diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 263b16b91f..7e0abbbfce 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -58,6 +58,10 @@ void Gps::setYaw(float yaw) { _gps_data.yaw = yaw; } +void Gps::setYawOffset(float yaw_offset) { + _gps_data.yaw_offset = yaw_offset; +} + void Gps::setFixType(int n) { _gps_data.fix_type = n; diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 4dcfecd702..8ff60d87b9 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -59,6 +59,7 @@ public: void setLongitude(int32_t lon); void setVelocity(const Vector3f& vel); void setYaw(float yaw); + void setYawOffset(float yaw); void setFixType(int n); void setNumberOfSatellites(int n); void setPdop(float pdop); diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index dee81174d2..dc4ca24ecd 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -54,6 +54,9 @@ class EkfGpsHeadingTest : public ::testing::Test { SensorSimulator _sensor_simulator; EkfWrapper _ekf_wrapper; + void runConvergenceScenario(float yaw_offset_rad = 0.f, float antenna_offset_rad = 0.f); + void checkConvergence(float truth, float tolerance = FLT_EPSILON); + // Setup the Ekf with synthetic measurements void SetUp() override { @@ -67,6 +70,29 @@ class EkfGpsHeadingTest : public ::testing::Test { } }; +void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad) +{ + // GIVEN: an initial GPS yaw, not aligned with the current one + float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad); + + _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYawOffset(antenna_offset_rad); + + // WHEN: the GPS yaw fusion is activated + _ekf_wrapper.enableGpsHeadingFusion(); + _sensor_simulator.runSeconds(5); + + // THEN: the estimate is reset and stays close to the measurement + checkConvergence(gps_heading, 0.05f); +} + +void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance) +{ + const float yaw_est = _ekf_wrapper.getYawAngle(); + EXPECT_NEAR(yaw_est, truth, math::radians(tolerance)) + << "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(truth); +} + TEST_F(EkfGpsHeadingTest, fusionStartWithReset) { // GIVEN:EKF that fuses GPS @@ -96,7 +122,9 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset) TEST_F(EkfGpsHeadingTest, yawConvergence) { // GIVEN: an initial GPS yaw, not aligned with the current one - float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f); + const float initial_yaw = math::radians(10.f); + float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + initial_yaw); + _sensor_simulator._gps.setYaw(gps_heading); // WHEN: the GPS yaw fusion is activated @@ -104,9 +132,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence) _sensor_simulator.runSeconds(5); // 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); + checkConvergence(gps_heading, 0.05f); // AND WHEN: the the measurement changes gps_heading += math::radians(2.f); @@ -115,9 +141,40 @@ TEST_F(EkfGpsHeadingTest, yawConvergence) // 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); + checkConvergence(gps_heading, 0.5f); +} + +TEST_F(EkfGpsHeadingTest, yaw0) +{ + runConvergenceScenario(); +} + +TEST_F(EkfGpsHeadingTest, yaw60) +{ + const float yaw_offset_rad = math::radians(60.f); + const float antenna_offset_rad = math::radians(80.f); + runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); +} + +TEST_F(EkfGpsHeadingTest, yaw180) +{ + const float yaw_offset_rad = math::radians(180.f); + const float antenna_offset_rad = math::radians(-20.f); + runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); +} + +TEST_F(EkfGpsHeadingTest, yawMinus120) +{ + const float yaw_offset_rad = math::radians(120.f); + const float antenna_offset_rad = math::radians(-42.f); + runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); +} + +TEST_F(EkfGpsHeadingTest, yawMinus30) +{ + const float yaw_offset_rad = math::radians(-30.f); + const float antenna_offset_rad = math::radians(10.f); + runConvergenceScenario(yaw_offset_rad, antenna_offset_rad); } TEST_F(EkfGpsHeadingTest, fallBackToMag)