Add reset position test for GPS and VISION

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();

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));
+}