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

EKF/common.h

@@ -81,8 +81,8 @@ struct outputSample {
 };
 
 struct outputVert {
-	float	    vel_d;		///< D velocity calculated using alternative algorithm (m/sec)
-	float	    vel_d_integ;	///< Integral of vel_d (m)
+	float	    vert_vel;		///< Vertical velocity calculated using alternative algorithm (m/sec)
+	float	    vert_vel_integ;	///< Integral of vertical velocity (m)
 	float	    dt;			///< delta time (sec)
 	uint64_t    time_us;		///< timestamp of the measurement (uSec)
 };

EKF/control.cpp

@@ -496,6 +496,9 @@ void Ekf::controlOpticalFlowFusion()
 				// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
 				const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
 				if (flow_aid_only) {
+					// TODO: Issue: First time we want to reset the optical flow velocity
+					// we will use _flow_compensated_XY_rad in the resetVelocity() function,
+					// but _flow_compensated_XY_rad is this zero as it gets updated for the first time below here.
 					resetVelocity();
 					resetPosition();
 

EKF/ekf.cpp

@@ -369,13 +369,13 @@ void Ekf::calculateOutputStates()
 	// increment the INS velocity states by the measurement plus corrections
 	// do the same for vertical state used by alternative correction algorithm
 	_output_new.vel += delta_vel_earth;
-	_output_vert_new.vel_d += delta_vel_earth(2);
+	_output_vert_new.vert_vel += delta_vel_earth(2);
 
 	// use trapezoidal integration to calculate the INS position states
 	// do the same for vertical state used by alternative correction algorithm
 	const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
 	_output_new.pos += delta_pos_NED;
-	_output_vert_new.vel_d_integ += delta_pos_NED(2);
+	_output_vert_new.vert_vel_integ += delta_pos_NED(2);
 
 	// accumulate the time for each update
 	_output_vert_new.dt += imu.delta_vel_dt;
@@ -447,21 +447,21 @@ void Ekf::calculateOutputStates()
 		const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
 		{
 			/*
-			 * Calculate a correction to be applied to vel_d that casues vel_d_integ to track the EKF
+			 * Calculate a correction to be applied to vert_vel that casues vert_vel_integ to track the EKF
 			 * down position state at the fusion time horizon using an alternative algorithm to what
-			 * is used for the vel and pos state tracking. The algorithm applies a correction to the vel_d
-			 * state history and propagates vel_d_integ forward in time using the corrected vel_d history.
+			 * is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel
+			 * state history and propagates vert_vel_integ forward in time using the corrected vert_vel history.
 			 * This provides an alternative vertical velocity output that is closer to the first derivative
 			 * of the position but does degrade tracking relative to the EKF state.
 			 */
 
 			// calculate down velocity and position tracking errors
-			const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
-			const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
+			const float vert_vel_err = (_state.vel(2) - _output_vert_delayed.vert_vel);
+			const float vert_vel_integ_err = (_state.pos(2) - _output_vert_delayed.vert_vel_integ);
 
 			// calculate a velocity correction that will be applied to the output state history
 			// using a PD feedback tuned to a 5% overshoot
-			const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
+			const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * pos_gain * 1.1f;
 
 			/*
 			 * Calculate corrections to be applied to vel and pos output state history.
@@ -470,7 +470,7 @@ void Ekf::calculateOutputStates()
 			 */
 
 			// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
-			// vel_d states and propagate vel_d_integ forward using the corrected vel_d
+			// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
 			uint8_t index = _output_vert_buffer.get_oldest_index();
 
 			const uint8_t size = _output_vert_buffer.get_length();
@@ -482,13 +482,13 @@ void Ekf::calculateOutputStates()
 
 				// correct the velocity
 				if (counter == 0) {
-					current_state.vel_d += vel_d_correction;
+					current_state.vert_vel += vert_vel_correction;
 				}
 
-				next_state.vel_d += vel_d_correction;
+				next_state.vert_vel += vert_vel_correction;
 
 				// position is propagated forward using the corrected velocity and a trapezoidal integrator
-				next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
+				next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
 
 				// advance the index
 				index = (index + 1) % size;

EKF/ekf.h

@@ -577,6 +577,12 @@ private:
 	// reset velocity states of the ekf
 	bool resetVelocity();
 
+	void resetVelocityTo(const Vector3f &vel);
+
+	inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
+
+	inline void resetVerticalVelocityTo(float new_vert_vel);
+
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();
 

EKF/ekf_helper.cpp

@@ -49,16 +49,11 @@
 // gps measurement then use for velocity initialisation
 bool Ekf::resetVelocity()
 {
-	// used to calculate the velocity change due to the reset
-	const Vector3f vel_before_reset = _state.vel;
-
-	// reset EKF states
 	if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
 		ECL_INFO_TIMESTAMPED("reset velocity to GPS");
 		// this reset is only called if we have new gps data at the fusion time horizon
-		_state.vel = _gps_sample_delayed.vel;
+		resetVelocityTo(_gps_sample_delayed.vel);
 
-		// use GPS accuracy to reset variances
 		P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
 
 	} else if (_control_status.flags.opt_flow) {
@@ -78,16 +73,11 @@ bool Ekf::resetVelocity()
 			vel_optflow_body(2) = 0.0f;
 
 			// rotate from body to earth frame
-			Vector3f vel_optflow_earth;
-			vel_optflow_earth = _R_to_earth * vel_optflow_body;
-
-			// take x and Y components
-			_state.vel(0) = vel_optflow_earth(0);
-			_state.vel(1) = vel_optflow_earth(1);
+			const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body;
 
+			resetHorizontalVelocityTo(Vector2f(vel_optflow_earth));
 		} else {
-			_state.vel(0) = 0.0f;
-			_state.vel(1) = 0.0f;
+			resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
 		}
 
 		// reset the horizontal velocity variance using the optical flow noise variance
@@ -99,35 +89,53 @@ bool Ekf::resetVelocity()
 		if(_params.fusion_mode & MASK_ROTATE_EV){
 			_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
 		}
-		_state.vel = _ev_vel;
+		resetVelocityTo(_ev_vel);
 		P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
+
 	} else {
 		ECL_INFO_TIMESTAMPED("reset velocity to zero");
 		// Used when falling back to non-aiding mode of operation
-		_state.vel(0) = 0.0f;
-		_state.vel(1) = 0.0f;
+		resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
 		P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
 	}
 
-	// calculate the change in velocity and apply to the output predictor state history
-	const Vector3f velocity_change = _state.vel - vel_before_reset;
+	return true;
+}
+
+void Ekf::resetVelocityTo(const Vector3f &new_vel) {
+	resetHorizontalVelocityTo(Vector2f(new_vel));
+	resetVerticalVelocityTo(new_vel(2));
+}
+
+void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) {
+	const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
+	_state.vel.xy() = new_horz_vel;
 
 	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
-		_output_buffer[index].vel += velocity_change;
+		_output_buffer[index].vel(0) += delta_horz_vel(0);
+		_output_buffer[index].vel(1) += delta_horz_vel(1);
 	}
