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

ekf2: add simple zero velocity update when vehicle at rest (#19149)

Browse Source 
- further decreases initial tilt alignment time (now down to 2.5 seconds if still) and improves initial bias estimates
This commit is contained in:
Daniel Agar
2022-03-12 12:56:31 -05:00
committed by GitHub
parent 3d54d25867
commit a430f0ccae
21 changed files with 683 additions and 503 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_gps.cpp
+11 -1
View File
@@ -58,7 +58,12 @@ public:
// Setup the Ekf with synthetic measurements
void SetUp() override
{
// run briefly to init, then manually set in air and at rest (default for a real vehicle)
_ekf->init(0);
_sensor_simulator.runSeconds(0.1);
_ekf->set_in_air_status(false);
_ekf->set_vehicle_at_rest(true);
_sensor_simulator.runSeconds(2);
_ekf_wrapper.enableGpsFusion();
_sensor_simulator.startGps();
@@ -104,11 +109,16 @@ TEST_F(EkfGpsTest, resetToGpsVelocity)
const uint64_t dt_us = 1e5;
_sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f(simulated_velocity) * dt_us * 1e-6);
_sensor_simulator._gps.stepHeightByMeters(simulated_velocity(2) * dt_us * 1e-6f);
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runMicroseconds(dt_us);
// THEN: a reset to GPS velocity should be done
const Vector3f estimated_velocity = _ekf->getVelocity();
EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-2f));
EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 1e-3f);
EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 1e-3f);
EXPECT_NEAR(estimated_velocity(2), simulated_velocity(2), 1e-3f);
// AND: the reset in velocity should be saved correctly
reset_logging_checker.capturePostResetState();