Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "PositionFilter.h"
00038 #include "KalmanFilter.h"
00039
00040 PositionFilter::PositionFilter() : t_last_update_(0), t_last_propagation_(0), kalman_filter_(0),
00041 fixed_pdf_(0), max_acceleration_(0), fixed_pdf_cov_(0), kalman_timeout_(0) {
00042 }
00043
00044 PositionFilter::PositionFilter(const PositionFilter& orig) : IStateEstimator(orig), t_last_update_(orig.t_last_update_),
00045 t_last_propagation_(orig.t_last_propagation_), kalman_filter_(0), fixed_pdf_(0), max_acceleration_(orig.max_acceleration_),
00046 fixed_pdf_cov_(orig.fixed_pdf_cov_ ), kalman_timeout_(orig.kalman_timeout_) {
00047
00048 if (orig.fixed_pdf_) {
00049 fixed_pdf_ = orig.fixed_pdf_->clone();
00050 }
00051
00052 if (orig.kalman_filter_) {
00053 kalman_filter_ = new KalmanFilter(*orig.kalman_filter_);
00054 }
00055 }
00056
00057 PositionFilter::~PositionFilter() {
00058 delete kalman_filter_;
00059 delete fixed_pdf_;
00060 }
00061
00062 PositionFilter* PositionFilter::clone() const {
00063 return new PositionFilter(*this);
00064 }
00065
00066 void PositionFilter::propagate(const mhf::Time& time) {
00067 if (t_last_propagation_ == 0) {
00068 t_last_propagation_ = time;
00069 return;
00070 }
00071
00072 mhf::Duration dt = time - t_last_propagation_;
00073 t_last_propagation_ = time;
00074
00075 assert(dt >= 0);
00076
00077 if (!kalman_filter_) {
00078 return;
00079 }
00080
00081 if ((time - t_last_update_) > kalman_timeout_ && kalman_timeout_ > 0) {
00082
00083 if (!fixed_pdf_) {
00084 int dimensions = kalman_filter_->getGaussian().getMean().n_rows;
00085 pbl::Matrix cov = arma::eye(dimensions, dimensions) * fixed_pdf_cov_;
00086 fixed_pdf_ = new pbl::Gaussian(kalman_filter_->getGaussian().getMean(), cov);
00087 } else {
00088 fixed_pdf_->setMean(kalman_filter_->getGaussian().getMean());
00089 }
00090
00091 delete kalman_filter_;
00092 kalman_filter_ = 0;
00093 return;
00094 }
00095
00096
00097 mhf::Duration small_dt = 0.05;
00098 if (dt < small_dt) {
00099 kalman_filter_->propagate(dt);
00100 } else {
00101 double total_dt = 0;
00102 for(; total_dt < dt; total_dt += small_dt) {
00103 kalman_filter_->propagate(small_dt);
00104 }
00105 if (total_dt < dt) {
00106 kalman_filter_->propagate(dt - total_dt);
00107 }
00108 }
00109 }
00110
00111 void PositionFilter::update(const pbl::PDF& z, const mhf::Time& time) {
00112 t_last_update_ = time;
00113
00114 if (z.type() == pbl::PDF::GAUSSIAN) {
00115 const pbl::Gaussian* G = pbl::PDFtoGaussian(z);
00116
00117 if (!kalman_filter_) {
00118 kalman_filter_ = new KalmanFilter(z.dimensions());
00119 kalman_filter_->setMaxAcceleration(max_acceleration_);
00120 kalman_filter_->init(*G);
00121 } else {
00122 kalman_filter_->update(*G);
00123 }
00124 } else {
00125 printf("PositionFilter can only be updated with Gaussians.\n");
00126 }
00127 }
00128
00129 void PositionFilter::reset() {
00130 delete kalman_filter_;
00131 kalman_filter_ = 0;
00132
00133 delete fixed_pdf_;
00134 fixed_pdf_ = 0;
00135 }
00136
00137 const pbl::PDF& PositionFilter::getValue() const {
00138 if (kalman_filter_) {
00139 return kalman_filter_->getGaussian();
00140 } else if (fixed_pdf_) {
00141 return *fixed_pdf_;
00142 }
00143
00144 std::cout << "SOMETHINGS WRONG" << std::endl;
00145 }
00146
00147 bool PositionFilter::setParameter(const std::string& param, bool b) {
00148 return false;
00149 }
00150
00151 bool PositionFilter::setParameter(const std::string ¶m, double v) {
00152 if (param == "max_acceleration") {
00153 max_acceleration_ = v;
00154 if (kalman_filter_) {
00155 kalman_filter_->setMaxAcceleration(max_acceleration_);
00156 }
00157 } else if (param == "fixed_pdf_cov") {
00158 fixed_pdf_cov_ = v;
00159 } else if (param == "kalman_timeout") {
00160 kalman_timeout_ = v;
00161 } else {
00162 return false;
00163 }
00164 return true;
00165 }
00166
00167 #include <pluginlib/class_list_macros.h>
00168 PLUGINLIB_DECLARE_CLASS( wire_state_estimators, PositionEstimator, PositionFilter, mhf::IStateEstimator )