Refactor velocity reset (#788)

* Refactor velocity reset

* Add unit tests for velocity resets

* Expand updates to vertical buffer to velocity resets outside of resetHeight

* Improve matrix library usage

* Improve naming of vertical output samples

* Fix update of output_vert_new during reset

* Improve naming of vertical output samples 2

test/CMakeLists.txt

@@ -48,6 +48,7 @@ set(SRCS
 	test_EKF_externalVision.cpp
 	test_EKF_airspeed.cpp
 	test_EKF_withReplayData.cpp
+	test_EKF_flow.cpp
 	test_SensorRangeFinder.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})

test/sensor_simulator/range_finder.cpp

@@ -17,7 +17,7 @@ void RangeFinder::send(uint64_t time)
 {
 	_range_sample.time_us = time;
 	_ekf->setRangeData(_range_sample);
-	_ekf->set_rangefinder_limits(0.2f, 20.f); // This usually comes from the driver
+	_ekf->set_rangefinder_limits(_min_distance, _max_distance);
 }
 
 void RangeFinder::setData(float range_data_meters, int8_t range_quality)
@@ -26,5 +26,11 @@ void RangeFinder::setData(float range_data_meters, int8_t range_quality)
 	_range_sample.quality = range_quality;
 }
 
+void RangeFinder::setLimits(float min_distance_m, float max_distance_m)
+{
+	_min_distance = min_distance_m;
+	_max_distance = max_distance_m;
+}
+
 } // namespace sensor
 } // namespace sensor_simulator

test/sensor_simulator/range_finder.h

@@ -51,12 +51,14 @@ public:
 	~RangeFinder();
 
 	void setData(float range_data, int8_t range_quality);
+	void setLimits(float min_distance_m, float max_distance_m);
 
 private:
 	rangeSample _range_sample {};
+	float _min_distance {0.2f};
+	float _max_distance {20.0f};
 
 	void send(uint64_t time) override;
-
 };
 
 } // namespace sensor

test/sensor_simulator/sensor_simulator.cpp

@@ -104,7 +104,7 @@ void SensorSimulator::loadSensorDataFromFile(std::string file_name)
 	_has_replay_data = true;
 }
 
-void SensorSimulator::setSensorDataToDefault()
+void SensorSimulator::setSensorRateToDefault()
 {
 	_imu.setRateHz(250);
 	_mag.setRateHz(80);
@@ -115,7 +115,7 @@ void SensorSimulator::setSensorDataToDefault()
 	_vio.setRateHz(30);
 	_airspeed.setRateHz(100);
 }
-void SensorSimulator::setSensorRateToDefault()
+void SensorSimulator::setSensorDataToDefault()
 {
 	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
 		     Vector3f{0.0f,0.0f,0.0f});

test/test_EKF_externalVision.cpp

@@ -101,6 +101,47 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 	EXPECT_FALSE(_ekf->global_position_is_valid());
 }
 
+TEST_F(EkfExternalVisionTest, visionVelocityReset)
+{
+	const Vector3f simulated_velocity(0.3f, -1.0f, 0.4f);
+
+	_sensor_simulator._vio.setVelocity(simulated_velocity);
+	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runMicroseconds(6e5);
+
+	// THEN: a reset to Vision velocity should be done
+	const Vector3f estimated_velocity = _ekf->getVelocity();
+	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-5f));
+}
+
+TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
+{
+	// 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_velocity_in_vision_frame(0.3f, -1.0f, 0.4f);
+	const Vector3f simulated_velocity_in_ekf_frame =
+		Dcmf(vision_to_ekf) * simulated_velocity_in_vision_frame;
+	_sensor_simulator._vio.setVelocity(simulated_velocity_in_vision_frame);
+	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runMicroseconds(6e5);
+
+	// THEN: a reset to Vision velocity should be done
+	const Vector3f estimated_velocity_in_ekf_frame = _ekf->getVelocity();
+	EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 1e-5f));
+	// And: the frame offset should be estimated correctly
+	Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
+	EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(),
+				    estimatedExternalVisionFrameOffset.canonical()));
+}
+
 TEST_F(EkfExternalVisionTest, visionVarianceCheck)
 {
 	const Vector3f velVar_init = _ekf->getVelocityVariance();

test/test_EKF_flow.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test the gps fusion
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "sensor_simulator/sensor_simulator.h"
+#include "sensor_simulator/ekf_wrapper.h"
+
+
+class EkfFlowTest : public ::testing::Test {
+ public:
+
+	EkfFlowTest(): ::testing::Test(),
+	_ekf{std::make_shared<Ekf>()},
+	_sensor_simulator(_ekf),
+	_ekf_wrapper(_ekf) {};
+
+	std::shared_ptr<Ekf> _ekf;
+	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		_ekf->init(0);
+		_sensor_simulator.runSeconds(2);
+	}
+
+	// Use this method to clean up any memory, network etc. after each test
+	void TearDown() override
+	{
+	}
+};
+
+TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
+{
+	// 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();
+	_ekf->set_in_air_status(true);
+	_sensor_simulator.runSeconds(1.5f);
+
+	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
+	EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground);
+
+	// 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);
+	_sensor_simulator._flow.setData(flow_sample);
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.startFlow();
+	_sensor_simulator.runSeconds(0.2);
+
+	// THEN: estimated velocity should match simulated velocity
+	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
+	EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity)); // TODO: This needs to change
+}
+
+TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
+{
+	// WHEN: being on ground
+	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
+	EXPECT_LT(estimated_distance_to_ground, 0.3f);
+
+	// WHEN: start fusing flow data
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.startFlow();
+	_sensor_simulator.runSeconds(0.6);
+
+	// THEN: estimated velocity should match simulated velocity
+	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
+	EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f)));
+}

test/test_EKF_gps.cpp

@@ -83,3 +83,28 @@ TEST_F(EkfGpsTest, gpsTimeout)
 	// TODO: this is not happening as expected
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 }
+
+TEST_F(EkfGpsTest, resetToGpsVelocity)
+{
+	// GIVEN:EKF that fuses GPS
+	// and has gps checks already passed
+
+	// WHEN: stopping GPS fusion
+	_sensor_simulator.stopGps();
+	_sensor_simulator.runSeconds(11);
+
+	// AND: simulate constant velocity gps samples for short time
+	_sensor_simulator.startGps();
+	const Vector3f simulated_velocity(0.5f, 1.0f, -0.3f);
+	_sensor_simulator._gps.setVelocity(simulated_velocity);
+	const uint64_t dt_us = 1e5;
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(
+		Vector2f(simulated_velocity) * dt_us * 1e-6);
+	_sensor_simulator._gps.stepHeightByMeters(
+		simulated_velocity(2) * dt_us * 1e-6);
+	_sensor_simulator.runMicroseconds(dt_us);
+
+	// THEN: a reset to GPS velocity should be done
+	const Vector3f estimated_velocity = _ekf->getVelocity();
+	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-2f));
+}