Add landing_target_estimator for precision landing from @ndepal

src/modules/landing_target_estimator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__landing_target_estimator
+	MAIN landing_target_estimator
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		landing_target_estimator_main.cpp
+		LandingTargetEstimator.cpp
+		KalmanFilter.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/modules/landing_target_estimator/KalmanFilter.cpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file KalmanFilter.h
+ * Simple Kalman Filter for variable gain low-passing
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ *
+ */
+
+#include "KalmanFilter.h"
+
+namespace landing_target_estimator
+{
+KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
+{
+	init(initial, covInit);
+}
+
+void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
+{
+	_x = initial;
+	_covariance = covInit;
+}
+
+void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
+{
+	matrix::Vector<float, 2> initial;
+	initial(0) = initial0;
+	initial(1) = initial1;
+	matrix::Matrix<float, 2, 2> covInit;
+	covInit(0, 0) = covInit00;
+	covInit(1, 1) = covInit11;
+
+	init(initial, covInit);
+}
+
+void KalmanFilter::predict(float dt, float acc, float acc_unc)
+{
+	_x(0) += _x(1) * dt + dt * dt / 2 * acc;
+	_x(1) += acc * dt;
+
+	matrix::Matrix<float, 2, 2> A; // propagation matrix
+	A(0, 0) = 1;
+	A(1, 1) = 1;
+	A(0, 1) = dt;
+
+	matrix::Matrix<float, 2, 1> G; // noise model
+	G(0, 0) = dt * dt / 2;
+	G(1, 0) = dt;
+
+	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
+
+	_covariance = A * _covariance * A.transpose() + process_noise;
+}
+
+bool KalmanFilter::update(float meas, float measUnc)
+{
+
+	// H = [1, 0]
+	_residual = meas - _x(0);
+
+	// H * P * H^T simply selects P(0,0)
+	_innovCov = _covariance(0, 0) + measUnc;
+
+	// outlier rejection
+	float beta = _residual / _innovCov * _residual;
+
+	// 5% false alarm probability
+	if (beta > 3.84f) {
+		return false;
+	}
+
+	matrix::Vector<float, 2> kalmanGain;
+	kalmanGain(0) = _covariance(0, 0);
+	kalmanGain(1) = _covariance(1, 0);
+	kalmanGain /= _innovCov;
+
+	_x += kalmanGain * _residual;
+
+	matrix::Matrix<float, 2, 2> identity;
+	identity.identity();
+
+	matrix::Matrix<float, 2, 2> KH; // kalmanGain * H
+	KH(0, 0) = kalmanGain(0);
+	KH(1, 0) = kalmanGain(1);
+
+	_covariance = (identity - KH) * _covariance;
+
+	return true;
+
+}
+
+void KalmanFilter::getState(matrix::Vector<float, 2> &state)
+{
+	state = _x;
+}
+
+void KalmanFilter::getState(float &state0, float &state1)
+{
+	state0 = _x(0);
+	state1 = _x(1);
+}
+
+void KalmanFilter::getCovariance(matrix::Matrix<float, 2, 2> &covariance)
+{
+	covariance = _covariance;
+}
+
+void KalmanFilter::getCovariance(float &cov00, float &cov11)
+{
+	cov00 = _covariance(0, 0);
+	cov11 = _covariance(1, 1);
+}
+
+void KalmanFilter::getInnovations(float &innov, float &innovCov)
+{
+	innov = _residual;
+	innovCov = _innovCov;
+}
+
+} // namespace landing_target_estimator

src/modules/landing_target_estimator/KalmanFilter.h

