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

ekf2: add terrain state

Browse Source
This commit is contained in:
bresch
2024-06-26 11:05:38 +02:00
committed by Silvan Fuhrer
parent 09f066a73a
commit 68980b59e2
53 changed files with 1936 additions and 1930 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_flow.cpp
+12 -12
View File
@@ -120,7 +120,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
_sensor_simulator.runSeconds(5.f);
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
const float estimated_distance_to_ground = _ekf->getHagl();
EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground);
reset_logging_checker.capturePreResetState();
@@ -133,10 +133,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
// Let it reset but not fuse more measurements. We actually need to send 2
// samples to get a reset because the first one cannot be used as the gyro
// compensation needs to be accumulated between two samples.
_sensor_simulator.runTrajectorySeconds(0.14);
_sensor_simulator.runTrajectorySeconds(1);
// THEN: estimated velocity should match simulated velocity
const Vector3f estimated_velocity = _ekf->getVelocity();
@@ -144,7 +141,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
simulated_velocity.print();
EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity))
<< "estimated vel = " << estimated_velocity(0) << ", "
<< estimated_velocity(1);
<< estimated_velocity(1) << "\n"
<< "simulated vel = " << simulated_velocity(0) << ", "
<< simulated_velocity(1);
EXPECT_NEAR(simulated_distance_to_ground, _ekf->getHagl(), 0.1f);
// AND: the reset in velocity should be saved correctly
reset_logging_checker.capturePostResetState();
@@ -158,7 +159,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
ResetLoggingChecker reset_logging_checker(_ekf);
// WHEN: being on ground
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
const float estimated_distance_to_ground = _ekf->getHagl();
EXPECT_LT(estimated_distance_to_ground, 0.3f);
reset_logging_checker.capturePreResetState();
@@ -177,11 +178,10 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f)))
<< estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1);
// AND: the reset in velocity should be saved correctly
// AND: the velocity isn't reset as it is already correct
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-9f));
}
TEST_F(EkfFlowTest, inAirConvergence)
@@ -222,9 +222,9 @@ TEST_F(EkfFlowTest, inAirConvergence)
// THEN: estimated velocity should converge to the simulated velocity
// This takes a bit of time because the data is inconsistent with IMU measurements
estimated_velocity = _ekf->getVelocity();
EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.05f)
EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.01f)
<< "estimated vel = " << estimated_velocity(0);
EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.05f)
EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.01f)
<< estimated_velocity(1);
}