LTE: advertise published topic early to be logged

src/modules/landing_target_estimator/KalmanFilter.cpp

@@ -43,30 +43,8 @@
 
 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)
+void KalmanFilter::predict(float dt, float acc)
 {
 	_x(0) += _x(1) * dt + dt * dt / 2 * acc;
 	_x(1) += acc * dt;
@@ -80,12 +58,12 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc)
 	G(0, 0) = dt * dt / 2;
 	G(1, 0) = dt;
 
-	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
+	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * _acc_var;
 
 	_covariance = A * _covariance * A.transpose() + process_noise;
 }
 
-bool KalmanFilter::update(float meas, float measUnc)
+bool KalmanFilter::fusePosition(float meas, float measUnc)
 {
 
 	// H = [1, 0]
@@ -122,28 +100,6 @@ bool KalmanFilter::update(float meas, float measUnc)
 
 }
 
-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;

src/modules/landing_target_estimator/KalmanFilter.h

@@ -51,11 +51,13 @@
 #include <matrix/Matrix.hpp>
 #include <matrix/Vector.hpp>
 
+#include "target_estimator.h"
+
 #pragma once
 
 namespace landing_target_estimator
 {
-class KalmanFilter
+class KalmanFilter : public TargetEstimator
 {
 public:
 	/**
@@ -63,31 +65,13 @@ public:
 	 */
 	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);
+	void setPosition(float pos) override { _x(0) = pos; }
+	void setVelocity(float vel) override { _x(1) = vel; }
 
 	/**
 	 * Predict the state with an external acceleration estimate
@@ -95,7 +79,7 @@ public:
 	 * @param acc           Acceleration estimate
 	 * @param acc_unc       Variance of acceleration estimate
 	 */
-	void predict(float dt, float acc, float acc_unc);
+	void predict(float dt, float acc) override;
 
 	/**
 	 * Update the state estimate with a measurement
@@ -103,33 +87,13 @@ public:
 	 * @param measUnc measurement uncertainty
 	 * @return update success (measurement not rejected)
 	 */
-	bool update(float meas, float measUnc);
+	bool fusePosition(float meas, float measUnc) override;
 
-	/**
-	 * Get the current filter state
-	 * @param x1 State
-	 */
-	void getState(matrix::Vector<float, 2> &state);
+	float getPosition() override { return _x(0); }
+	float getVelocity() override { return _x(1); }
 
-	/**
-	 * 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);
+	float getPosVar() override { return _covariance(0, 0); }
+	float getVelVar() override { return _covariance(0, 0); }
 
 	/**
 	 * Get measurement innovation and covariance of last update call

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -50,26 +50,29 @@
 namespace landing_target_estimator
 {
 
-LandingTargetEstimator::LandingTargetEstimator()
+using namespace matrix;
+
+LandingTargetEstimator::LandingTargetEstimator() :
+	ModuleParams(nullptr)
 {
-	_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");
-	_paramHandle.sensor_yaw = param_find("LTEST_SENS_ROT");
-	_paramHandle.offset_x = param_find("LTEST_SENS_POS_X");
-	_paramHandle.offset_y = param_find("LTEST_SENS_POS_Y");
-	_paramHandle.offset_z = param_find("LTEST_SENS_POS_Z");
+	_targetPosePub.advertise();
 	_check_params(true);
 }
 
+LandingTargetEstimator::~LandingTargetEstimator()
+{
+	delete _target_estimator[0];
+	delete _target_estimator[1];
+}
+
 void LandingTargetEstimator::update()
 {
 	_check_params(false);
 
+	if ((_target_estimator[0] == nullptr) || (_target_estimator[1] == nullptr)) {
+		return;
+	}
+
 	_update_topics();
 
 	/* predict */
@@ -93,8 +96,8 @@ void LandingTargetEstimator::update()
 				a.zero();
 			}
 