@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file KalmanFilter.h
+ * Simple Kalman Filter for variable gain low-passing
+ *
+ * Constant velocity model. Prediction according to
+ * x_{k+1} = A * x_{k}
+ * with A = [1 dt; 0 1]
+ *
+ * Update with a direct measurement of the first state:
+ * H = [1 0]
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ *
+ */
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <matrix/Matrix.hpp>
+#include <matrix/Vector.hpp>
+
+#pragma once
+
+namespace landing_target_estimator
+{
+class KalmanFilter
+{
+public:
+	/**
+	 * Default constructor, state not initialized
+	 */
+	KalmanFilter() {};
+
+	/**
+	 * Constructor, initialize state
+	 */
+	KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
+
+	/**
+	 * Default desctructor
+	 */
+	virtual ~KalmanFilter() {};
+
+	/**
+	 * Initialize filter state
+	 * @param initial initial state
+	 * @param covInit initial covariance
+	 */
+	void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
+
+	/**
+	 * Initialize filter state, only specifying diagonal covariance elements
+	 * @param initial0  first initial state
+	 * @param initial1  second initial state
+	 * @param covInit00 initial variance of first state
+	 * @param covinit11 initial variance of second state
+	 */
+	void init(float initial0, float initial1, float covInit00, float covInit11);
+
+	/**
+	 * Predict the state with an external acceleration estimate
+	 * @param dt            Time delta in seconds since last state change
+	 * @param acc           Acceleration estimate
+	 * @param acc_unc       Variance of acceleration estimate
+	 */
+	void predict(float dt, float acc, float acc_unc);
+
+	/**
+	 * Update the state estimate with a measurement
+	 * @param meas    state measeasurement
+	 * @param measUnc measurement uncertainty
+	 * @return update success (measurement not rejected)
+	 */
+	bool update(float meas, float measUnc);
+
+	/**
+	 * Get the current filter state
+	 * @param x1 State
+	 */
+	void getState(matrix::Vector<float, 2> &state);
+
+	/**
+	 * Get the current filter state
+	 * @param state0 First state
+	 * @param state1 Second state
+	 */
+	void getState(float &state0, float &state1);
+
+	/**
+	 * Get state covariance
+	 * @param covariance Covariance of the state
+	 */
+	void getCovariance(matrix::Matrix<float, 2, 2> &covariance);
+
+	/**
+	 * Get state variances (diagonal elements)
+	 * @param cov00 Variance of first state
+	 * @param cov11 Variance of second state
+	 */
+	void getCovariance(float &cov00, float &cov11);
+
+	/**
+	 * Get measurement innovation and covariance of last update call
+	 * @param innov Measurement innovation
+	 * @param innovCov Measurement innovation covariance
+	 */
+	void getInnovations(float &innov, float &innovCov);
+
+private:
+	matrix::Vector<float, 2> _x; // state
+
+	matrix::Matrix<float, 2, 2> _covariance; // state covariance
+
+	float _residual; // residual of last measurement update
+
+	float _innovCov; // innovation covariance of last measurement update
+};
+} // namespace landing_target_estimator

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file LandingTargetEstimator.cpp
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ * @author Mohammed Kabir <kabir@uasys.io>
+ *
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <drivers/drv_hrt.h>
+
+#include "LandingTargetEstimator.h"
+
+#define SEC2USEC 1000000.0f
+
+
+namespace landing_target_estimator
+{
+
+LandingTargetEstimator::LandingTargetEstimator() :
+	_targetPosePub(nullptr),
+	_targetInnovationsPub(nullptr),
+	_paramHandle(),
+	_vehicleLocalPosition_valid(false),
+	_vehicleAttitude_valid(false),
+	_sensorBias_valid(false),
+	_new_irlockReport(false),
+	_estimator_initialized(false),
+	_faulty(false),
+	_last_predict(0),
+	_last_update(0)
+{
+	_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
+	_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
+	_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
+	_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
+	_paramHandle.mode = param_find("LTEST_MODE");
+	_paramHandle.scale_x = param_find("LTEST_SCALE_X");
+	_paramHandle.scale_y = param_find("LTEST_SCALE_Y");
+
+	// Initialize uORB topics.
+	_initialize_topics();
+
+	_check_params(true);
+}
+
+LandingTargetEstimator::~LandingTargetEstimator()
+{
+}
+
+void LandingTargetEstimator::update()
+{
+	_check_params(false);
+
+	_update_topics();
+
+	/* predict */
+	if (_estimator_initialized) {
+		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
+			PX4_WARN("Timeout");
+			_estimator_initialized = false;
+
+		} else if (_vehicleLocalPosition_valid
+			   && _vehicleLocalPosition.v_xy_valid) {
+			float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
+
+			// predict target position with the help of accel data
+			matrix::Vector3f a;
+
+			if (_sensorBias_valid) {
+				matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
+				_R_att = matrix::Dcm<float>(q_att);
+				a(0) = _sensorBias.accel_x;
+				a(1) = _sensorBias.accel_y;
+				a(2) = _sensorBias.accel_z;
+				a = _R_att * a;
+
+			} else {
+				a.zero();
+			}
+
+			_kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
+			_kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
+
+			_last_predict = hrt_absolute_time();
+		}
+	}
+
+	if (!_new_irlockReport) {
+		// nothing to do
+		return;
+	}
+
+	// mark this sensor measurement as consumed
+	_new_irlockReport = false;
+
+	if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
+		// don't have the data needed for an update
+		return;
+	}
+
+	if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
+		return;
+	}
+
+	// TODO account for sensor orientation as set by parameter
+	// default orientation has camera x pointing in body y, camera y in body -x
+
+	matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
+	sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward
+	sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right
+	sensor_ray(2) = 1.0f;
+
+	// rotate the unit ray into the navigation frame, assume sensor frame = body frame
+	matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
+	_R_att = matrix::Dcm<float>(q_att);
+	sensor_ray = _R_att * sensor_ray;
+
+	if (fabsf(sensor_ray(2)) < 1e-6f) {
+		// z component of measurement unsafe, don't use this measurement
+		return;
+	}
+
+	float dist = _vehicleLocalPosition.dist_bottom;
+
+	// scale the ray s.t. the z component has length of dist
+	_rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
+	_rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
+
+	if (!_estimator_initialized) {
+		PX4_INFO("Init");
+		_kalman_filter_x.init(_rel_pos(0), 0, _params.pos_unc_init, _params.vel_unc_init);
+		_kalman_filter_y.init(_rel_pos(1), 0, _params.pos_unc_init, _params.vel_unc_init);
+
+		_estimator_initialized = true;
+		_last_update = hrt_absolute_time();
+		_last_predict = _last_update;
+
+	} else {
+		// update
+		bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist);
+		bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist);
+
+		if (!update_x || !update_y) {
+			if (!_faulty) {
+				_faulty = true;
+				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
+			}
+
+		} else {
+			_faulty = false;
+		}
+
+		if (!_faulty) {
+			// only publish if both measurements were good
+
+			_target_pose.timestamp = _irlockReport.timestamp;
+
+			float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
+			_kalman_filter_x.getState(x, xvel);
+			_kalman_filter_x.getCovariance(covx, covx_v);
+
+			_kalman_filter_y.getState(y, yvel);
+			_kalman_filter_y.getCovariance(covy, covy_v);
+
+			_target_pose.is_static = (_params.mode == TargetMode::Stationary);
+
+			_target_pose.rel_pos_valid = true;
+			_target_pose.rel_vel_valid = true;
+			_target_pose.x_rel = x;
+			_target_pose.y_rel = y;
+			_target_pose.z_rel = dist;
+			_target_pose.vx_rel = xvel;
+			_target_pose.vy_rel = yvel;
+
+			_target_pose.cov_x_rel = covx;
+			_target_pose.cov_y_rel = covy;
+
+			_target_pose.cov_vx_rel = covx_v;
+			_target_pose.cov_vy_rel = covy_v;
+
+			if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
+				_target_pose.x_abs = x + _vehicleLocalPosition.x;
+				_target_pose.y_abs = y + _vehicleLocalPosition.y;
+				_target_pose.z_abs = dist + _vehicleLocalPosition.z;
+				_target_pose.abs_pos_valid = true;
+
+			} else {
+				_target_pose.abs_pos_valid = false;
+			}
+
+			if (_targetPosePub == nullptr) {
+				_targetPosePub = orb_advertise(ORB_ID(landing_target_pose), &_target_pose);
+
+			} else {
+				orb_publish(ORB_ID(landing_target_pose), _targetPosePub, &_target_pose);
+			}
+
+			_last_update = hrt_absolute_time();
+			_last_predict = _last_update;
+		}
+
+		float innov_x, innov_cov_x, innov_y, innov_cov_y;
+		_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
+		_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
+
+		_target_innovations.timestamp = _irlockReport.timestamp;
+		_target_innovations.innov_x = innov_x;
+		_target_innovations.innov_cov_x = innov_cov_x;
+		_target_innovations.innov_y = innov_y;
+		_target_innovations.innov_cov_y = innov_cov_y;
+
+		if (_targetInnovationsPub == nullptr) {
+			_targetInnovationsPub = orb_advertise(ORB_ID(landing_target_innovations), &_target_innovations);
+
+		} else {
+			orb_publish(ORB_ID(landing_target_innovations), _targetInnovationsPub, &_target_innovations);
+		}
+	}
+
+}
+
+void LandingTargetEstimator::_check_params(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		_update_params();
+	}
+}
+
+void LandingTargetEstimator::_initialize_topics()
+{
+	// subscribe to position, attitude, arming and velocity changes
+	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias));
+	_irlockReportSub = orb_subscribe(ORB_ID(irlock_report));
+	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
+}
+
+void LandingTargetEstimator::_update_topics()
+{
+	_vehicleLocalPosition_valid = _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub,
+				      &_vehicleLocalPosition);
+	_vehicleAttitude_valid = _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
+	_sensorBias_valid = _orb_update(ORB_ID(sensor_bias), _sensorBiasSub, &_sensorBias);
+
+	_new_irlockReport = _orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport);
+}
+
+
+bool LandingTargetEstimator::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	bool newData = false;
+
+	// check if there is new data to grab
+	if (orb_check(handle, &newData) != OK) {
+		return false;
+	}
+
+	if (!newData) {
+		return false;
+	}
+
+	if (orb_copy(meta, handle, buffer) != OK) {
+		return false;
+	}
+
+	return true;
+}
+
+void LandingTargetEstimator::_update_params()
+{
+	param_get(_paramHandle.acc_unc, &_params.acc_unc);
+	param_get(_paramHandle.meas_unc, &_params.meas_unc);
+	param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init);
+	param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init);
+	int mode = 0;
+	param_get(_paramHandle.mode, &mode);
+	_params.mode = (TargetMode)mode;
+	param_get(_paramHandle.scale_x, &_params.scale_x);
+	param_get(_paramHandle.scale_y, &_params.scale_y);
+}
+
+
+} // namespace landing_target_estimator

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -0,0 +1,173 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file LandingTargetEstimator.h
+ * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor.
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ * @author Mohammed Kabir <kabir@uasys.io>
+ *
+ */
+
+#pragma once
+
+#include <px4_workqueue.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/param/param.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/irlock_report.h>
+#include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/landing_target_innovations.h>
+#include <uORB/topics/parameter_update.h>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <matrix/Matrix.hpp>
+#include "KalmanFilter.h"
+
+
+namespace landing_target_estimator
+{
+
+class LandingTargetEstimator
+{
+public:
+
+	LandingTargetEstimator();
+	virtual ~LandingTargetEstimator();
+
+	/*
+	 * Get new measurements and update the state estimate
+	 */
+	void update();
+
+protected:
+	/*
+	 * Called once to initialize uORB topics.
+	 */
+	void _initialize_topics();
+
+	/*
+	 * Update uORB topics.
+	 */
+	void _update_topics();
+
+	/*
+	 * Update parameters.
+	 */
+	void _update_params();
+
+	/*
+	 * Convenience function for polling uORB subscriptions.
+	 *
+	 * @return true if there was new data and it was successfully copied
+	 */
+	static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+
+	/* timeout after which filter is reset if target not seen */
+	static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
+
+	orb_advert_t _targetPosePub;
+	struct landing_target_pose_s _target_pose;
+
+	orb_advert_t _targetInnovationsPub;
+	struct landing_target_innovations_s _target_innovations;
+
+	int _parameterSub;
+
+private:
+
+	enum class TargetMode {
+		Moving = 0,
+		Stationary
+	};
+
+	/**
+	* Handles for parameters
+	**/
+	struct {
+		param_t acc_unc;
+		param_t meas_unc;
+		param_t pos_unc_init;
+		param_t vel_unc_init;
+		param_t mode;
+		param_t scale_x;
+		param_t scale_y;
+	} _paramHandle;
+
+	struct {
+		float acc_unc;
+		float meas_unc;
+		float pos_unc_init;
+		float vel_unc_init;
+		TargetMode mode;
+		float scale_x;
+		float scale_y;
+	} _params;
+
+	int _vehicleLocalPositionSub;
+	int _attitudeSub;
+	int _sensorBiasSub;
+	int _irlockReportSub;
+
+	struct vehicle_local_position_s	_vehicleLocalPosition;
+	struct vehicle_attitude_s	_vehicleAttitude;
+	struct sensor_bias_s		_sensorBias;
+	struct irlock_report_s		_irlockReport;
+
+	// keep track of which topics we have received
+	bool _vehicleLocalPosition_valid;
+	bool _vehicleAttitude_valid;
+	bool _sensorBias_valid;
+	bool _new_irlockReport;
+	bool _estimator_initialized;
+	// keep track of whether last measurement was rejected
+	bool _faulty;
+
+	matrix::Dcm<float> _R_att;
+	matrix::Vector<float, 2> _rel_pos;
+	KalmanFilter _kalman_filter_x;
+	KalmanFilter _kalman_filter_y;
+	hrt_abstime _last_predict; // timestamp of last filter prediction
+	hrt_abstime _last_update; // timestamp of last filter update (used to check timeout)
+
+	void _check_params(const bool force);
+
+	void _update_state();
+};
+
+
+} // namespace landing_target_estimator

