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

ekf2: EV fusion in body frame (#23191)

Browse Source
This commit is contained in:
Marco Hauswirth
2024-07-04 21:17:19 -04:00
committed by GitHub
parent 1f33abb4e9
commit a1f43636f3
9 changed files with 392 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_externalVision.cpp
+6 -19
View File
@@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
TEST_F(EkfExternalVisionTest, velocityFrameBody)
{
// GIVEN: Drone is turned 90 degrees
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator.simulateOrientation(quat_sim);
_ekf_wrapper.setMagFuseTypeNone();
_sensor_simulator.runSeconds(_tilt_align_time);
_ekf->set_vehicle_at_rest(false);
// Without any measurement x and y velocity variance are close
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
// WHEN: measurement is given in BODY-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToBody();
float yaw_var0 = _ekf->getYawVar();
const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityFrameToBody();
_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
_sensor_simulator._vio.setVelocity(vel_body);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
// THEN: As the drone is turned 90 degrees, velocity variance
// along local y axis is expected to be bigger
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f);
const Vector3f vel_earth_est = _ekf->getVelocity();
EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f);
const Vector3f vel_var = _ekf->getVelocityVariance();
EXPECT_TRUE(yaw_var0 < _ekf->getYawVar());
EXPECT_TRUE(vel_var(1) < vel_var(0));
}
TEST_F(EkfExternalVisionTest, velocityFrameLocal)