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 #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
00028 this->_post = new DiscretePdf(*prior);
00029
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 Sun Oct 5 2014 22:29:52