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

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -1,5 +1,4 @@
 #include "BlockLocalPositionEstimator.hpp"
-#include <drivers/drv_hrt.h>
 #include <systemlib/mavlink_log.h>
 #include <fcntl.h>
 #include <systemlib/err.h>
@@ -35,9 +34,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// gps 10 hz
 	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
 	// vision 50 hz
-	_sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),
+	_sub_visual_odom(ORB_ID(vehicle_visual_odometry), 1000 / 50, 0, &getSubscriptions()),
 	// mocap 50 hz
-	_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()),
+	_sub_mocap_odom(ORB_ID(vehicle_mocap_odometry), 1000 / 50, 0, &getSubscriptions()),
 	// all distance sensors, 10 hz
 	_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
 	_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
@@ -121,7 +120,26 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_lidarUpdated(false),
 	_sonarUpdated(false),
 	_landUpdated(false),
-	_baroUpdated(false)
+	_baroUpdated(false),
+
+	// sensor validation flags
+	_vision_xy_valid(false),
+	_vision_z_valid(false),
+	_mocap_xy_valid(false),
+	_mocap_z_valid(false),
+
+	// sensor std deviations
+	_vision_eph(0.0),
+	_vision_epv(0.0),
+	_mocap_eph(0.0),
+	_mocap_epv(0.0),
+
+	// local to global coversion related variables
+	_is_global_cov_init(false),
+	_global_ref_timestamp(0.0),
+	_ref_lat(0.0),
+	_ref_lon(0.0),
+	_ref_alt(0.0)
 {
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
@@ -257,8 +275,8 @@ void BlockLocalPositionEstimator::update()
 
 	_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
 	_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
-	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
+	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
-	_mocapUpdated = _sub_mocap.updated();
+	_mocapUpdated = _sub_mocap_odom.updated();
 	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
 	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
 	_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
@@ -349,7 +367,7 @@ void BlockLocalPositionEstimator::update()
 	// reinitialize x if necessary
 	bool reinit_x = false;
 
-	for (int i = 0; i < n_x; i++) {
+	for (size_t i = 0; i < n_x; i++) {
 		// should we do a reinit
 		// of sensors here?
 		// don't want it to take too long
@@ -361,7 +379,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	if (reinit_x) {
-		for (int i = 0; i < n_x; i++) {
+		for (size_t i = 0; i < n_x; i++) {
 			_x(i) = 0;
 		}
 
@@ -371,8 +389,8 @@ void BlockLocalPositionEstimator::update()
 	// force P symmetry and reinitialize P if necessary
 	bool reinit_P = false;
 
-	for (int i = 0; i < n_x; i++) {
+	for (size_t i = 0; i < n_x; i++) {
-		for (int j = 0; j <= i; j++) {
+		for (size_t j = 0; j <= i; j++) {
 			if (!PX4_ISFINITE(_P(i, j))) {
 				mavlink_and_console_log_info(&mavlink_log_pub,
 							     "%sreinit P (%d, %d) not finite", msg_label, i, j);
@@ -542,13 +560,15 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
 
 	// lie about eph/epv to allow visual odometry only navigation when velocity est. good
-	float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
+	float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
-	float epv = sqrtf(_P(X_z, X_z));
+	float evv = sqrtf(_P(X_vz, X_vz));
 	float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
+	float epv = sqrtf(_P(X_z, X_z));
+
 	float eph_thresh = 3.0f;
 	float epv_thresh = 3.0f;
 
-	if (vxy_stddev < _vxy_pub_thresh.get()) {
+	if (evh < _vxy_pub_thresh.get()) {
 		if (eph > eph_thresh) {
 			eph = eph_thresh;
 		}
@@ -563,10 +583,12 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
 		_pub_lpos.get().timestamp = _timeStamp;
+
 		_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
 		_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
 		_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
 		_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;
+
 		_pub_lpos.get().x = xLP(X_x);	// north
 		_pub_lpos.get().y = xLP(X_y);	// east
 
@@ -577,14 +599,19 @@ void BlockLocalPositionEstimator::publishLocalPos()
 			_pub_lpos.get().z = xLP(X_z);	// down
 		}
 
-		_pub_lpos.get().vx = xLP(X_vx);	// north
+		_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
-		_pub_lpos.get().vy = xLP(X_vy);	// east
+
-		_pub_lpos.get().vz = xLP(X_vz);	// down
+		_pub_lpos.get().vx = xLP(X_vx);		// north
+		_pub_lpos.get().vy = xLP(X_vy);		// east
+		_pub_lpos.get().vz = xLP(X_vz);		// down
 
 		// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
 		_pub_lpos.get().z_deriv = xLP(X_vz);
 
-		_pub_lpos.get().yaw = _eul(2);
+		_pub_lpos.get().ax = _u(U_ax);		// north
+		_pub_lpos.get().ay = _u(U_ay);		// east
+		_pub_lpos.get().az = _u(U_az);		// down
+
 		_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
 		_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
 		_pub_lpos.get().ref_timestamp = _time_origin;
@@ -600,15 +627,13 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
 		_pub_lpos.get().eph = eph;
 		_pub_lpos.get().epv = epv;
-		_pub_lpos.update();
+		_pub_lpos.get().evh = evh;
-		//TODO provide calculated values for these
+		_pub_lpos.get().evv = evv;
-		_pub_lpos.get().evh = 0.0f;
-		_pub_lpos.get().evv = 0.0f;
 		_pub_lpos.get().vxy_max = INFINITY;
 		_pub_lpos.get().vz_max = INFINITY;
 		_pub_lpos.get().hagl_min = INFINITY;
 		_pub_lpos.get().hagl_max = INFINITY;
-
+		_pub_lpos.update();
 	}
 }
 
@@ -616,11 +641,40 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 {
 	_pub_est_status.get().timestamp = _timeStamp;
 
-	for (int i = 0; i < n_x; i++) {
+	for (size_t i = 0; i < n_x; i++) {
 		_pub_est_status.get().states[i] = _x(i);
-		_pub_est_status.get().covariances[i] = _P(i, i);
 	}
 
+	// matching EKF2 covariances indexing
+	// quaternion - not determined, as it is a position estimator
+	_pub_est_status.get().covariances[0] = NAN;
+	_pub_est_status.get().covariances[1] = NAN;
+	_pub_est_status.get().covariances[2] = NAN;
+	_pub_est_status.get().covariances[3] = NAN;
+	// linear velocity
+	_pub_est_status.get().covariances[4] = _P(X_vx, X_vx);
+	_pub_est_status.get().covariances[5] = _P(X_vy, X_vy);
+	_pub_est_status.get().covariances[6] = _P(X_vz, X_vz);
+	// position
+	_pub_est_status.get().covariances[7] = _P(X_x, X_x);
+	_pub_est_status.get().covariances[8] = _P(X_y, X_y);
+	_pub_est_status.get().covariances[9] = _P(X_z, X_z);
+	// gyro bias - not determined
+	_pub_est_status.get().covariances[10] = NAN;
+	_pub_est_status.get().covariances[11] = NAN;
+	_pub_est_status.get().covariances[12] = NAN;
+	// accel bias
+	_pub_est_status.get().covariances[13] = _P(X_bx, X_bx);
+	_pub_est_status.get().covariances[14] = _P(X_by, X_by);
+	_pub_est_status.get().covariances[15] = _P(X_bz, X_bz);
+	// mag - not determined
+	for (size_t i = 16; i <= 21; i++) {
+		_pub_est_status.get().covariances[i] = NAN;
+	}
+	// replacing the hor wind cov with terrain altitude covariance
+	_pub_est_status.get().covariances[22] = _P(X_tz, X_tz);
+	_pub_est_status.get().covariances[23] = NAN;
+
 	_pub_est_status.get().n_states = n_x;
 	_pub_est_status.get().health_flags = _sensorFault;
 	_pub_est_status.get().timeout_flags = _sensorTimeout;
@@ -640,13 +694,14 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	float alt = -xLP(X_z) + _altOrigin;
 
 	// lie about eph/epv to allow visual odometry only navigation when velocity est. good
-	float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
+	float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
-	float epv = sqrtf(_P(X_z, X_z));
 	float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
+	float epv = sqrtf(_P(X_z, X_z));
+
 	float eph_thresh = 3.0f;
 	float epv_thresh = 3.0f;
 
-	if (vxy_stddev < _vxy_pub_thresh.get()) {
+	if (evh < _vxy_pub_thresh.get()) {
 		if (eph > eph_thresh) {
 			eph = eph_thresh;
 		}
@@ -666,8 +721,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().vel_n = xLP(X_vx);
 		_pub_gpos.get().vel_e = xLP(X_vy);
 		_pub_gpos.get().vel_d = xLP(X_vz);
-
+		_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
-		_pub_gpos.get().yaw = _eul(2);
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
@@ -771,9 +825,8 @@ void BlockLocalPositionEstimator::updateSSParams()
 void BlockLocalPositionEstimator::predict()
 {
 	// get acceleration
-	matrix::Quatf q(&_sub_att.get().q[0]);
+	_q = matrix::Quatf(&_sub_att.get().q[0]);
-	_eul = matrix::Euler<float>(q);
+	_R_att = matrix::Dcm<float>(_q);
-	_R_att = matrix::Dcm<float>(q);
 	Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 	// note, bias is removed in dynamics function
 	_u = _R_att * a;
@@ -838,12 +891,12 @@ void BlockLocalPositionEstimator::predict()
 				      _B * _R * _B.transpose() + _Q) * getDt();
 
 	// covariance propagation logic
-	for (int i = 0; i < n_x; i++) {
+	for (size_t i = 0; i < n_x; i++) {
 		if (_P(i, i) > P_MAX) {
 			// if diagonal element greater than max, stop propagating
 			dP(i, i) = 0;
 
-			for (int j = 0; j < n_x; j++) {
+			for (size_t j = 0; j < n_x; j++) {
 				dP(i, j) = 0;
 				dP(j, i) = 0;
 			}

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <px4_posix.h>
+#include <drivers/drv_hrt.h>
 #include <px4_module_params.h>
 #include <controllib/blocks.hpp>
 #include <mathlib/mathlib.h>
@@ -21,9 +22,9 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_odometry.h>
 
 // uORB Publications
 #include <uORB/Publication.hpp>
@@ -251,8 +252,8 @@ private:
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
 	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
-	uORB::Subscription<vehicle_local_position_s> _sub_vision_pos;
+	uORB::Subscription<vehicle_odometry_s> _sub_visual_odom;
-	uORB::Subscription<att_pos_mocap_s> _sub_mocap;
+	uORB::Subscription<vehicle_odometry_s> _sub_mocap_odom;
 	uORB::Subscription<distance_sensor_s> _sub_dist0;
 	uORB::Subscription<distance_sensor_s> _sub_dist1;
 	uORB::Subscription<distance_sensor_s> _sub_dist2;
@@ -411,13 +412,32 @@ private:
 	bool _landUpdated;
 	bool _baroUpdated;
 
+	// sensor validation flags
+	bool _vision_xy_valid;
+	bool _vision_z_valid;
+	bool _mocap_xy_valid;
+	bool _mocap_z_valid;
+
+	// sensor std deviations
+	float _vision_eph;
+	float _vision_epv;
+	float _mocap_eph;
+	float _mocap_epv;
+
+	// local to global coversion related variables
+	bool _is_global_cov_init;
+	uint64_t _global_ref_timestamp;
+	double _ref_lat;
+	double _ref_lon;
+	float _ref_alt;
+
 	// state space
 	Vector<float, n_x>  _x;	// state vector
 	Vector<float, n_u>  _u;	// input vector
 	Matrix<float, n_x, n_x>  _P;	// state covariance matrix
 
+	matrix::Quatf _q;
 	matrix::Dcm<float> _R_att;
-	Vector3f _eul;
 
 	Matrix<float, n_x, n_x>  _A;	// dynamics matrix
 	Matrix<float, n_x, n_u>  _B;	// input matrix

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 lat = y_global(Y_gps_x);
-	double  lon = y_global(Y_gps_y);
+	double lon = y_global(Y_gps_y);
-	float  alt = y_global(Y_gps_z);
+	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(euler.phi()) *
-	       cosf(_eul(1));
+	       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;
+			_altOriginGlobal = true;
-			_altOrigin = 0;
+			_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
 		}
 	}
 }
 
 int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 {
-	y.setZero();
+	if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
-	y(Y_mocap_x) = _sub_mocap.get().x;
+		// check if the mocap data is valid based on the covariances
-	y(Y_mocap_y) = _sub_mocap.get().y;
+		_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
-	y(Y_mocap_z) = _sub_mocap.get().z;
+		_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
-	_mocapStats.update(y);
+		_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
-	_time_last_mocap = _sub_mocap.get().timestamp;
+		_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
-	return OK;
+	} 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();
+	// use std dev from mocap data if available
-	R(Y_mocap_x, Y_mocap_x) = mocap_p_var;
+	if (_mocap_eph > _mocap_p_stddev.get()) {
-	R(Y_mocap_y, Y_mocap_y) = mocap_p_var;
+		R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph;
-	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
+		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(euler.phi()) *
-	       cosf(_eul(1));
+	       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));
+						     double(_ref_lat), double(_ref_lon), double(_ref_alt));
-			map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
+			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();
+	if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
-	y(Y_vision_x) = _sub_vision_pos.get().x;
+		// check if the vision data is valid based on the covariances
-	y(Y_vision_y) = _sub_vision_pos.get().y;
+		_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
-	y(Y_vision_z) = _sub_vision_pos.get().z;
+		_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
-	_visionStats.update(y);
+		_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
-	_time_last_vision_p = _sub_vision_pos.get().timestamp;
+		_vision_z_valid = _vision_epv <= EP_MAX_STD_DEV;
-	return OK;
+	} 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
+	// use std dev from vision data if available
-	if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
+	if (_vision_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_x, Y_vision_x) = _vision_eph * _vision_eph;
-		R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().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()) {
+	if (_vision_epv > _vision_z_stddev.get()) {
-		R(Y_vision_z, Y_vision_z) = _sub_vision_pos.get().epv * _sub_vision_pos.get().epv;
+		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; }
+	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));
 
 	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);