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

Support vision velocity expressed in body frame too (#708)

Browse Source 
* Support vision velocity expressed in body frame

* Use switch statement for vision velocity frame

* Robustify vision velocity frame test

* Increase lower bound on vision velocity noise to 0.05 m/s
This commit is contained in:
kritz
2020-05-12 16:03:35 +02:00
committed by GitHub
parent c3de452e8e
commit 98801ad17b
8 changed files with 170 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_externalVision.cpp
+77 -1
View File
@@ -59,7 +59,6 @@ class EkfExternalVisionTest : public ::testing::Test {
void SetUp() override
{
_ekf->init(0);
_sensor_simulator.runSeconds(3);
}
// Use this method to clean up any memory, network etc. after each test
@@ -70,6 +69,7 @@ class EkfExternalVisionTest : public ::testing::Test {
TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
{
_sensor_simulator.runSeconds(3);
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
@@ -104,6 +104,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
TEST_F(EkfExternalVisionTest, visionVelocityReset)
{
_sensor_simulator.runSeconds(3);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
@@ -129,6 +130,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
{
_sensor_simulator.runSeconds(3);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
@@ -164,6 +166,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
{
_sensor_simulator.runSeconds(3);
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
@@ -178,6 +181,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
{
_sensor_simulator.runSeconds(3);
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
// Heading of drone in EKF frame is 0°
@@ -202,6 +206,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
TEST_F(EkfExternalVisionTest, visionVarianceCheck)
{
_sensor_simulator.runSeconds(3);
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
@@ -216,6 +221,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
TEST_F(EkfExternalVisionTest, visionAlignment)
{
_sensor_simulator.runSeconds(3);
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
// Heading of drone in EKF frame is 0°
@@ -244,3 +250,73 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
estimatedExternalVisionFrameOffset.canonical()));
}
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);
_sensor_simulator.runSeconds(3);
// 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 vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f};
const Matrix3f vel_cov_body(vel_cov_data);
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(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), 80.f, 10.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);
}
TEST_F(EkfExternalVisionTest, velocityFrameLocal)
{
// 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(3);
// 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 LOCAL-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToLocal();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f};
const Matrix3f vel_cov_earth(vel_cov_data);
const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
_sensor_simulator._vio.setVelocity(vel_earth);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
// THEN: Independently on drones heading, velocity variance
// along local x axis is expected to be bigger
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_new(0) / velVar_new(1), 80.f, 10.f);
const Vector3f vel_earth_est = _ekf->getVelocity();
EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f);
EXPECT_NEAR(vel_earth_est(1), 0.0f, 0.1f);
}