PositionFilter.cpp
Go to the documentation of this file.
1 /************************************************************************
2  * Copyright (C) 2012 Eindhoven University of Technology (TU/e). *
3  * All rights reserved. *
4  ************************************************************************
5  * Redistribution and use in source and binary forms, with or without *
6  * modification, are permitted provided that the following conditions *
7  * are met: *
8  * *
9  * 1. Redistributions of source code must retain the above *
10  * copyright notice, this list of conditions and the following *
11  * disclaimer. *
12  * *
13  * 2. Redistributions in binary form must reproduce the above *
14  * copyright notice, this list of conditions and the following *
15  * disclaimer in the documentation and/or other materials *
16  * provided with the distribution. *
17  * *
18  * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR *
19  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
20  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
21  * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE *
22  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR *
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT *
24  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; *
25  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
26  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE *
28  * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
29  * DAMAGE. *
30  * *
31  * The views and conclusions contained in the software and *
32  * documentation are those of the authors and should not be *
33  * interpreted as representing official policies, either expressed or *
34  * implied, of TU/e. *
35  ************************************************************************/
36 
37 #include "PositionFilter.h"
38 #include "KalmanFilter.h"
39 
40 PositionFilter::PositionFilter() : t_last_update_(0), t_last_propagation_(0), kalman_filter_(0),
41  fixed_pdf_(0), max_acceleration_(0), fixed_pdf_cov_(0), kalman_timeout_(0) {
42 }
43 
44 PositionFilter::PositionFilter(const PositionFilter& orig) : mhf::IStateEstimator(orig), t_last_update_(orig.t_last_update_),
47 
48  if (orig.fixed_pdf_) {
49  fixed_pdf_ = orig.fixed_pdf_->clone();
50  }
51 
52  if (orig.kalman_filter_) {
54  }
55 }
56 
58  delete kalman_filter_;
59  delete fixed_pdf_;
60 }
61 
63  return new PositionFilter(*this);
64 }
65 
67  if (t_last_propagation_ == 0) {
68  t_last_propagation_ = time;
69  return;
70  }
71 
73  t_last_propagation_ = time;
74 
75  assert(dt >= 0);
76 
77  if (!kalman_filter_) {
78  return;
79  }
80 
81  if ((time - t_last_update_) > kalman_timeout_ && kalman_timeout_ > 0) {
82  //if ((time - t_last_update_) > 10.0) {
83  if (!fixed_pdf_) {
84  int dimensions = kalman_filter_->getGaussian().getMean().n_rows;
85  pbl::Matrix cov = arma::eye(dimensions, dimensions) * fixed_pdf_cov_;
87  } else {
89  }
90 
91  delete kalman_filter_;
92  kalman_filter_ = 0;
93  return;
94  }
95 
96  // TODO: fix the kalman filter update (we shouldn't need a loop here...)
97  mhf::Duration small_dt = 0.05;
98  if (dt < small_dt) {
100  } else {
101  double total_dt = 0;
102  for(; total_dt < dt; total_dt += small_dt) {
103  kalman_filter_->propagate(small_dt);
104  }
105  if (total_dt < dt) {
106  kalman_filter_->propagate(dt - total_dt);
107  }
108  }
109 }
110 
111 void PositionFilter::update(const pbl::PDF& z, const mhf::Time& time) {
112  t_last_update_ = time;
113 
114  if (z.type() == pbl::PDF::GAUSSIAN) {
115  const pbl::Gaussian* G = pbl::PDFtoGaussian(z);
116 
117  if (!kalman_filter_) {
120  kalman_filter_->init(*G);
121  } else {
122  kalman_filter_->update(*G);
123  }
124  } else {
125  printf("PositionFilter can only be updated with Gaussians.\n");
126  }
127 }
128 
130  delete kalman_filter_;
131  kalman_filter_ = 0;
132 
133  delete fixed_pdf_;
134  fixed_pdf_ = 0;
135 }
136 
138  if (kalman_filter_) {
139  return kalman_filter_->getGaussian();
140  } else if (fixed_pdf_) {
141  return *fixed_pdf_;
142  }
143 
144  std::cout << "SOMETHINGS WRONG" << std::endl;
145 }
146 
147 bool PositionFilter::setParameter(const std::string& param, bool b) {
148  return false;
149 }
150 
151 bool PositionFilter::setParameter(const std::string &param, double v) {
152  if (param == "max_acceleration") {
153  max_acceleration_ = v;
154  if (kalman_filter_) {
156  }
157  } else if (param == "fixed_pdf_cov") {
158  fixed_pdf_cov_ = v;
159  } else if (param == "kalman_timeout") {
160  kalman_timeout_ = v;
161  } else {
162  return false;
163  }
164  return true;
165 }
166 
void setMaxAcceleration(double a_max)
Set the maximum expected acceleration. This parameter is used to determine the system noise in the pr...
int dimensions() const
PDFType type() const
mhf::Time t_last_update_
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 ...
Definition: KalmanFilter.h:46
virtual void init(const pbl::Gaussian &G)
Initializes the Kalman filter according to the given Gaussian, i.e., the initial state and state cova...
double Time
virtual void propagate(const mhf::Time &time)
Propagates the internal state to Time time.
double Duration
double kalman_timeout_
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 &param, 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.
double max_acceleration_
Gaussian * clone() const
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)
arma::mat Matrix
const pbl::PDF & getValue() const
Returns the current estimated state value.
const Gaussian * PDFtoGaussian(const PDF &pdf)
pbl::Gaussian * fixed_pdf_


wire_state_estimators
Author(s): Sjoerd van den Dries, Jos Elfring
autogenerated on Fri Apr 16 2021 02:32:34