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
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
00053
00054
00055
00056 }
00057
00058 FilterPropDens::~FilterPropDens(){}
00059
00060
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
00117 this->_TmpPrior->CovarianceSet(_sample_cov);
00118 this->_filter->Reset(_TmpPrior);
00119
00120 if ( _sysmodel == NULL )
00121 {
00122 if (_measmodel->SystemWithoutSensorParams() == false){
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){
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
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 }
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