41 fixed_pdf_(0), max_acceleration_(0), fixed_pdf_cov_(0), kalman_timeout_(0) {
73 t_last_propagation_ = time;
102 for(; total_dt < dt; total_dt += small_dt) {
125 printf(
"PositionFilter can only be updated with Gaussians.\n");
144 std::cout <<
"SOMETHINGS WRONG" << std::endl;
152 if (param ==
"max_acceleration") {
157 }
else if (param ==
"fixed_pdf_cov") {
159 }
else if (param ==
"kalman_timeout") {
void setMaxAcceleration(double a_max)
Set the maximum expected acceleration. This parameter is used to determine the system noise in the pr...
void update(const pbl::PDF &z, const mhf::Time &time)
Updates the internal state based on measurement z.
virtual void reset()
Resets the internal state of the estimator to its initial value.
virtual ~PositionFilter()
Kalman filter with constant-velocity system model. The system noise is automatically calculated from ...
virtual void init(const pbl::Gaussian &G)
Initializes the Kalman filter according to the given Gaussian, i.e., the initial state and state cova...
virtual void propagate(const mhf::Time &time)
Propagates the internal state to Time time.
void setMean(const arma::vec &mu)
const arma::vec & getMean() const
mhf::Time t_last_propagation_
virtual PositionFilter * clone() const
KalmanFilter * kalman_filter_
const Op< T1, op_cov > cov(const Base< typename T1::elem_type, T1 > &X, const uword norm_type=0)
Estimator specialized in estimating the position of a target. The estimator uses a Kalman filter with...
virtual void propagate(const double &dt)
Propagates the state of the Kalman filter according to the (constant- velocity) system model...
bool setParameter(const std::string ¶m, bool b)
Set a boolean parameter of this state estimator.
const pbl::Gaussian & getGaussian() const
Returns the Kalman state and covariance as Gaussian.
void update(const pbl::Gaussian &z)
Updates the Kalman filter with the given (Gaussian) measurement.
arma_inline const Gen< mat::elem_type, gen_ones_diag > eye(const uword n_rows, const uword n_cols)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
const pbl::PDF & getValue() const
Returns the current estimated state value.
const Gaussian * PDFtoGaussian(const PDF &pdf)
pbl::Gaussian * fixed_pdf_