Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

EV_yaw: reset ev_to_ekf to identity when yaw is reset to EV

Browse Source 
Otherwise, the old rotation matrix is used and not updated anymore
because the EKF is fusing EV yaw data.
This commit is contained in:
bresch
2021-06-17 14:44:20 +02:00
committed by Mathieu Bresciani
parent 804c3563fb
commit 611ace0ed6
2 changed files with 42 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+1
View File
@@ -532,6 +532,7 @@ bool Ekf::resetYawToEv()
const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw_new, yaw_new_variance, true);
_R_ev_to_ekf.setIdentity();
return true;
}
test/test_EKF_externalVision.cpp
+41
View File
@@ -322,3 +322,44 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f);
EXPECT_NEAR(vel_earth_est(1), 0.0f, 0.1f);
}
TEST_F(EkfExternalVisionTest, positionFrameLocal)
{
// GIVEN: Drone is turned 90 degrees
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator.simulateOrientation(quat_sim);
_sensor_simulator.runSeconds(_tilt_align_time);
// 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
const Vector3f pos_var_earth(0.01f, 0.01f, 0.01f);
Vector3f pos_earth(0.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setPositionVariance(pos_var_earth);
_sensor_simulator._vio.setPosition(pos_earth);
_ekf_wrapper.enableExternalVisionPositionFusion(); // EV_POS
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
// THEN: the fusions should start and the position should not drift
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
Vector3f pos_earth_est = _ekf->getPosition();
EXPECT_NEAR(pos_earth_est(0), pos_earth(0), 0.01f);
EXPECT_NEAR(pos_earth_est(1), pos_earth(1), 0.01f);
// WHEN: the measurement in EV FRD frame changes
pos_earth = Vector3f(0.3f, 0.0f, 0.0f);
_sensor_simulator._vio.setPosition(pos_earth);
_sensor_simulator.runSeconds(4);
// THEN: the position should converge to the EV position
// Note that the estimate is now in the EV frame because it is
// the reference frame
pos_earth_est = _ekf->getPosition();
EXPECT_NEAR(pos_earth_est(0), pos_earth(0), 0.1f);
EXPECT_NEAR(pos_earth_est(1), pos_earth(1), 0.1f);
}