$search
00001 /************************************************************************ 00002 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00003 * All rights reserved. * 00004 ************************************************************************ 00005 * Redistribution and use in source and binary forms, with or without * 00006 * modification, are permitted provided that the following conditions * 00007 * are met: * 00008 * * 00009 * 1. Redistributions of source code must retain the above * 00010 * copyright notice, this list of conditions and the following * 00011 * disclaimer. * 00012 * * 00013 * 2. Redistributions in binary form must reproduce the above * 00014 * copyright notice, this list of conditions and the following * 00015 * disclaimer in the documentation and/or other materials * 00016 * provided with the distribution. * 00017 * * 00018 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00019 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00020 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00021 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00022 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00024 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00025 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00026 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00028 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00029 * DAMAGE. * 00030 * * 00031 * The views and conclusions contained in the software and * 00032 * documentation are those of the authors and should not be * 00033 * interpreted as representing official policies, either expressed or * 00034 * implied, of TU/e. * 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 //if ((time - t_last_update_) > 10.0) { 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 // TODO: fix the kalman filter update (we shouldn't need a loop here...) 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 )