src/modules/landing_target_estimator/landing_target_estimator_main.cpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file landing_target_estimator_main.cpp
+ * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor.
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ * @author Mohammed Kabir <kabir@uasys.io>
+ *
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+#include "LandingTargetEstimator.h"
+
+
+namespace landing_target_estimator
+{
+
+static bool thread_should_exit = false;	/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;			/**< Handle of daemon task / thread */
+
+/* Run main loop at this rate in Hz. */
+static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50;
+
+/**
+ * Landing target position estimator app start / stop handling function
+ * This makes the module accessible from the nuttx shell
+ * @ingroup apps
+ */
+extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int landing_target_estimator_thread_main(int argc, char *argv[]);
+
+/**
+* Main entry point for this module
+**/
+int landing_target_estimator_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		goto exiterr;
+	}
+
+	if (argc >= 2 && !strcmp(argv[1], "start")) {
+		if (thread_running) {
+			PX4_INFO("already running");
+			/* this is not an error */
+			return 0;
+		}
+
+		thread_should_exit = false;
+		daemon_task = px4_task_spawn_cmd("landing_target_estimator",
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT,
+						 2000,
+						 landing_target_estimator_thread_main,
+						 (argv) ? (char *const *)&argv[2] : nullptr);
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+
+		if (!thread_running) {
+			PX4_WARN("landing_target_estimator not running");
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			PX4_INFO("running");
+
+		} else {
+			PX4_INFO("not started");
+		}
+
+		return 0;
+	}
+
+exiterr:
+	PX4_WARN("usage: landing_target_estimator {start|stop|status}");
+	return 1;
+}
+
+int landing_target_estimator_thread_main(int argc, char *argv[])
+{
+	PX4_DEBUG("starting");
+
+	thread_running = true;
+
+	LandingTargetEstimator est;
+
+	while (!thread_should_exit) {
+		est.update();
+		usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ);
+	}
+
+	PX4_DEBUG("exiting");
+
+	thread_running = false;
+
+	return 0;
+}
+
+}

