histogramfilter.cpp
Go to the documentation of this file.
1 // $Id: histogramfilter.cpp 14935 2007-12-17 $
2 // Copyright (C) 2007 Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18 #include "histogramfilter.h"
19 #include <cmath>
20 #include <iostream>
21 
22 
23  template <typename MeasVar>
25  : Filter<int,MeasVar>(prior)
26  {
27  // create posterior density
28  this->_post = new DiscretePdf(*prior);
29  // Initialise lists of probabilities
30  _old_prob = (prior->ProbabilitiesGet());
32  }
33 
34  template <typename MeasVar>
36  {
37  delete this->_post;
38  }
39 
40  template <typename MeasVar>
41  void
43  {
44  bool without_inputs = sysmodel->SystemWithoutInputs();
45  int num_states = ( (DiscretePdf*)this->_post )->NumStatesGet();
46  _old_prob = ( (DiscretePdf*)this->_post )->ProbabilitiesGet();
47 
48  for (int to_state = 0; to_state< num_states ; to_state++)
49  {
50  double temp = 0;
51  if (without_inputs)
52  {
53  for (int from_state = 0; from_state< num_states ; from_state++)
54  {
55  temp = temp + (double)sysmodel->ProbabilityGet(to_state,from_state) * _old_prob[from_state];
56  }
57  }
58  else
59  {
60  for (int from_state = 0; from_state< num_states ; from_state++)
61  {
62  temp = temp + (double)sysmodel->ProbabilityGet(to_state,from_state,u) * _old_prob[from_state];
63  }
64  }
65  _new_prob[to_state] = temp;
66  }
67  ( (DiscretePdf*)this->_post )->ProbabilitiesSet(_new_prob);
68  }
69 
70  template <typename MeasVar>
71  void
73  {
74  int num_states = ( (DiscretePdf*)this->_post )->NumStatesGet();
75  _old_prob = ( (DiscretePdf*)this->_post )->ProbabilitiesGet();
76  for (int state = 0; state< num_states ; state++)
77  {
78  if (measmodel->SystemWithoutSensorParams() == true) _old_prob[state] = _old_prob[state] * measmodel->ProbabilityGet(z,state);
79  else _old_prob[state] = _old_prob[state] * measmodel->ProbabilityGet(z,state,s);
80  _old_prob[state] = (Probability)((double)( _old_prob[state]) );
81  }
82  ( (DiscretePdf*)this->_post )->ProbabilitiesSet(_old_prob);
83  }
84 
85  template <typename MeasVar>
86  bool
88  const int& u,
89  MeasurementModel<MeasVar,int>* const measmodel,
90  const MeasVar& z, const int& s)
91  {
92  if (sysmodel != NULL) SysUpdate(sysmodel,u);
93  if (measmodel != NULL) MeasUpdate(measmodel,z,s);
94  return true;
95  }
96 
97  template <typename MeasVar>
100  {
102  }
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:77
bool UpdateInternal(SystemModel< int > *const sysmodel, const int &u, MeasurementModel< MeasVar, int > *const measmodel, const MeasVar &z, const int &s)
Actual implementation of Update, varies along filters.
Class representing a PDF on a discrete variable.
Definition: discretepdf.h:34
bool SystemWithoutInputs() const
Has the system inputs or not.
Definition: systemmodel.cpp:84
Class representing the histogram filter.
Pdf< int > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
virtual ~HistogramFilter()
Destructor.
virtual Pdf< StateVar > * PostGet()
Get Posterior density.
Definition: filter.cpp:126
Probability ProbabilityGet(const MeasVar &z, const StateVar &x, const StateVar &s)
Get the probability of a certain measurement.
void MeasUpdate(MeasurementModel< MeasVar, int > *const measmodel, const MeasVar &z, const int &s)
Measurement Update.
virtual DiscretePdf * PostGet()
Get Posterior density.
void SysUpdate(SystemModel< int > *const sysmodel, const int &u)
vector< Probability > _old_prob
While updating store list of old probabilities.
vector< Probability > _new_prob
While updating store list of new probabilities.
#define MeasVar
Definition: asirfilter.h:23
Probability ProbabilityGet(const T &x_k, const T &x_kminusone, const T &u)
Get the probability of arriving in a next state.
vector< Probability > ProbabilitiesGet() const
Get all probabilities.
Class representing a probability (a double between 0 and 1)
Definition: bfl_constants.h:39
bool SystemWithoutSensorParams() const
Number of Conditional Arguments.


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 Mon Feb 28 2022 21:56:33