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

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_externalVision.cpp
+5 -7
View File
@@ -78,6 +78,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
{
_sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator._vio.setPositionFrameToLocalNED();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
@@ -147,7 +148,6 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f);
const Vector3f simulated_velocity_in_ekf_frame =
@@ -179,6 +179,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
_sensor_simulator._vio.setPositionFrameToLocalNED();
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
@@ -197,7 +198,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_position_in_vision_frame(8.3f, -1.0f, 0.0f);
const Vector3f simulated_position_in_ekf_frame =
@@ -236,7 +236,6 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf externalVisionFrameOffset(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
// Simulate high uncertainty on vision x axis which is in this case
// the y EKF frame axis
@@ -254,9 +253,9 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
EXPECT_TRUE(velVar_new(1) > velVar_new(0));
// THEN: the frame offset should be estimated correctly
Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
estimatedExternalVisionFrameOffset.canonical()));
// Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
// EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
// estimatedExternalVisionFrameOffset.canonical()));
}
TEST_F(EkfExternalVisionTest, velocityFrameBody)
@@ -340,7 +339,6 @@ TEST_F(EkfExternalVisionTest, positionFrameLocal)
// WHEN: using EV yaw fusion and rotate EV is set
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, 0.f));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment(); // ROTATE_EV
_ekf_wrapper.enableExternalVisionHeadingFusion(); // EV_YAW
// AND WHEN: using EV position aiding