fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 07:33: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/sensor_simulator/vio.cpp
+19 -2
View File
@@ -26,7 +26,12 @@ void Vio::setData(const extVisionSample& vio_data)
void Vio::setVelocityVariance(const Vector3f& velVar)
{
_vio_data.velVar = velVar;
setVelocityCovariance(matrix::diag(velVar));
}
void Vio::setVelocityCovariance(const Matrix3f& velCov)
{
_vio_data.velCov = velCov;
}
void Vio::setPositionVariance(const Vector3f& posVar)
@@ -54,6 +59,17 @@ void Vio::setOrientation(const Quatf& quat)
_vio_data.quat = quat;
}
void Vio::setVelocityFrameToBody()
{
_vio_data.vel_frame = BODY_FRAME_FRD;
}
void Vio::setVelocityFrameToLocal()
{
_vio_data.vel_frame = LOCAL_FRAME_FRD;
}
extVisionSample Vio::dataAtRest()
{
extVisionSample vio_data;
@@ -61,8 +77,9 @@ extVisionSample Vio::dataAtRest()
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
vio_data.angVar = 0.05f;
vio_data.vel_frame = LOCAL_FRAME_FRD;
return vio_data;
}
test/sensor_simulator/vio.h
+3
View File
@@ -52,11 +52,14 @@ public:
void setData(const extVisionSample& vio_data);
void setVelocityVariance(const Vector3f& velVar);
void setVelocityCovariance(const Matrix3f& velCov);
void setPositionVariance(const Vector3f& posVar);
void setAngularVariance(float angVar);
void setVelocity(const Vector3f& vel);
void setPosition(const Vector3f& pos);
void setOrientation(const Quatf& quat);
void setVelocityFrameToLocal();
void setVelocityFrameToBody();
extVisionSample dataAtRest();
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);
}