-			_kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
-			_kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
+			_target_estimator[0]->predict(dt, -a(0));
+			_target_estimator[1]->predict(dt, -a(1));
 
 			_last_predict = hrt_absolute_time();
 		}
@@ -110,11 +113,20 @@ void LandingTargetEstimator::update()
 
 
 	if (!_estimator_initialized) {
-		float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
-		float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
-		PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
-		_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init);
-		_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init);
+		Vector2f v_init;
+		v_init(0) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
+		v_init(1) = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
+
+		Vector2f p_init(_target_position_report.rel_pos_x, _target_position_report.rel_pos_y);
+
+		PX4_INFO("Init %.2f %.2f", (double)v_init(0), (double)v_init(1));
+
+		for (int i = 0; i < 2; i++) {
+			_target_estimator[i]->setPosition(p_init(i));
+			_target_estimator[i]->setVelocity(v_init(i));
+			_target_estimator[i]->setStatePosVar(_param_ltest_pos_unc_in.get());
+			_target_estimator[i]->setStateVelVar(_param_ltest_vel_unc_in.get());
+		}
 
 		_estimator_initialized = true;
 		_last_update = hrt_absolute_time();
@@ -122,9 +134,9 @@ void LandingTargetEstimator::update()
 
 	} else {
 		// update
-		const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z;
-		bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
-		bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
+		const float measurement_uncertainty = _param_ltest_meas_unc.get() * _dist_z * _dist_z;
+		bool update_x = _target_estimator[0]->fusePosition(_target_position_report.rel_pos_x, measurement_uncertainty);
+		bool update_y = _target_estimator[1]->fusePosition(_target_position_report.rel_pos_y, measurement_uncertainty);
 
 		if (!update_x || !update_y) {
 			if (!_faulty) {
@@ -139,30 +151,26 @@ void LandingTargetEstimator::update()
 		if (!_faulty) {
 			// only publish if both measurements were good
 
+			const float x = _target_estimator[0]->getPosition();
+			const float y = _target_estimator[1]->getPosition();
+
 			_target_pose.timestamp = _target_position_report.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.is_static = ((TargetMode)_param_ltest_mode.get() == 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 = _target_position_report.rel_pos_z ;
-			_target_pose.vx_rel = xvel;
-			_target_pose.vy_rel = yvel;
+			_target_pose.vx_rel = _target_estimator[0]->getVelocity();
+			_target_pose.vy_rel = _target_estimator[1]->getVelocity();
 
-			_target_pose.cov_x_rel = covx;
-			_target_pose.cov_y_rel = covy;
+			_target_pose.cov_x_rel = _target_estimator[0]->getPosVar();
+			_target_pose.cov_y_rel = _target_estimator[1]->getPosVar();
 
-			_target_pose.cov_vx_rel = covx_v;
-			_target_pose.cov_vy_rel = covy_v;
+			_target_pose.cov_vx_rel = _target_estimator[0]->getVelVar();
+			_target_pose.cov_vy_rel = _target_estimator[0]->getVelVar();
 
 			if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
 				_target_pose.x_abs = x + _vehicleLocalPosition.x;
@@ -180,15 +188,16 @@ void LandingTargetEstimator::update()
 			_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);
+		//TODO:fix
+		/* float innov_x, innov_cov_x, innov_y, innov_cov_y; */
+		/* _target_estimator[0]->getInnovations(innov_x, innov_cov_x); */
+		/* _target_estimator[1]->getInnovations(innov_y, innov_cov_y); */
 
-		_target_innovations.timestamp = _target_position_report.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;
+		/* _target_innovations.timestamp = _target_position_report.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; */
 
 		_targetInnovationsPub.publish(_target_innovations);
 	}
@@ -200,7 +209,7 @@ void LandingTargetEstimator::_check_params(const bool force)
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);
 
-		_update_params();
+		updateParams();
 	}
 }
 
@@ -224,12 +233,12 @@ void LandingTargetEstimator::_update_topics()
 		}
 
 		matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
