lpe: add vehicle_odometry and data validation handlers; improve inout interface

src/modules/local_position_estimator/sensors/flow.cpp

@@ -36,8 +36,10 @@ void BlockLocalPositionEstimator::flowInit()
 
 int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 {
+	matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
+
 	// check for sane pitch/roll
-	if (_eul(0) > 0.5f || _eul(1) > 0.5f) {
+	if (euler.phi() > 0.5f || euler.theta() > 0.5f) {
 		return -1;
 	}
 
@@ -58,8 +60,6 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 		return -1;
 	}
 
-	matrix::Eulerf euler = matrix::Quatf(_sub_att.get().q);
-
 	float d = agl() * cosf(euler.phi()) * cosf(euler.theta());
 
 	// optical flow in x, y axis
@@ -159,7 +159,8 @@ void BlockLocalPositionEstimator::flowCorrect()
 			   + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
 			   + _sub_att.get().yawspeed * _sub_att.get().yawspeed;
 
-	float rot_sq = _eul(0) * _eul(0) + _eul(1) * _eul(1);
+	matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
+	float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();
 
 	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
 				  _flow_r.get() * _flow_r.get() * rot_sq +

src/modules/local_position_estimator/sensors/gps.cpp

@@ -113,9 +113,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	if (gpsMeasure(y_global) != OK) { return; }
 
 	// gps measurement in local frame
-	double  lat = y_global(Y_gps_x);
-	double  lon = y_global(Y_gps_y);
-	float  alt = y_global(Y_gps_z);
+	double lat = y_global(Y_gps_x);
+	double lon = y_global(Y_gps_y);
+	float alt = y_global(Y_gps_z);
 	float px = 0;
 	float py = 0;
 	float pz = -(alt - _gpsAltOrigin);
@@ -189,7 +189,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
 
 	// publish innovations
-	for (int i = 0; i < 6; i++) {
+	for (size_t i = 0; i < 6; i++) {
 		_pub_innov.get().vel_pos_innov[i] = r(i);
 		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
 	}

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -52,9 +52,10 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
 	_lidarStats.update(Scalarf(d));
 	_time_last_lidar = _timeStamp;
 	y.setZero();
+	matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
 	y(0) = (d + _lidar_z_offset.get()) *
-	       cosf(_eul(0)) *
-	       cosf(_eul(1));
+	       cosf(euler.phi()) *
+	       cosf(euler.theta());
 	return OK;
 }
 

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -9,6 +9,10 @@ extern orb_advert_t mavlink_log_pub;
 static const uint32_t		REQ_MOCAP_INIT_COUNT = 20;
 static const uint32_t		MOCAP_TIMEOUT = 200000;	// 0.2 s
 
+// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV
+// TODO: the user should be allowed to set these values by a parameter
+static constexpr float 	EP_MAX_STD_DEV = 100.0f;
+
 void BlockLocalPositionEstimator::mocapInit()
 {
 	// measure
@@ -32,23 +36,55 @@ void BlockLocalPositionEstimator::mocapInit()
 		_sensorTimeout &= ~SENSOR_MOCAP;
 		_sensorFault &= ~SENSOR_MOCAP;
 
+		// get reference for global position
+		globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
+		_global_ref_timestamp = hrt_absolute_time();
+		_is_global_cov_init = globallocalconverter_initialized();
+
+		if (!_map_ref.init_done && _is_global_cov_init && !FUSE_VIS_POS) {
+			// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
+						     double(_ref_lat), double(_ref_lon), double(_ref_alt));
+			map_projection_init(&_map_ref, _ref_lat, _ref_lon);
+			// set timestamp when origin was set to current time
+			_time_origin = _timeStamp;
+		}
+
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
-			_altOriginGlobal = false;
-			_altOrigin = 0;
+			_altOriginGlobal = true;
+			_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
 		}
 	}
 }
 
 int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 {
-	y.setZero();
-	y(Y_mocap_x) = _sub_mocap.get().x;
-	y(Y_mocap_y) = _sub_mocap.get().y;
-	y(Y_mocap_z) = _sub_mocap.get().z;
-	_mocapStats.update(y);
-	_time_last_mocap = _sub_mocap.get().timestamp;
-	return OK;
+	if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
+		// check if the mocap data is valid based on the covariances
+		_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
+		_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
+		_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
+		_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
+	} else {
+		// if we don't have covariances, assume every reading
+		_mocap_xy_valid = true;
+		_mocap_z_valid = true;
+	}
+
+	if (!_mocap_xy_valid || !_mocap_z_valid) {
+		_time_last_mocap = _sub_mocap_odom.get().timestamp;
+		return !OK;
+
+	} else {
+		y.setZero();
+		y(Y_mocap_x) = _sub_mocap_odom.get().x;
+		y(Y_mocap_y) = _sub_mocap_odom.get().y;
+		y(Y_mocap_z) = _sub_mocap_odom.get().z;
+		_mocapStats.update(y);
+		_time_last_mocap = _sub_mocap_odom.get().timestamp;
+		return OK;
+	}
 }
 
 void BlockLocalPositionEstimator::mocapCorrect()
