LPE : add support for precision landing measurement fusion

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -45,6 +45,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_dist_subs(),
 	_sub_lidar(nullptr),
 	_sub_sonar(nullptr),
+	_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
 
 	// publications
 	_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
@@ -92,6 +93,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pn_t_noise_density(this, "PN_T"),
 	_t_max_grade(this, "T_MAX_GRADE"),
 
+	// landing target
+	_target_min_cov(this, "LT_COV"),
+	_target_mode(this, "LTEST_MODE", false),
+
 	// init origin
 	_fake_origin(this, "FAKE_ORIGIN"),
 	_init_origin_lat(this, "LAT"),
@@ -134,6 +139,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_time_last_vision_p(0),
 	_time_last_mocap(0),
 	_time_last_land(0),
+	_time_last_target(0),
 
 	// reference altitudes
 	_altOrigin(0),
@@ -313,6 +319,7 @@ void BlockLocalPositionEstimator::update()
 	_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
+	bool targetPositionUpdated = _sub_landing_target_pose.updated();
 
 	// get new data
 	updateSubscriptions();
@@ -528,10 +535,20 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
+	if (targetPositionUpdated) {
+		if (_sensorTimeout & SENSOR_LAND_TARGET) {
+			landingTargetInit();
+
+		} else {
+			landingTargetCorrect();
+		}
+	}
+
 	if (_altOriginInitialized) {
 		// update all publications if possible
 		publishLocalPos();
 		publishEstimatorStatus();
+		_pub_innov.get().timestamp = _timeStamp;
 		_pub_innov.update();
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
@@ -562,6 +579,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
 	visionCheckTimeout();
 	mocapCheckTimeout();
 	landCheckTimeout();
+	landingTargetCheckTimeout();
 }
 
 bool BlockLocalPositionEstimator::landed()

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -21,6 +21,7 @@
 #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>
 
 // uORB Publications
 #include <uORB/Publication.hpp>
@@ -112,6 +113,7 @@ public:
 	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
 	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
 	enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
