Support vision velocity expressed in body frame too (#708)

* Support vision velocity expressed in body frame

* Use switch statement for vision velocity frame

* Robustify vision velocity frame test

* Increase lower bound on vision velocity noise to 0.05 m/s

EKF/common.h

@@ -55,6 +55,8 @@ using matrix::Vector2f;
 using matrix::Vector3f;
 using matrix::wrap_pi;
 
+enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD};
+
 struct gps_message {
 	uint64_t time_usec;
 	int32_t lat;		///< Latitude in 1E-7 degrees
@@ -139,11 +141,12 @@ struct flowSample {
 
 struct extVisionSample {
 	Vector3f pos;	///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
-	Vector3f vel;	///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
+	Vector3f vel;	///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
 	Quatf quat;		///< quaternion defining rotation from body to earth frame
 	Vector3f posVar;	///< XYZ position variances (m**2)
-	Vector3f velVar;	///< XYZ velocity variances ((m/sec)**2)
+	Matrix3f velCov;	///< XYZ velocity covariances ((m/sec)**2)
 	float angVar;		///< angular heading variance (rad**2)
+	velocity_frame_t vel_frame = BODY_FRAME_FRD;
 	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };
 

EKF/control.cpp

@@ -202,14 +202,14 @@ void Ekf::controlExternalVisionFusion()
 				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
 					_control_status.flags.ev_pos = true;
 					resetPosition();
-					ECL_INFO_TIMESTAMPED("commencing external vision position fusion");
+					ECL_INFO_TIMESTAMPED("starting vision pos fusion");
 				}
 
 				// turn on use of external vision measurements for velocity
 				if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
 					_control_status.flags.ev_vel = true;
 					resetVelocity();
-					ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion");
+					ECL_INFO_TIMESTAMPED("starting vision vel fusion");
 				}
 			}
 		}
@@ -235,7 +235,7 @@ void Ekf::controlExternalVisionFusion()
 				stopMagHdgFusion();
 				stopMag3DFusion();
 
-				ECL_INFO_TIMESTAMPED("commencing external vision yaw fusion");
+				ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
 			}
 		}
 
@@ -324,22 +324,7 @@ void Ekf::controlExternalVisionFusion()
 			Vector3f ev_vel_obs_var;
 			Vector2f ev_vel_innov_gates;
 
-			Vector3f vel_aligned{_ev_sample_delayed.vel};
-			Matrix3f ev_vel_var = matrix::diag(_ev_sample_delayed.velVar);
-
-			// rotate measurement into correct earth frame if required
-			if (_params.fusion_mode & MASK_ROTATE_EV) {
-				vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel;
-				ev_vel_var = _R_ev_to_ekf * ev_vel_var * _R_ev_to_ekf.transpose();
-			}
-
-			// correct velocity for offset relative to IMU
-			const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-			const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-			vel_aligned -= vel_offset_earth;
-
-			_ev_vel_innov = _state.vel - vel_aligned;
+			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
 
 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
@@ -350,7 +335,7 @@ void Ekf::controlExternalVisionFusion()
 				}
 			}
 
-			ev_vel_obs_var = matrix::max(ev_vel_var.diag(), sq(0.01f));
+			ev_vel_obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f));
 
 			ev_vel_innov_gates.setAll(fmaxf(_params.ev_vel_innov_gate, 1.0f));
 
@@ -368,7 +353,7 @@ void Ekf::controlExternalVisionFusion()
 
 		// Turn off EV fusion mode if no data has been received
 		stopEvFusion();
-		ECL_INFO_TIMESTAMPED("External Vision Data Stopped");
+		ECL_INFO_TIMESTAMPED("vision data stopped");
 
 	}
 }

EKF/ekf.h

@@ -645,6 +645,10 @@ private:
 	// update the rotation matrix which transforms EV navigation frame measurements into NED
 	void calcExtVisRotMat();
 
+	Vector3f getVisionVelocityInEkfFrame();
+
+	Vector3f getVisionVelocityVarianceInEkfFrame();
+
 	// limit the diagonal of the covariance matrix
 	// force symmetry when the argument is true
 	void fixCovarianceErrors(bool force_symmetry);

EKF/ekf_helper.cpp

@@ -106,8 +106,8 @@ void Ekf::resetVelocityToVision() {
 	if(_params.fusion_mode & MASK_ROTATE_EV){
 		_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
 	}
-	resetVelocityTo(_ev_vel);
-	P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
+	resetVelocityTo(getVisionVelocityInEkfFrame());
+	P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame());
 }
 
 void Ekf::resetHorizontalVelocityToZero() {
@@ -310,13 +310,15 @@ void Ekf::resetHeight()
 		P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
 	}
 
-	if (vert_pos_reset) {
 	// 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++;
+	}
 
 	// 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;
 	}
 