@@ -56,7 +92,10 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// measure
 	Vector<float, n_y_mocap> y;
 
-	if (mocapMeasure(y) != OK) { return; }
+	if (mocapMeasure(y) != OK) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv);
+		return;
+	}
 
 	// mocap measurement matrix, measures position
 	Matrix<float, n_y_mocap, n_x> C;
@@ -68,11 +107,23 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// noise matrix
 	Matrix<float, n_y_mocap, n_y_mocap> R;
 	R.setZero();
-	float mocap_p_var = _mocap_p_stddev.get() * \
-			    _mocap_p_stddev.get();
-	R(Y_mocap_x, Y_mocap_x) = mocap_p_var;
-	R(Y_mocap_y, Y_mocap_y) = mocap_p_var;
-	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
+
+	// use std dev from mocap data if available
+	if (_mocap_eph > _mocap_p_stddev.get()) {
+		R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph;
+		R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph;
+
+	} else {
+		R(Y_mocap_x, Y_mocap_x) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
+		R(Y_mocap_y, Y_mocap_y) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
+	}
+
+	if (_mocap_epv > _mocap_p_stddev.get()) {
+		R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv;
+
+	} else {
+		R(Y_mocap_z, Y_mocap_z) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
+	}
 
 	// residual
 	Vector<float, n_y_mocap> r = y - C * _x;
@@ -80,12 +131,12 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
 
 	// publish innovations
-	for (int i = 0; i < 3; i++) {
+	for (size_t i = 0; i < 3; i++) {
 		_pub_innov.get().vel_pos_innov[i] = r(i);
 		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
 	}
 
-	for (int i = 3; i < 6; i++) {
+	for (size_t i = 3; i < 6; i++) {
 		_pub_innov.get().vel_pos_innov[i] = 0;
 		_pub_innov.get().vel_pos_innov_var[i] = 1;
 	}

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -66,9 +66,10 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 	_sonarStats.update(Scalarf(d));
 	_time_last_sonar = _timeStamp;
 	y.setZero();
+	matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
 	y(0) = (d + _sonar_z_offset.get()) *
-	       cosf(_eul(0)) *
-	       cosf(_eul(1));
+	       cosf(euler.phi()) *
+	       cosf(euler.theta());
 	return OK;
 }
 

src/modules/local_position_estimator/sensors/vision.cpp

@@ -14,6 +14,10 @@ static const uint32_t		REQ_VISION_INIT_COUNT = 1;
 // set the timeout to 0.5 seconds
 static const uint32_t		VISION_TIMEOUT = 500000;	// 0.5 s
 
+// set pose/velocity as invalid if standard deviation is bigger than EP_MAX_STD_DEV
+// TODO: the user should be allowed to set these values by a parameter
+static constexpr float 	EP_MAX_STD_DEV = 100.0f;
+
 void BlockLocalPositionEstimator::visionInit()
 {
 	// measure
@@ -37,11 +41,16 @@ void BlockLocalPositionEstimator::visionInit()
 		_sensorTimeout &= ~SENSOR_VISION;
 		_sensorFault &= ~SENSOR_VISION;
 
-		if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) {
+		// get reference for global position
+		globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
+		_global_ref_timestamp = hrt_absolute_time();
+		_is_global_cov_init = globallocalconverter_initialized();
+
+		if (!_map_ref.init_done && _is_global_cov_init) {
 			// initialize global origin using the visual estimator reference
 			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
-						     double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt));
-			map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
+						     double(_ref_lat), double(_ref_lon), double(_ref_alt));
+			map_projection_init(&_map_ref, _ref_lat, _ref_lon);
 			// set timestamp when origin was set to current time
 			_time_origin = _timeStamp;
 		}