+	enum {Y_target_x = 0, Y_target_y, n_y_target};
 	enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
 	enum {
 		FUSE_GPS = 1 << 0,
@@ -215,6 +217,12 @@ private:
 	void landInit();
 	void landCheckTimeout();
 
+	// landing target
+	int  landingTargetMeasure(Vector<float, n_y_target> &y);
+	void landingTargetCorrect();
+	void landingTargetInit();
+	void landingTargetCheckTimeout();
+
 	// timeouts
 	void checkTimeouts();
 
@@ -251,6 +259,7 @@ private:
 	uORB::Subscription<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
 	uORB::Subscription<distance_sensor_s> *_sub_lidar;
 	uORB::Subscription<distance_sensor_s> *_sub_sonar;
+	uORB::Subscription<landing_target_pose_s> _sub_landing_target_pose;
 
 	// publications
 	uORB::Publication<vehicle_local_position_s> _pub_lpos;
@@ -318,6 +327,14 @@ private:
 	BlockParamFloat  _pn_t_noise_density;
 	BlockParamFloat  _t_max_grade;
 
+	// target mode paramters from landing_target_estimator module
+	enum TargetMode {
+		Target_Moving = 0,
+		Target_Stationary = 1
+	};
+	BlockParamFloat _target_min_cov;
+	BlockParamInt 	_target_mode;
+
 	// init origin
 	BlockParamInt    _fake_origin;
 	BlockParamFloat  _init_origin_lat;
@@ -360,6 +377,7 @@ private:
 	uint64_t _time_last_vision_p;
 	uint64_t _time_last_mocap;
 	uint64_t _time_last_land;
+	uint64_t _time_last_target;
 
 	// reference altitudes
 	float _altOrigin;

src/modules/local_position_estimator/CMakeLists.txt

@@ -48,6 +48,7 @@ px4_add_module(
 		sensors/vision.cpp
 		sensors/mocap.cpp
 		sensors/land.cpp
+		sensors/landing_target.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/local_position_estimator/params.c

@@ -424,6 +424,17 @@ PARAM_DEFINE_FLOAT(LPE_LAND_Z, 0.03f);
  */
 PARAM_DEFINE_FLOAT(LPE_LAND_VXY, 0.05f);
 
+/**
+ * Minimum landing target standard covariance, uses reported covariance if greater.
+ *
+ * @group Local Position Estimator
+ * @unit m^2
+ * @min 0.0
+ * @max 10
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(LPE_LT_COV, 0.0001f);
+
 /**
  * Integer bitmask controlling data fusion
  *

src/modules/local_position_estimator/sensors/landing_target.cpp

@@ -0,0 +1,121 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+static const uint64_t 	TARGET_TIMEOUT =   2000000; // [us]
+
+void BlockLocalPositionEstimator::landingTargetInit()
+{
+	if (_target_mode.get() == Target_Moving) {
+		// target is in moving mode, do not initialize
+		return;
+	}
+
+	Vector<float, n_y_target> y;
+
+	if (landingTargetMeasure(y) == OK) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init");
+		_sensorTimeout &= ~SENSOR_LAND_TARGET;
+		_sensorFault &= ~SENSOR_LAND_TARGET;
+	}
+}
+
+int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target> &y)
+{
+	if (_target_mode.get() == Target_Stationary) {
+		if (_sub_landing_target_pose.get().rel_vel_valid) {
+			y(0) = _sub_landing_target_pose.get().vx_rel;
+			y(1) = _sub_landing_target_pose.get().vy_rel;
+			_time_last_target = _timeStamp;
+
+		} else {
+			return -1;
+		}
+
+		return OK;
+
+	}
+
+	return -1;
+}
+
+void BlockLocalPositionEstimator::landingTargetCorrect()
+{
+	if (_target_mode.get() == Target_Moving) {
+		// nothing to do in this mode
+		return;
+	}
+
+	// measure
+	Vector<float, n_y_target> y;
+
+	if (landingTargetMeasure(y) != OK) { return; }
+
+	// calculate covariance
+	float cov_vx = _sub_landing_target_pose.get().cov_vx_rel;
+	float cov_vy = _sub_landing_target_pose.get().cov_vy_rel;
+
+	// use sensor value only if reasoanble
+	if (cov_vx < _target_min_cov.get() || cov_vy < _target_min_cov.get()) {
+		cov_vx = _target_min_cov.get();
+		cov_vy = _target_min_cov.get();
+	}
+
+	// target measurement matrix and noise matrix
+	Matrix<float, n_y_target, n_x> C;
+	C.setZero();
+	// residual = (y + vehicle velocity)
+	// sign change because target velocitiy is -vehicle velocity
+	C(Y_target_x, X_vx) = -1;
+	C(Y_target_y, X_vy) = -1;
+
+	// covariance matrix
+	SquareMatrix<float, n_y_target> R;
+	R.setZero();
+	R(0, 0) = cov_vx;
+	R(1, 1) = cov_vy;
+
+	// residual
+	Vector<float, n_y_target> r = y - C * _x;
+
+	// residual covariance, (inverse)
+	Matrix<float, n_y_target, n_y_target> S_I =
+		inv<float, n_y_target>(C * _P * C.transpose() + R);
+
+	// fault detection
+	float beta = (r.transpose()  * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_target]) {
+		if (!(_sensorFault & SENSOR_LAND_TARGET)) {
+			mavlink_and_console_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
+			_sensorFault |= SENSOR_LAND_TARGET;
+		}
+
+		// abort correction
+		return;
+
+	} else if (_sensorFault & SENSOR_LAND_TARGET) {
+		_sensorFault &= ~SENSOR_LAND_TARGET;
+		mavlink_and_console_log_info(&mavlink_log_pub, "Landing target OK");
+	}
+
+	// kalman filter correction
+	Matrix<float, n_x, n_y_target> K =
+		_P * C.transpose() * S_I;
+	Vector<float, n_x> dx = K * r;
+	_x += dx;
+	_P -= K * C * _P;
+
+}
+
+void BlockLocalPositionEstimator::landingTargetCheckTimeout()
+{
+	if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
+		if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
+			_sensorTimeout |= SENSOR_LAND_TARGET;
+			mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout");
+		}
+	}
+}