From c19f40e574f7eafa341a5bf66b836a08e9ba9e1f Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Wed, 15 Apr 2020 21:02:40 +0200 Subject: [PATCH] Add reset position test for GPS and VISION --- test/test_EKF_externalVision.cpp | 38 ++++++++++++++++++++++++++++++++ test/test_EKF_gps.cpp | 23 +++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/test/test_EKF_externalVision.cpp b/test/test_EKF_externalVision.cpp index 6c181c4bf6..69b11a72b0 100644 --- a/test/test_EKF_externalVision.cpp +++ b/test/test_EKF_externalVision.cpp @@ -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(); diff --git a/test/test_EKF_gps.cpp b/test/test_EKF_gps.cpp index bf5f3ef007..fd311d5005 100644 --- a/test/test_EKF_gps.cpp +++ b/test/test_EKF_gps.cpp @@ -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)); +}