dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

flow: add unit test for yaw motion (#950)

This commit is contained in:
Mathieu Bresciani 2020-12-25 09:47:14 +01:00 committed by GitHub
parent df7f261e03
commit 18f334f4db
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 110 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
test/sensor_simulator/ekf_wrapper.cpp
View File

@ -95,6 +95,11 @@ bool EkfWrapper::isIntendingFlowFusion() const
return _ekf->control_status_flags().opt_flow;
}
void EkfWrapper::setFlowOffset(const Vector3f &offset)
{
_ekf_params->flow_pos_body = offset;
}
void EkfWrapper::enableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode |= MASK_USE_EVPOS;

1
test/sensor_simulator/ekf_wrapper.h
View File

@ -71,6 +71,7 @@ public:
void enableFlowFusion();
void disableFlowFusion();
bool isIntendingFlowFusion() const;
void setFlowOffset(const matrix::Vector3f &offset);
void enableExternalVisionPositionFusion();
void disableExternalVisionPositionFusion();

119
test/test_EKF_flow.cpp
View File

@ -71,18 +71,43 @@ class EkfFlowTest : public ::testing::Test {
void TearDown() override
{
}
void startRangeFinderFusion(float distance);
void startZeroFlowFusion();
void setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample, const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground);
};
void EkfFlowTest::startRangeFinderFusion(float distance)
{
_sensor_simulator._rng.setData(distance, 100);
_sensor_simulator._rng.setLimits(0.1f, 9.f);
_sensor_simulator.startRangeFinder();
}
void EkfFlowTest::startZeroFlowFusion()
{
// Start fusing zero flow data
_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
}
void EkfFlowTest::setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample, const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground)
{
flow_sample.flow_xy_rad =
Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
}
TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
{
ResetLoggingChecker reset_logging_checker(_ekf);
// WHEN: simulate being 5m above ground
const float simulated_distance_to_ground = 5.f;
_sensor_simulator._rng.setData(simulated_distance_to_ground, 100);
_sensor_simulator._rng.setLimits(0.1f, 9.f);
_sensor_simulator.startRangeFinder();
startRangeFinderFusion(simulated_distance_to_ground);
_ekf->set_in_air_status(true);
_sensor_simulator.runSeconds(5.f);
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
@ -93,9 +118,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
// WHEN: start fusing flow data
const Vector2f simulated_horz_velocity(0.5f, -0.2f);
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
flow_sample.flow_xy_rad =
Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
_sensor_simulator._flow.setData(flow_sample);
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
@ -152,10 +175,9 @@ TEST_F(EkfFlowTest, inAirConvergence)
{
// WHEN: simulate being 5m above ground
const float simulated_distance_to_ground = 5.f;
_sensor_simulator._rng.setData(simulated_distance_to_ground, 100);
_sensor_simulator._rng.setLimits(0.1f, 9.f);
_sensor_simulator.startRangeFinder();
startRangeFinderFusion(simulated_distance_to_ground);
_ekf->set_in_air_status(true);
_sensor_simulator.runSeconds(5.f);
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
@ -163,9 +185,7 @@ TEST_F(EkfFlowTest, inAirConvergence)
// WHEN: start fusing flow data
Vector2f simulated_horz_velocity(0.5f, -0.2f);
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
flow_sample.flow_xy_rad =
Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
_sensor_simulator._flow.setData(flow_sample);
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
@ -182,9 +202,7 @@ TEST_F(EkfFlowTest, inAirConvergence)
// AND: when the velocity changes
simulated_horz_velocity = Vector2f(0.8f, -0.5f);
flow_sample.flow_xy_rad =
Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
_sensor_simulator._flow.setData(flow_sample);
_sensor_simulator.runSeconds(5.0);
@ -196,3 +214,74 @@ TEST_F(EkfFlowTest, inAirConvergence)
EXPECT_NEAR(estimated_horz_velocity(1), simulated_horz_velocity(1), 0.05f)
<< estimated_horz_velocity(1);
}
TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
{
// WHEN: fusing range finder and optical flow data in air
const float simulated_distance_to_ground = 5.f;
startRangeFinderFusion(simulated_distance_to_ground);
startZeroFlowFusion();
_ekf->set_in_air_status(true);
_sensor_simulator.runSeconds(5.f);
// AND WHEN: there is a pure yaw rotation
const Vector3f body_rate(0.f, 0.f, 3.14159f);
const Vector3f flow_offset(0.15, -0.05f, 0.2f);
_ekf_wrapper.setFlowOffset(flow_offset);
const Vector2f simulated_horz_velocity(body_rate % flow_offset);
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
// use autopilot gyro data
flow_sample.gyro_xyz.setAll(NAN);
_sensor_simulator._flow.setData(flow_sample);
_sensor_simulator._imu.setGyroData(body_rate);
_sensor_simulator.runSeconds(10.f);
// THEN: the flow due to the yaw rotation and the offsets is canceled
// and the velocity estimate stays 0
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
<< "estimated vel = " << estimated_horz_velocity(0);
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
<< "estimated vel = " << estimated_horz_velocity(1);
}
TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
{
// WHEN: fusing range finder and optical flow data in air
const float simulated_distance_to_ground = 5.f;
startRangeFinderFusion(simulated_distance_to_ground);
startZeroFlowFusion();
_ekf->set_in_air_status(true);
_sensor_simulator.runSeconds(5.f);
// AND WHEN: there is a pure yaw rotation
const Vector3f body_rate(0.f, 0.f, 3.14159f);
const Vector3f flow_offset(-0.15, 0.05f, 0.2f);
_ekf_wrapper.setFlowOffset(flow_offset);
const Vector2f simulated_horz_velocity(body_rate % flow_offset);
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
// use flow sensor gyro data
// for clarification of the sign, see definition of flowSample
flow_sample.gyro_xyz = -body_rate * flow_sample.dt;
_sensor_simulator._flow.setData(flow_sample);
_sensor_simulator._imu.setGyroData(body_rate);
_sensor_simulator.runSeconds(10.f);
// THEN: the flow due to the yaw rotation and the offsets is canceled
// and the velocity estimate stays 0
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
<< "estimated vel = " << estimated_horz_velocity(0);
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
<< "estimated vel = " << estimated_horz_velocity(1);
}