histogramfilter.cpp
Go to the documentation of this file.
00001 // $Id: histogramfilter.cpp 14935 2007-12-17  $
00002 // Copyright (C) 2007 Tinne De Laet  <tinne dot delaet at mech dot kuleuven dot be>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 //
00018 #include "histogramfilter.h"
00019 #include <cmath>
00020 #include <iostream>
00021 
00022 
00023   template <typename MeasVar>
00024   HistogramFilter<MeasVar>::HistogramFilter(DiscretePdf * prior)
00025     : Filter<int,MeasVar>(prior)
00026   {
00027     // create posterior density
00028     this->_post = new DiscretePdf(*prior);
00029     // Initialise lists of probabilities
00030     _old_prob = (prior->ProbabilitiesGet());
00031     _new_prob = _old_prob;
00032   }
00033 
00034   template <typename MeasVar>
00035   HistogramFilter<MeasVar>::~HistogramFilter()
00036   {
00037     delete this->_post;
00038   }
00039 
00040   template <typename MeasVar>
00041   void
00042   HistogramFilter<MeasVar>::SysUpdate(SystemModel<int>* const sysmodel, const int& u)
00043   {
00044     bool without_inputs = sysmodel->SystemWithoutInputs();
00045     int num_states = ( (DiscretePdf*)this->_post )->NumStatesGet();
00046     _old_prob = ( (DiscretePdf*)this->_post )->ProbabilitiesGet();
00047 
00048     for (int to_state = 0; to_state< num_states ; to_state++)
00049     {
00050         double temp = 0;
00051         if (without_inputs)
00052         {
00053             for (int from_state = 0; from_state< num_states ; from_state++)
00054             {
00055                 temp = temp + (double)sysmodel->ProbabilityGet(to_state,from_state) * _old_prob[from_state];
00056             }
00057         }
00058         else
00059         {
00060             for (int from_state = 0; from_state< num_states ; from_state++)
00061             {
00062                 temp = temp + (double)sysmodel->ProbabilityGet(to_state,from_state,u) * _old_prob[from_state];
00063             }
00064         }
00065        _new_prob[to_state] = temp;
00066     }
00067     ( (DiscretePdf*)this->_post )->ProbabilitiesSet(_new_prob);
00068   }
00069 
00070   template <typename MeasVar>
00071   void
00072   HistogramFilter<MeasVar>::MeasUpdate(MeasurementModel<MeasVar,int>* const measmodel, const MeasVar& z, const int& s)
00073   {
00074       int num_states = ( (DiscretePdf*)this->_post )->NumStatesGet();
00075       _old_prob = ( (DiscretePdf*)this->_post )->ProbabilitiesGet();
00076       for (int state = 0; state< num_states  ; state++)
00077       {
00078           if (measmodel->SystemWithoutSensorParams() == true)  _old_prob[state] = _old_prob[state] * measmodel->ProbabilityGet(z,state);
00079           else _old_prob[state] = _old_prob[state] * measmodel->ProbabilityGet(z,state,s);
00080           _old_prob[state] = (Probability)((double)( _old_prob[state]) );
00081       }
00082       ( (DiscretePdf*)this->_post )->ProbabilitiesSet(_old_prob);
00083   }
00084 
00085   template <typename MeasVar>
00086   bool
00087   HistogramFilter<MeasVar>::UpdateInternal(SystemModel<int>* const sysmodel,
00088                                const int& u,
00089                                MeasurementModel<MeasVar,int>* const measmodel,
00090                                const MeasVar& z, const int& s)
00091   {
00092     if (sysmodel != NULL)       SysUpdate(sysmodel,u);
00093     if (measmodel != NULL)      MeasUpdate(measmodel,z,s);
00094     return true;
00095   }
00096 
00097   template <typename MeasVar>
00098   DiscretePdf*
00099   HistogramFilter<MeasVar>::PostGet()
00100   {
00101     return (DiscretePdf*)Filter<int,MeasVar>::PostGet();
00102   }


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Thu Feb 11 2016 22:31:56