fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 14:53: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_flow.cpp
+12
View File
@@ -64,7 +64,12 @@ public:
const float max_ground_distance = 50.f;
_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
// 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(7);
}
@@ -111,6 +116,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
_sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground);
startRangeFinderFusion(simulated_distance_to_ground);
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.f);
@@ -182,7 +188,9 @@ TEST_F(EkfFlowTest, inAirConvergence)
const float simulated_distance_to_ground = 5.f;
_sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground);
startRangeFinderFusion(simulated_distance_to_ground);
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.f);
@@ -224,7 +232,9 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
const float simulated_distance_to_ground = 5.f;
startRangeFinderFusion(simulated_distance_to_ground);
startZeroFlowFusion();
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.f);
@@ -259,7 +269,9 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
const float simulated_distance_to_ground = 5.f;
startRangeFinderFusion(simulated_distance_to_ground);
startZeroFlowFusion();
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.f);