@@ -49,20 +58,38 @@ void BlockLocalPositionEstimator::visionInit()
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
 			_altOriginGlobal = true;
-			_altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f;
+			_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
 		}
 	}
 }
 
 int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
 {
-	y.setZero();
-	y(Y_vision_x) = _sub_vision_pos.get().x;
-	y(Y_vision_y) = _sub_vision_pos.get().y;
-	y(Y_vision_z) = _sub_vision_pos.get().z;
-	_visionStats.update(y);
-	_time_last_vision_p = _sub_vision_pos.get().timestamp;
-	return OK;
+	if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
+		// check if the vision data is valid based on the covariances
+		_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
+		_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
+		_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
+		_vision_z_valid = _vision_epv <= EP_MAX_STD_DEV;
+	} else {
+		// if we don't have covariances, assume every reading
+		_vision_xy_valid = true;
+		_vision_z_valid = true;
+	}
+
+	if (!_vision_xy_valid || !_vision_z_valid) {
+		_time_last_vision_p = _sub_visual_odom.get().timestamp;
+		return !OK;
+
+	} else {
+		y.setZero();
+		y(Y_vision_x) = _sub_visual_odom.get().x;
+		y(Y_vision_y) = _sub_visual_odom.get().y;
+		y(Y_vision_z) = _sub_visual_odom.get().z;
+		_visionStats.update(y);
+		_time_last_vision_p = _sub_visual_odom.get().timestamp;
+		return OK;
+	}
 }
 
 void BlockLocalPositionEstimator::visionCorrect()
@@ -70,7 +97,10 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// measure
 	Vector<float, n_y_vision> y;
 
-	if (visionMeasure(y) != OK) { return; }
+	if (visionMeasure(y) != OK) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", _vision_eph, _vision_epv);
+		return;
+	}
 
 	// vision measurement matrix, measures position
 	Matrix<float, n_y_vision, n_x> C;
@@ -83,18 +113,18 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Matrix<float, n_y_vision, n_y_vision> R;
 	R.setZero();
 
-	// use error estimates from vision topic if available
-	if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
-		R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
-		R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
+	// use std dev from vision data if available
+	if (_vision_eph > _vision_xy_stddev.get()) {
+		R(Y_vision_x, Y_vision_x) = _vision_eph * _vision_eph;
+		R(Y_vision_y, Y_vision_y) = _vision_eph * _vision_eph;
 
 	} else {
 		R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
 		R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
 	}
 
-	if (_sub_vision_pos.get().epv > _vision_z_stddev.get()) {
-		R(Y_vision_z, Y_vision_z) = _sub_vision_pos.get().epv * _sub_vision_pos.get().epv;
+	if (_vision_epv > _vision_z_stddev.get()) {
+		R(Y_vision_z, Y_vision_z) = _vision_epv * _vision_epv;
 
 	} else {
 		R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
@@ -103,20 +133,33 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// vision delayed x
 	uint8_t i_hist = 0;
 
-	float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f;	// measurement delay in seconds
+	float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp) * 1e-6f;	// measurement delay in seconds
 
 	if (vision_delay < 0.0f) { vision_delay = 0.0f; }
 
 	// use auto-calculated delay from measurement if parameter is set to zero
-	if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist)  < 0) { return; }
-
-	//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision delay : %0.2f ms", double(vision_delay * 1000.0f));
+	if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0){ return; }
 
 	Vector<float, n_x> x0 = _xDelay.get(i_hist);
 
 	// residual
-	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_vision, 1> r = y - C * x0;
+	// residual covariance
+	Matrix<float, n_y_vision, n_y_vision> S = C * _P * C.transpose() + R;
+
+	// publish innovations
+	for (size_t i = 0; i < 3; i++) {
+		_pub_innov.get().vel_pos_innov[i] = r(i, 0);
+		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
+	}
+
+	for (size_t i = 3; i < 6; i++) {
+		_pub_innov.get().vel_pos_innov[i] = 0;
+		_pub_innov.get().vel_pos_innov_var[i] = 1;
+	}
+
+	// residual covariance, (inverse)
+	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);