filterproposaldensity.cpp
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
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 
19 #include "filterproposaldensity.h"
20 
21 namespace BFL
22 {
23  using namespace MatrixWrapper;
24 
25 
26 #define FilterPropDens FilterProposalDensity
27 
28  FilterPropDens::FilterPropDens(AnalyticSystemModelGaussianUncertainty * SysModel,
29  AnalyticMeasurementModelGaussianUncertainty * MeasModel)
30  : AnalyticConditionalGaussian(),
31  _sysmodel(SysModel),
32  _measmodel(MeasModel)
33  {
34  if (SysModel != NULL)
35  {
36  _TmpPrior = new Gaussian(SysModel->StateSizeGet());
37  _sample_cov.resize(SysModel->StateSizeGet());
38 
39  this->DimensionSet(SysModel->StateSizeGet());
40  if (MeasModel != NULL)
41  this->NumConditionalArgumentsSet(SysModel->SystemPdfGet()->NumConditionalArgumentsGet()
42  + MeasModel->MeasurementPdfGet()->NumConditionalArgumentsGet());
43  }
44  else
45  {
46  _TmpPrior = new Gaussian();
47  }
48 
49  _sysmodel = SysModel;
50  _measmodel = MeasModel;
51 
52  /* _filter remains uninitialised here!!!
53  This is Only an interface class! Or should we make an
54  enumerate or something else here, like some nameserved stuff?
55  */
56  }
57 
58  FilterPropDens::~FilterPropDens(){}
59 
60  // BUG Copy constructor not implemented yet
62 
63  void
64  FilterPropDens::SystemModelSet(AnalyticSystemModelGaussianUncertainty * SysModel)
65  {
66  assert ( SysModel != NULL );
67  assert ( (this->DimensionGet() == 0) || (this->DimensionGet() == (unsigned int)SysModel->StateSizeGet()) );
68  if ((this->DimensionGet() == 0))
69  {
70  _TmpPrior->DimensionSet(SysModel->StateSizeGet());
71  _sample_cov.resize(SysModel->StateSizeGet());
72  }
73  this->DimensionSet(SysModel->StateSizeGet());
74  if (_measmodel != NULL)
75  this->NumConditionalArgumentsSet(SysModel->SystemPdfGet()->NumConditionalArgumentsGet()
76  + _measmodel->MeasurementPdfGet()->NumConditionalArgumentsGet());
77  _sysmodel = SysModel;
78  }
79 
80 
81  void
82  FilterPropDens::MeasurementModelSet(AnalyticMeasurementModelGaussianUncertainty * MeasModel)
83  {
84  assert ( MeasModel != NULL );
85  if (_sysmodel != NULL)
86  this->NumConditionalArgumentsSet(_sysmodel->SystemPdfGet()->NumConditionalArgumentsGet()
87  + MeasModel->MeasurementPdfGet()->NumConditionalArgumentsGet());
88  _measmodel = MeasModel;
89  }
90 
91  void
92  FilterPropDens::SampleCovSet(SymmetricMatrix & cov)
93  {
94  assert (cov.rows() == this->DimensionGet());
95  _sample_cov = cov;
96  }
97 
98  ColumnVector
99  FilterPropDens::ExpectedValueGet() const
100  {
101  this->FilterStep();
102  return (this->_filter->PostGet()->ExpectedValueGet());
103  }
104 
105  SymmetricMatrix
106  FilterPropDens::CovarianceGet() const
107  {
108  this->FilterStep();
109  return (_filter->PostGet()->CovarianceGet());
110  }
111 
112  void
113  FilterPropDens::FilterStep() const
114  {
115  this->_TmpPrior->ExpectedValueSet(this->ConditionalArgumentGet(0));
116  // See above, last argument is previous covariance Matrix
117  this->_TmpPrior->CovarianceSet(_sample_cov);
118  this->_filter->Reset(_TmpPrior);
119  // Updatestep
120  if ( _sysmodel == NULL )
121  {
122  if (_measmodel->SystemWithoutSensorParams() == false){ // 2 conditional arguments
123  _filter->Update(_measmodel,this->ConditionalArgumentGet(1),this->ConditionalArgumentGet(2));}
124  else{
125  _filter->Update(_measmodel,this->ConditionalArgumentGet(1));}
126  }
127  else if ( _measmodel == NULL )
128  {
129  if (_sysmodel->SystemWithoutInputs() == false){// 2 conditional arguments
130  _filter->Update(_sysmodel,this->ConditionalArgumentGet(1));}
131  else _filter->Update(_sysmodel);
132  }
133  else
134  {
135  if ( ( _sysmodel ->SystemWithoutInputs() == false) && (_measmodel->SystemWithoutSensorParams() == false) ){
136  _filter->Update(_sysmodel,this->ConditionalArgumentGet(1),
137  _measmodel,this->ConditionalArgumentGet(2),this->ConditionalArgumentGet(3));}
138  else if ( ( _sysmodel->SystemWithoutInputs() == true) && (_measmodel->SystemWithoutSensorParams() == false) ){
139  _filter->Update(_sysmodel,_measmodel,this->ConditionalArgumentGet(1),this->ConditionalArgumentGet(2));}
140  else if ( (_sysmodel->SystemWithoutInputs() == false) && (_measmodel->SystemWithoutSensorParams() == true) ){
141  _filter->Update(_sysmodel,this->ConditionalArgumentGet(1),
142  _measmodel,this->ConditionalArgumentGet(2));}
143  else // No inputs, no sensor parameters
144  _filter->Update(_sysmodel,_measmodel,this->ConditionalArgumentGet(1));
145  }
146  }
147 
148  Matrix
149  FilterPropDens::dfGet(unsigned int i) const
150  {
151  cerr << "FilterPropDens::dfGet() never necessary?" << endl;
152  exit(-BFL_ERRMISUSE);
153  }
154 
155 } // End namespace BFL
#define MeasModel
#define FilterPropDens
#define BFL_ERRMISUSE


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