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

Add reset position test for GPS and VISION

This commit is contained in:
Kamil Ritz 2020-04-15 21:02:40 +02:00 committed by Mathieu Bresciani
parent 78a6b9f7a8
commit c19f40e574
2 changed files with 61 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
test/test_EKF_externalVision.cpp
View File

@ -162,6 +162,44 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(0.01f));
}
TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
{
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
// THEN: a reset to Vision velocity should be done
const Vector3f estimated_position = _ekf->getPosition();
EXPECT_TRUE(isEqual(estimated_position, simulated_position, 1e-5f));
}
TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
{
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
// Heading of drone in EKF frame is 0°
// 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 =
Dcmf(vision_to_ekf) * simulated_position_in_vision_frame;
_sensor_simulator._vio.setPosition(simulated_position_in_vision_frame);
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
// THEN: a reset to Vision velocity should be done
const Vector3f estimated_position_in_ekf_frame = _ekf->getPosition();
EXPECT_TRUE(isEqual(estimated_position_in_ekf_frame, simulated_position_in_ekf_frame, 1e-2f));
}
TEST_F(EkfExternalVisionTest, visionVarianceCheck)
{
const Vector3f velVar_init = _ekf->getVelocityVariance();

23
test/test_EKF_gps.cpp
View File

@ -117,3 +117,26 @@ TEST_F(EkfGpsTest, resetToGpsVelocity)
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-2f));
}
TEST_F(EkfGpsTest, resetToGpsPosition)
{
// GIVEN:EKF that fuses GPS
// and has gps checks already passed
const Vector3f previous_position = _ekf->getPosition();
// WHEN: stopping GPS fusion
_sensor_simulator.stopGps();
_sensor_simulator.runSeconds(11);
// AND: simulate jump in position
_sensor_simulator.startGps();
const Vector3f simulated_position_change(2.0f, -1.0f, 0.f);
_sensor_simulator._gps.stepHorizontalPositionByMeters(
Vector2f(simulated_position_change));
_sensor_simulator.runMicroseconds(1e5);
// THEN: a reset to the new GPS position should be done
const Vector3f estimated_position = _ekf->getPosition();
EXPECT_TRUE(isEqual(estimated_position,
previous_position + simulated_position_change, 1e-2f));
}