From 8626019ae017fcdd203be23140ccf3208ad87427 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 2 Dec 2024 18:18:14 +0100 Subject: [PATCH] EKF2: reset global position using variance --- .../aid_sources/aux_global_position/aux_global_position.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/test/test_EKF_flow.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 973a70dbe4..1d425cb853 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -110,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } else { // Try to initialize using measurement - if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, - sample.epv)) { + if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, + sq(sample.epv))) { ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index aa4597b25d..f578a929aa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -191,9 +191,9 @@ public: void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); - bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); - bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN, - float epv = NAN); + bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN); + bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN, + float vpos_var = NAN); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index bc33c350e7..7e00703971 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -383,7 +383,7 @@ TEST_F(EkfFlowTest, deadReckoning) const float altitude_new = 1500.0; const float eph = 50.f; const float epv = 10.f; - _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv); + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv); const Vector3f lpos_after_reset = _ekf->getPosition();