From 9551c1b61487d24b64a6435131270279394058a4 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 23 Aug 2023 14:06:11 +0200 Subject: [PATCH] ekf2: send global position if origin is set manually Even if the aiding is local only, a valid global estimate can be sent as long as we know the EKF's origin. --- src/modules/ekf2/EKF/ekf_helper.cpp | 23 ++++++- src/modules/ekf2/EKF/estimator_interface.h | 4 +- src/modules/ekf2/EKF/gps_checks.cpp | 4 +- src/modules/ekf2/test/test_EKF_basics.cpp | 65 +++++++++++++++++++ .../ekf2/test/test_EKF_fusionLogic.cpp | 1 - 5 files changed, 90 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 0e7a007465..2235a9dbb2 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -44,6 +44,7 @@ #include "python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h" #include +#include #include void Ekf::resetHorizontalVelocityToZero() @@ -432,6 +433,24 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; + const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); + const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); + const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); + + if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { + _mag_declination_gps = mag_declination_gps; + _mag_inclination_gps = mag_inclination_gps; + _mag_strength_gps = mag_strength_gps; + + _wmm_gps_time_last_set = _time_delayed_us; + } + + // We don't know the uncertainty of the origin + _gpos_origin_eph = 0.f; + _gpos_origin_epv = 0.f; + + _NED_origin_initialised = true; + // minimum change in position or height that triggers a reset static constexpr float MIN_RESET_DIST_M = 0.01f; @@ -468,7 +487,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gps_origin_eph)); + float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gpos_origin_eph)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -486,7 +505,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9) + sq(_gps_origin_epv)); + *ekf_epv = sqrtf(P(9, 9) + sq(_gpos_origin_epv)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f9c62967c6..e4b386f141 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -375,8 +375,8 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized bool _NED_origin_initialised{false}; - float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin - float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin + float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin + float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index f42ad5599b..7c18520afc 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -85,8 +85,8 @@ bool Ekf::collect_gps(const gpsMessage &gps) _NED_origin_initialised = true; // save the horizontal and vertical position uncertainty of the origin - _gps_origin_eph = gps.eph; - _gps_origin_epv = gps.epv; + _gpos_origin_eph = gps.eph; + _gpos_origin_epv = gps.epv; _information_events.flags.gps_checks_passed = true; ECL_INFO("GPS checks passed"); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index d7b00f1b8d..83602c7316 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -271,6 +271,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + EXPECT_FALSE(_ekf->global_origin_valid()); + _latitude_new = 45.0000005; _longitude_new = 111.0000005; _altitude_new = 1500.0; @@ -282,6 +284,11 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + // Global origin has been initialized but since there is no position aiding, the global + // position is still not valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + _sensor_simulator.runSeconds(1); float hpos = 0.f; @@ -299,4 +306,62 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_NEAR(vvel, 0.f, 0.02f); } +TEST_F(EkfBasicsTest, global_position_from_local_ev) +{ + // GIVEN: external vision (local) aiding + _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator._vio.setPositionFrameToLocalNED(); + _sensor_simulator.startExternalVision(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); +} + +TEST_F(EkfBasicsTest, global_position_from_opt_flow) +{ + // GIVEN: optical flow aiding + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + _sensor_simulator.startFlow(); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startRangeFinder(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); +} + // TODO: Add sampling tests diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index cfcc62d660..39de68a90f 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -183,7 +183,6 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow) const float max_ground_distance = 50.f; _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); - _sensor_simulator.startFlow(); _ekf_wrapper.enableFlowFusion(); _ekf->set_in_air_status(true);