@@ -392,11 +394,11 @@ bool Ekf::realignYawGPS()
 
 		// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
 		if (badMagYaw || !_control_status.flags.yaw_align) {
-			ECL_WARN_TIMESTAMPED("bad yaw corrected using GPS course");
+			ECL_WARN_TIMESTAMPED("bad yaw, using GPS course");
 
 			// declare the magnetometer as failed if a bad yaw has occurred more than once
 			if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
-				ECL_WARN_TIMESTAMPED("stopping magnetometer use");
+				ECL_WARN_TIMESTAMPED("stopping mag use");
 				_control_status.flags.mag_fault = true;
 			}
 
@@ -1504,6 +1506,51 @@ void Ekf::updateBaroHgtOffset()
 	}
 }
 
+Vector3f Ekf::getVisionVelocityInEkfFrame()
+{
+	Vector3f vel;
+	// correct velocity for offset relative to IMU
+	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+
+	// rotate measurement into correct earth frame if required
+	switch(_ev_sample_delayed.vel_frame) {
+		case BODY_FRAME_FRD:
+			vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body);
+			break;
+		case LOCAL_FRAME_FRD:
+			const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+			if (_params.fusion_mode & MASK_ROTATE_EV)
+			{
+				vel = _R_ev_to_ekf *_ev_sample_delayed.vel - vel_offset_earth;
+			} else {
+				vel = _ev_sample_delayed.vel - vel_offset_earth;
+			}
+			break;
+	}
+	return vel;
+}
+
+Vector3f Ekf::getVisionVelocityVarianceInEkfFrame()
+{
+	Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
+
+	// rotate measurement into correct earth frame if required
+	switch(_ev_sample_delayed.vel_frame) {
+		case BODY_FRAME_FRD:
+			ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose();
+			break;
+
+		case LOCAL_FRAME_FRD:
+			if(_params.fusion_mode & MASK_ROTATE_EV)
+			{
+				ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose();
+			}
+			break;
+	}
+	return ev_vel_cov.diag();
+}
+
 // update the rotation matrix which rotates EV measurements into the EKF's navigation frame
 void Ekf::calcExtVisRotMat()
 {
@@ -1755,7 +1802,7 @@ bool Ekf::resetYawToEKFGSF()
 			// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
 			// and cause another navigation failure
 			_control_status.flags.mag_fault = true;
-			ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
+			ECL_INFO_TIMESTAMPED("Emergency yaw reset - mag use stopped");
 		}
 
 		return true;

EKF/gps_checks.cpp

@@ -99,11 +99,9 @@ bool Ekf::collect_gps(const gps_message &gps)
 		// if the user has selected GPS as the primary height source, switch across to using it
 
 		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
-			ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)");
 			startGpsHgtFusion();
-		} else {
-			ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
 		}
+		ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
 	}
 
 	// start collecting GPS if there is a 3D fix and the NED origin has been set

test/sensor_simulator/vio.cpp

@@ -26,7 +26,12 @@ void Vio::setData(const extVisionSample& vio_data)
 
 void Vio::setVelocityVariance(const Vector3f& velVar)
 {
-	_vio_data.velVar = velVar;
+	setVelocityCovariance(matrix::diag(velVar));
+}
+
+void Vio::setVelocityCovariance(const Matrix3f& velCov)
+{
+	_vio_data.velCov = velCov;
 }
 
 void Vio::setPositionVariance(const Vector3f& posVar)
@@ -54,6 +59,17 @@ void Vio::setOrientation(const Quatf& quat)
 	_vio_data.quat = quat;
 }
 
+void Vio::setVelocityFrameToBody()
+{
+	_vio_data.vel_frame = BODY_FRAME_FRD;
+}
+
+void Vio::setVelocityFrameToLocal()
+{
+	_vio_data.vel_frame = LOCAL_FRAME_FRD;
+}
+
+
 extVisionSample Vio::dataAtRest()
 {
 	extVisionSample vio_data;
@@ -61,8 +77,9 @@ extVisionSample Vio::dataAtRest()
 	vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
 	vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
 	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
-	vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
+	vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
 	vio_data.angVar = 0.05f;
+	vio_data.vel_frame = LOCAL_FRAME_FRD;
 	return vio_data;
 }
 

test/sensor_simulator/vio.h

@@ -52,11 +52,14 @@ public:
 
 	void setData(const extVisionSample& vio_data);
 	void setVelocityVariance(const Vector3f& velVar);
+	void setVelocityCovariance(const Matrix3f& velCov);
 	void setPositionVariance(const Vector3f& posVar);
 	void setAngularVariance(float angVar);
 	void setVelocity(const Vector3f& vel);
 	void setPosition(const Vector3f& pos);
 	void setOrientation(const Quatf& quat);
