filterproposaldensity.cpp
Go to the documentation of this file.
00001 // $Id: filterproposaldensity.cpp 29495 2008-08-13 12:57:49Z tdelaet $
00002 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
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 
00019 #include "filterproposaldensity.h"
00020 
00021 namespace BFL
00022 {
00023   using namespace MatrixWrapper;
00024 
00025 
00026 #define FilterPropDens FilterProposalDensity
00027 
00028   FilterPropDens::FilterPropDens(AnalyticSystemModelGaussianUncertainty * SysModel,
00029                                  AnalyticMeasurementModelGaussianUncertainty * MeasModel)
00030     : AnalyticConditionalGaussian(),
00031       _sysmodel(SysModel),
00032       _measmodel(MeasModel)
00033   {
00034     if (SysModel != NULL)
00035       {
00036         _TmpPrior = new Gaussian(SysModel->StateSizeGet());
00037         _sample_cov.resize(SysModel->StateSizeGet());
00038 
00039         this->DimensionSet(SysModel->StateSizeGet());
00040         if (MeasModel != NULL)
00041           this->NumConditionalArgumentsSet(SysModel->SystemPdfGet()->NumConditionalArgumentsGet()
00042                                            + MeasModel->MeasurementPdfGet()->NumConditionalArgumentsGet());
00043       }
00044     else
00045       {
00046         _TmpPrior = new Gaussian();
00047       }
00048 
00049     _sysmodel = SysModel;
00050     _measmodel = MeasModel;
00051 
00052     /*  _filter remains uninitialised here!!!
00053         This is Only an interface class!  Or should we make an
00054         enumerate or something else here, like some nameserved stuff?
00055     */
00056   }
00057 
00058   FilterPropDens::~FilterPropDens(){}
00059 
00060   // BUG Copy constructor not implemented yet
00061   FilterPropDens::FilterPropDens(const FilterPropDens & fpd){}
00062 
00063   void
00064   FilterPropDens::SystemModelSet(AnalyticSystemModelGaussianUncertainty * SysModel)
00065   {
00066     assert ( SysModel != NULL );
00067     assert ( (this->DimensionGet() == 0) || (this->DimensionGet() == (unsigned int)SysModel->StateSizeGet()) );
00068     if ((this->DimensionGet() == 0))
00069       {
00070         _TmpPrior->DimensionSet(SysModel->StateSizeGet());
00071         _sample_cov.resize(SysModel->StateSizeGet());
00072       }
00073     this->DimensionSet(SysModel->StateSizeGet());
00074     if (_measmodel != NULL)
00075       this->NumConditionalArgumentsSet(SysModel->SystemPdfGet()->NumConditionalArgumentsGet()
00076                                        + _measmodel->MeasurementPdfGet()->NumConditionalArgumentsGet());
00077     _sysmodel = SysModel;
00078   }
00079 
00080 
00081   void
00082   FilterPropDens::MeasurementModelSet(AnalyticMeasurementModelGaussianUncertainty * MeasModel)
00083   {
00084     assert ( MeasModel != NULL );
00085     if (_sysmodel != NULL)
00086       this->NumConditionalArgumentsSet(_sysmodel->SystemPdfGet()->NumConditionalArgumentsGet()
00087                                        + MeasModel->MeasurementPdfGet()->NumConditionalArgumentsGet());
00088     _measmodel = MeasModel;
00089   }
00090 
00091   void
00092   FilterPropDens::SampleCovSet(SymmetricMatrix & cov)
00093   {
00094     assert (cov.rows() == this->DimensionGet());
00095     _sample_cov = cov;
00096   }
00097 
00098   ColumnVector
00099   FilterPropDens::ExpectedValueGet() const
00100   {
00101     this->FilterStep();
00102     return (this->_filter->PostGet()->ExpectedValueGet());
00103   }
00104 
00105   SymmetricMatrix
00106   FilterPropDens::CovarianceGet() const
00107   {
00108     this->FilterStep();
00109     return (_filter->PostGet()->CovarianceGet());
00110   }
00111 
00112   void
00113   FilterPropDens::FilterStep() const
00114   {
00115     this->_TmpPrior->ExpectedValueSet(this->ConditionalArgumentGet(0));
00116     // See above, last argument is previous covariance Matrix
00117     this->_TmpPrior->CovarianceSet(_sample_cov);
00118     this->_filter->Reset(_TmpPrior);
00119     // Updatestep
00120     if ( _sysmodel == NULL )
00121       {
00122         if (_measmodel->SystemWithoutSensorParams() == false){ // 2 conditional arguments
00123           _filter->Update(_measmodel,this->ConditionalArgumentGet(1),this->ConditionalArgumentGet(2));}
00124         else{
00125           _filter->Update(_measmodel,this->ConditionalArgumentGet(1));}
00126       }
00127     else if ( _measmodel == NULL )
00128       {
00129         if (_sysmodel->SystemWithoutInputs() == false){// 2 conditional arguments
00130           _filter->Update(_sysmodel,this->ConditionalArgumentGet(1));}
00131         else _filter->Update(_sysmodel);
00132       }
00133     else
00134       {
00135         if ( ( _sysmodel ->SystemWithoutInputs() == false) && (_measmodel->SystemWithoutSensorParams() == false) ){
00136           _filter->Update(_sysmodel,this->ConditionalArgumentGet(1),
00137                           _measmodel,this->ConditionalArgumentGet(2),this->ConditionalArgumentGet(3));}
00138         else if ( ( _sysmodel->SystemWithoutInputs() == true) && (_measmodel->SystemWithoutSensorParams() == false) ){
00139           _filter->Update(_sysmodel,_measmodel,this->ConditionalArgumentGet(1),this->ConditionalArgumentGet(2));}
00140         else if ( (_sysmodel->SystemWithoutInputs() == false) && (_measmodel->SystemWithoutSensorParams() == true) ){
00141           _filter->Update(_sysmodel,this->ConditionalArgumentGet(1),
00142                           _measmodel,this->ConditionalArgumentGet(2));}
00143         else // No inputs, no sensor parameters
00144           _filter->Update(_sysmodel,_measmodel,this->ConditionalArgumentGet(1));
00145       }
00146   }
00147 
00148   Matrix
00149   FilterPropDens::dfGet(unsigned int i) const
00150   {
00151     cerr << "FilterPropDens::dfGet() never necessary?" << endl;
00152     exit(-BFL_ERRMISUSE);
00153   }
00154 
00155 } // End namespace BFL


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