src/modules/landing_target_estimator/landing_target_estimator_params.c

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014-2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file landing_target_estimator_params.c
+ * Landing target estimator algorithm parameters.
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ * @author Mohammed Kabir <kabir@uasys.io>
+ *
+ */
+
+/**
+ * Landing target mode
+ *
+ * Configure the mode of the landing target. Depending on the mode, the landing target observations are used differently to aid position estimation.
+ *
+ * Mode Moving:     The landing target may be moving around while in the field of view of the vehicle. Landing target measurements are not used to aid positioning.
+ * Mode Stationary: The landing target is stationary. Measured velocity w.r.t. the landing target is used to aid velocity estimation.
+ *
+ * @min 0
+ * @max 1
+ * @group Landing target Estimator
+ * @value 0 Moving
+ * @value 1 Stationary
+ */
+PARAM_DEFINE_INT32(LTEST_MODE, 0);
+
+/**
+ * Acceleration uncertainty
+ *
+ * Variance of acceleration measurement used for landing target position prediction.
+ * Higher values results in tighter following of the measurements and more lenient outlier rejection
+ *
+ * @unit (m/s^2)^2
+ * @min 0.01
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f);
+
+/**
+ * Landing target measurement uncertainty
+ *
+ * Variance of the landing target measurement from the driver.
+ * Higher values results in less agressive following of the measurement and a smoother output as well as fewer rejected measurements.
+ *
+ * @unit tan(rad)^2
+ * @decimal 4
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f);
+
+/**
+ * Initial landing target position uncertainty
+ *
+ * Initial variance of the relative landing target position in x and y direction
+ *
+ * @unit m^2
+ * @min 0.001
+ * @decimal 3
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f);
+
+/**
+ * Initial landing target velocity uncertainty
+ *
+ * Initial variance of the relative landing target velocity in x and y direction
+ *
+ * @unit (m/s)^2
+ * @min 0.001
+ * @decimal 3
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 1.0f);
+
+/**
+ * Scale factor for sensor measurements in sensor x axis
+ *
+ * Landing target x measurements are scaled by this factor before being used
+ *
+ * @min 0.01
+ * @decimal 3
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f);
+
+/**
+ * Scale factor for sensor measurements in sensor y axis
+ *
+ * Landing target y measurements are scaled by this factor before being used
+ *
+ * @min 0.01
+ * @decimal 3
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f);