+	void setVelocityFrameToLocal();
+	void setVelocityFrameToBody();
 
 	extVisionSample dataAtRest();
 

test/test_EKF_externalVision.cpp

@@ -59,7 +59,6 @@ class EkfExternalVisionTest : public ::testing::Test {
 	void SetUp() override
 	{
 		_ekf->init(0);
-		_sensor_simulator.runSeconds(3);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -70,6 +69,7 @@ class EkfExternalVisionTest : public ::testing::Test {
 
 TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 {
+	_sensor_simulator.runSeconds(3);
 	_ekf_wrapper.enableExternalVisionPositionFusion();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(2);
@@ -104,6 +104,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 
 TEST_F(EkfExternalVisionTest, visionVelocityReset)
 {
+	_sensor_simulator.runSeconds(3);
 	ResetLoggingChecker reset_logging_checker(_ekf);
 	reset_logging_checker.capturePreResetState();
 
@@ -129,6 +130,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
 
 TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 {
+	_sensor_simulator.runSeconds(3);
 	ResetLoggingChecker reset_logging_checker(_ekf);
 	reset_logging_checker.capturePreResetState();
 	// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
@@ -164,6 +166,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 
 TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
 {
+	_sensor_simulator.runSeconds(3);
 	const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
 
 	_sensor_simulator._vio.setPosition(simulated_position);
@@ -178,6 +181,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
 
 TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
 {
+	_sensor_simulator.runSeconds(3);
 	// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
 	//        Heading of drone in EKF frame is 0°
 
@@ -202,6 +206,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
 
 TEST_F(EkfExternalVisionTest, visionVarianceCheck)
 {
+	_sensor_simulator.runSeconds(3);
 	const Vector3f velVar_init = _ekf->getVelocityVariance();
 	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
 
@@ -216,6 +221,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
 
 TEST_F(EkfExternalVisionTest, visionAlignment)
 {
+	_sensor_simulator.runSeconds(3);
 	// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
 	//        Heading of drone in EKF frame is 0°
 
@@ -244,3 +250,73 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 	EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
 				    estimatedExternalVisionFrameOffset.canonical()));
 }
+
+TEST_F(EkfExternalVisionTest, velocityFrameBody)
+{
+	// GIVEN: Drone is turned 90 degrees
+	const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
+	_sensor_simulator.simulateOrientation(quat_sim);
+	_sensor_simulator.runSeconds(3);
+
+	// Without any measurement x and y velocity variance are close
+	const Vector3f velVar_init = _ekf->getVelocityVariance();
+	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
+
+	// WHEN: measurement is given in BODY-FRAME and
+	//       x variance is bigger than y variance
+	_sensor_simulator._vio.setVelocityFrameToBody();
+	float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
+				  0.0f, 0.01f, 0.0f,
+				  0.0f, 0.0f, 0.01f};
+	const Matrix3f vel_cov_body(vel_cov_data);
+	const Vector3f vel_body(1.0f, 0.0f, 0.0f);
+	_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
+	_sensor_simulator._vio.setVelocity(vel_body);
+	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: As the drone is turned 90 degrees, velocity variance
+	//       along local y axis is expected to be bigger
+	const Vector3f velVar_new = _ekf->getVelocityVariance();
+	EXPECT_NEAR(velVar_new(1) / velVar_new(0), 80.f, 10.f);
+
+	const Vector3f vel_earth_est = _ekf->getVelocity();
+	EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
+	EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f);
+}
+
+TEST_F(EkfExternalVisionTest, velocityFrameLocal)
+{
+	// GIVEN: Drone is turned 90 degrees
+	const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
+	_sensor_simulator.simulateOrientation(quat_sim);
+	_sensor_simulator.runSeconds(3);
+
+	// Without any measurement x and y velocity variance are close
+	const Vector3f velVar_init = _ekf->getVelocityVariance();
+	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
+
+	// WHEN: measurement is given in LOCAL-FRAME and
+	//       x variance is bigger than y variance
+	_sensor_simulator._vio.setVelocityFrameToLocal();
+	float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
+				  0.0f, 0.01f, 0.0f,
+				  0.0f, 0.0f, 0.01f};
+	const Matrix3f vel_cov_earth(vel_cov_data);
+	const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
+	_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
+	_sensor_simulator._vio.setVelocity(vel_earth);
+	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: Independently on drones heading, velocity variance
+	//       along local x axis is expected to be bigger
+	const Vector3f velVar_new = _ekf->getVelocityVariance();
+	EXPECT_NEAR(velVar_new(0) / velVar_new(1), 80.f, 10.f);
+
+	const Vector3f vel_earth_est = _ekf->getVelocity();
+	EXPECT_NEAR(vel_earth_est(0), 1.0f, 0.1f);
+	EXPECT_NEAR(vel_earth_est(1), 0.0f, 0.1f);
+}