diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 81888fa2b0..21fb9730e5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -733,9 +733,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons resetVerticalPositionTo(_gps_alt_ref - current_alt); - _baro_hgt_offset -= _state_reset_status.posD_change; + _baro_hgt_offset += _state_reset_status.posD_change; - _rng_hgt_offset -= _state_reset_status.posD_change; + _rng_hgt_offset += _state_reset_status.posD_change; _ev_hgt_offset -= _state_reset_status.posD_change; } diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 5167eb9de7..d7b00f1b8d 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -249,12 +249,15 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) float vpos = 0.f; float hvel = 0.f; float vvel = 0.f; + float baro_vpos = 0.f; // After the change of origin, the pos and vel innovations should stay small _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); + _ekf->getBaroHgtInnovRatio(baro_vpos); EXPECT_NEAR(hpos, 0.f, 0.05f); EXPECT_NEAR(vpos, 0.f, 0.05f); + EXPECT_NEAR(baro_vpos, 0.f, 0.05f); EXPECT_NEAR(hvel, 0.f, 0.02f); EXPECT_NEAR(vvel, 0.f, 0.02f);