+	_output_new.vel(0) += delta_horz_vel(0);
+	_output_new.vel(1) += delta_horz_vel(1);
 
-	// apply the change in velocity to our newest velocity estimate
-	// which was already taken out from the output buffer
-	_output_new.vel += velocity_change;
-
-	// capture the reset event
-	_state_reset_status.velNE_change(0) = velocity_change(0);
-	_state_reset_status.velNE_change(1) = velocity_change(1);
-	_state_reset_status.velD_change = velocity_change(2);
+	_state_reset_status.velNE_change = delta_horz_vel;
 	_state_reset_status.velNE_counter++;
-	_state_reset_status.velD_counter++;
+}
 
-	return true;
+void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
+	const float delta_vert_vel = new_vert_vel - _state.vel(2);
+	_state.vel(2) = new_vert_vel;
+
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		_output_buffer[index].vel(2) += delta_vert_vel;
+		_output_vert_buffer[index].vert_vel += delta_vert_vel;
+	}
+	_output_new.vel(2) += delta_vert_vel;
+	_output_vert_delayed.vert_vel = new_vert_vel;
+	_output_vert_new.vert_vel += delta_vert_vel;
+
+	_state_reset_status.velD_change = delta_vert_vel;
+	_state_reset_status.velD_counter++;
 }
 
 // Reset position states. If we have a recent and valid
@@ -222,8 +230,6 @@ void Ekf::resetHeight()
 	// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
 	float old_vert_pos = _state.pos(2);
 	bool vert_pos_reset = false;
-	float old_vert_vel = _state.vel(2);
-	bool vert_vel_reset = false;
 
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
@@ -297,66 +303,44 @@ void Ekf::resetHeight()
 	// reset the vertical velocity state
 	if (_control_status.flags.gps && !_gps_hgt_intermittent) {
 		// If we are using GPS, then use it to reset the vertical velocity
-		_state.vel(2) = gps_newest.vel(2);
+		resetVerticalVelocityTo(gps_newest.vel(2));
 
 		// the state variance is the same as the observation
 		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
 
 	} else {
 		// we don't know what the vertical velocity is, so set it to zero
-		_state.vel(2) = 0.0f;
+		resetVerticalVelocityTo(0.0f);
 
 		// Set the variance to a value large enough to allow the state to converge quickly
 		// that does not destabilise the filter
 		P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
-
 	}
 
-	vert_vel_reset = true;
-
 	// store the reset amount and time to be published
 	if (vert_pos_reset) {
 		_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
 		_state_reset_status.posD_counter++;
 	}
 
-	if (vert_vel_reset) {
-		_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
-		_state_reset_status.velD_counter++;
-	}
-
 	// apply the change in height / height rate to our newest height / height rate estimate
 	// which have already been taken out from the output buffer
 	if (vert_pos_reset) {
 		_output_new.pos(2) += _state_reset_status.posD_change;
 	}
 
-	if (vert_vel_reset) {
-		_output_new.vel(2) += _state_reset_status.velD_change;
-	}
-
 	// add the reset amount to the output observer buffered data
 	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
 		if (vert_pos_reset) {
 			_output_buffer[i].pos(2) += _state_reset_status.posD_change;
-			_output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change;
-		}
-
-		if (vert_vel_reset) {
-			_output_buffer[i].vel(2) += _state_reset_status.velD_change;
-			_output_vert_buffer[i].vel_d += _state_reset_status.velD_change;
+			_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
 		}
 	}
 
 	// add the reset amount to the output observer vertical position state
 	if (vert_pos_reset) {
-		_output_vert_delayed.vel_d_integ = _state.pos(2);
-		_output_vert_new.vel_d_integ = _state.pos(2);
-	}
-
-	if (vert_vel_reset) {
-		_output_vert_delayed.vel_d = _state.vel(2);
-		_output_vert_new.vel_d = _state.vel(2);
+		_output_vert_delayed.vert_vel_integ = _state.pos(2);
+		_output_vert_new.vert_vel_integ = _state.pos(2);
 	}
 }
 

EKF/estimator_interface.h

@@ -302,7 +302,7 @@ public:
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
 	float getVerticalPositionDerivative() const
 	{
-		return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
+		return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2);
 	}
 
 	// get the position of the body frame origin in local earth frame

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