-		sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
-		sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
+		sensor_ray(0) = _irlockReport.pos_x * _param_ltest_scale_x.get(); // forward
+		sensor_ray(1) = _irlockReport.pos_y * _param_ltest_scale_y.get(); // right
 		sensor_ray(2) = 1.0f;
 
 		// rotate unit ray according to sensor orientation
-		_S_att = get_rot_matrix(_params.sensor_yaw);
+		_S_att = get_rot_matrix(static_cast<enum Rotation>(_param_ltest_sens_rot.get()));
 		sensor_ray = _S_att * sensor_ray;
 
 		// rotate the unit ray into the navigation frame
@@ -242,7 +251,7 @@ void LandingTargetEstimator::_update_topics()
 			return;
 		}
 
-		_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
+		_dist_z = _vehicleLocalPosition.dist_bottom - _param_ltest_sens_pos_z.get();
 
 		// scale the ray s.t. the z component has length of _uncertainty_scale
 		_target_position_report.timestamp = _irlockReport.timestamp;
@@ -251,8 +260,8 @@ void LandingTargetEstimator::_update_topics()
 		_target_position_report.rel_pos_z = _dist_z;
 
 		// Adjust relative position according to sensor offset
-		_target_position_report.rel_pos_x += _params.offset_x;
-		_target_position_report.rel_pos_y += _params.offset_y;
+		_target_position_report.rel_pos_x += _param_ltest_sens_pos_x.get();
+		_target_position_report.rel_pos_y += _param_ltest_sens_pos_y.get();
 
 		_new_sensorReport = true;
 
@@ -282,28 +291,48 @@ void LandingTargetEstimator::_update_topics()
 	}
 }
 
-void LandingTargetEstimator::_update_params()
+void LandingTargetEstimator::updateParams()
 {
-	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);
+	int32_t current_target_estimator_mode = _param_ltest_mode.get();
+	ModuleParams::updateParams();
 
-	int32_t mode = 0;
-	param_get(_paramHandle.mode, &mode);
-	_params.mode = (TargetMode)mode;
+	if ((current_target_estimator_mode != _param_ltest_mode.get()) || (_target_estimator[0] == nullptr)
+	    || (_target_estimator[1] == nullptr)) {
+		selectTargetEstimator();
+	}
 
-	param_get(_paramHandle.scale_x, &_params.scale_x);
-	param_get(_paramHandle.scale_y, &_params.scale_y);
-
-	int32_t sensor_yaw = 0;
-	param_get(_paramHandle.sensor_yaw, &sensor_yaw);
-	_params.sensor_yaw = static_cast<enum Rotation>(sensor_yaw);
-
-	param_get(_paramHandle.offset_x, &_params.offset_x);
-	param_get(_paramHandle.offset_y, &_params.offset_y);
-	param_get(_paramHandle.offset_z, &_params.offset_z);
+	_target_estimator[0]->setInputAccVar(_param_ltest_acc_unc.get());
+	_target_estimator[1]->setInputAccVar(_param_ltest_acc_unc.get());
 }
 
+void LandingTargetEstimator::selectTargetEstimator()
+{
+	const TargetMode target_mode = (TargetMode)_param_ltest_mode.get();
+
+	TargetEstimator *tmp_x = nullptr;
+	TargetEstimator *tmp_y = nullptr;
+
+	switch (target_mode) {
+	case TargetMode::Moving:
+		/* tmp = new xxx */
+		break;
+
+	case TargetMode::Stationary:
+		tmp_x = new KalmanFilter();
+		tmp_y = new KalmanFilter();
+		break;
+	}
+
+	if ((tmp_x == nullptr) || (tmp_y == nullptr)) {
+		PX4_ERR("LTE init failed");
+		_param_ltest_mode.set(0);
+
+	} else {
+		delete _target_estimator[0];
+		delete _target_estimator[1];
+		_target_estimator[0] = tmp_x;
+		_target_estimator[1] = tmp_y;
+	}
+}
 
 } // namespace landing_target_estimator

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -42,6 +42,7 @@
 
 #pragma once
 
