$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 "MultiModelFilter.h" 00038 #include "KalmanFilter.h" 00039 00040 #include "PositionFilter.h" 00041 00042 MultiModelFilter::MultiModelFilter() : initialized_(false) { 00043 PositionFilter* e1 = new PositionFilter(); 00044 e1->setParameter("max_acceleration", 0.0); 00045 00046 PositionFilter* e2 = new PositionFilter(); 00047 e2->setParameter("max_acceleration", 8.0); 00048 00049 addEstimator(e1); 00050 addEstimator(e2); 00051 } 00052 00053 MultiModelFilter::MultiModelFilter(const MultiModelFilter& orig) : IStateEstimator(orig), initialized_(orig.initialized_), 00054 mixture_(orig.mixture_) { 00055 for(unsigned int i = 0; i < orig.estimators_.size(); ++i) { 00056 estimators_.push_back(orig.estimators_[i]->clone()); 00057 weights_.push_back(orig.weights_[i]); 00058 } 00059 } 00060 00061 MultiModelFilter::~MultiModelFilter() { 00062 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00063 delete estimators_[i]; 00064 } 00065 } 00066 00067 MultiModelFilter* MultiModelFilter::clone() const { 00068 return new MultiModelFilter(*this); 00069 } 00070 00071 void MultiModelFilter::addEstimator(mhf::IStateEstimator* estimator) { 00072 estimators_.push_back(estimator); 00073 00074 weights_.resize(estimators_.size()); 00075 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00076 weights_[i] = 1 / (double)estimators_.size(); 00077 } 00078 } 00079 00080 void MultiModelFilter::propagate(const mhf::Time& time) { 00081 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00082 estimators_[i]->propagate(time); 00083 } 00084 00085 // propagate weights (TODO: use transition matrix here) 00086 double w0 = 0.5 * weights_[0] + 0.5 * weights_[1]; 00087 double w1 = 0.5 * weights_[0] + 0.5 * weights_[1]; 00088 00089 weights_[0] = w0; 00090 weights_[1] = w1; 00091 } 00092 00093 void MultiModelFilter::update(const pbl::PDF& z, const mhf::Time& time) { 00094 if (!initialized_) { 00095 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00096 estimators_[i]->update(z, time); 00097 } 00098 initialized_ = true; 00099 return; 00100 } 00101 00102 // update weights based on likelihoods 00103 double total_weight = 0; 00104 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00105 weights_[i] = weights_[i] * estimators_[i]->getValue().getLikelihood(z); 00106 total_weight += weights_[i]; 00107 } 00108 00109 // normalize weights and find best estimator 00110 unsigned int i_best = 0; 00111 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00112 weights_[i] /= total_weight; 00113 if (weights_[i] > weights_[i_best]) { 00114 i_best = i; 00115 } 00116 } 00117 00118 // update most probable estimator with the evidence, set others to 0 weight 00119 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00120 if (i == i_best) { 00121 weights_[i] = 1; 00122 } else { 00123 weights_[i] = 0; 00124 estimators_[i]->reset(); 00125 } 00126 estimators_[i]->update(z, time); 00127 } 00128 } 00129 00130 void MultiModelFilter::reset() { 00131 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00132 estimators_[i]->reset(); 00133 } 00134 } 00135 00136 const pbl::PDF& MultiModelFilter::getValue() const { 00137 mixture_.clear(); 00138 for(unsigned int i = 0; i < estimators_.size(); ++i) { 00139 mixture_.addComponent(estimators_[i]->getValue(), weights_[i]); 00140 } 00141 mixture_.normalizeWeights(); 00142 return mixture_; 00143 } 00144 00145 void MultiModelFilter::setValue(const pbl::PDF& pdf) { 00146 00147 } 00148 00149 bool MultiModelFilter::setParameter(const std::string& param, bool b) { 00150 return false; 00151 } 00152 00153 bool MultiModelFilter::setParameter(const std::string ¶m, double v) { 00154 return true; 00155 } 00156 00157 #include <pluginlib/class_list_macros.h> 00158 PLUGINLIB_DECLARE_CLASS( wire_state_estimators, MultiModelEstimator, MultiModelFilter, mhf::IStateEstimator )