LPE : add support for precision landing measurement 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");
+		}
+	}
+}