fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 20:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_test: fix height offset compensation after origin reset

Browse Source
This commit is contained in:
bresch
2022-08-04 16:50:31 +02:00
committed by Mathieu Bresciani
parent e34de53e2e
commit 61f390b0dd
2 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -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;
}
src/modules/ekf2/test/test_EKF_basics.cpp
+3
View File
@@ -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);