+#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/workqueue.h>
 #include <drivers/drv_hrt.h>
 #include <parameters/param.h>
@@ -69,12 +70,12 @@ using namespace time_literals;
 namespace landing_target_estimator
 {
 
-class LandingTargetEstimator
+class LandingTargetEstimator: public ModuleParams
 {
 public:
 
 	LandingTargetEstimator();
-	virtual ~LandingTargetEstimator() = default;
+	virtual ~LandingTargetEstimator();
 
 	/*
 	 * Get new measurements and update the state estimate
@@ -91,7 +92,7 @@ protected:
 	/*
 	 * Update parameters.
 	 */
-	void _update_params();
+	void updateParams() override;
 
 	/* timeout after which filter is reset if target not seen */
 	static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
@@ -111,37 +112,6 @@ private:
 		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;
-		param_t offset_x;
-		param_t offset_y;
-		param_t offset_z;
-		param_t sensor_yaw;
-	} _paramHandle;
-
-	struct {
-		float acc_unc;
-		float meas_unc;
-		float pos_unc_init;
-		float vel_unc_init;
-		TargetMode mode;
-		float scale_x;
-		float scale_y;
-		float offset_x;
-		float offset_y;
-		float offset_z;
-		enum Rotation sensor_yaw;
-	} _params;
-
 	struct {
 		hrt_abstime timestamp;
 		float rel_pos_x;
@@ -149,6 +119,8 @@ private:
 		float rel_pos_z;
 	} _target_position_report;
 
+	void selectTargetEstimator();
+
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -175,8 +147,7 @@ private:
 	matrix::Dcmf _R_att; //Orientation of the body frame
 	matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame
 	matrix::Vector2f _rel_pos;
-	KalmanFilter _kalman_filter_x;
-	KalmanFilter _kalman_filter_y;
+	TargetEstimator *_target_estimator[2] {nullptr, nullptr};
 	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
 	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
 	float _dist_z{1.0f};
@@ -184,5 +155,19 @@ private:
 	void _check_params(const bool force);
 
 	void _update_state();
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_ltest_acc_unc,
+		(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_ltest_meas_unc,
+		(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_ltest_pos_unc_in,
+		(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_ltest_vel_unc_in,
+		(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
+		(ParamFloat<px4::params::LTEST_SCALE_X>) _param_ltest_scale_x,
+		(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_ltest_scale_y,
+		(ParamInt<px4::params::LTEST_SENS_ROT>) _param_ltest_sens_rot,
+		(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_ltest_sens_pos_x,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_ltest_sens_pos_y,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_ltest_sens_pos_z
+	)
 };
 } // namespace landing_target_estimator

src/modules/landing_target_estimator/target_estimator.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 target_estimator.cpp
+ * @brief Interface for target estimators
+ */
+
+class TargetEstimator
+{
+public:
+	TargetEstimator() = default;
+	virtual ~TargetEstimator() = default;
+
+	virtual void predict(float dt, float acceleration) = 0;
+	virtual bool fusePosition(float pos, float var) { return true; }
+	virtual bool fuseVeloticy(float pos, float var) { return true; }
+
+	virtual void setPosition(float pos) {};
+	virtual void setVelocity(float vel) {};
+	virtual void setTargetAcc(float acc) {};
+
+	virtual void setInputAccVar(float var) { _acc_var = var; }
+
+	virtual void setStatePosVar(float var) {};
+	virtual void setStateVelVar(float var) {};
+	virtual void setStateAccVar(float var) {};
+
+	virtual float getPosition() { return 0.f; }
+	virtual float getVelocity() { return 0.f; }
+	virtual float getAcceleration() { return 0.f; }
+
+	virtual float getPosVar() = 0;
+	virtual float getVelVar() { return 0.f; }
+	virtual float getAccVar() { return 0.f; }
+
+protected:
+	float _acc_var{};
+};