PaFilBFLRelRel.h
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
00020
00021
00022
00023
00024
00025
00026
00030
00032
00033
00034 #ifndef __PA_FIL_BFL_RR_H_
00035 #define __PA_FIL_BFL_RR_H_
00036
00037 #include <filter/bootstrapfilter.h>
00038
00039 #include <model/systemmodel.h>
00040 #include <model/measurementmodel.h>
00041
00042
00043 #include <iostream>
00044 #include <fstream>
00045
00046 #include <pdf/conditionalpdf.h>
00047 #include <pdf/gaussian.h>
00048 #include "PaFilBFL.h"
00049
00050 using namespace MatrixWrapper;
00051 using namespace BFL;
00052 using namespace std;
00053
00054
00055 namespace MipAlgorithms {
00057 class NonlinearMobileRobRelRelPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
00058 public:
00061 NonlinearMobileRobRelRelPdf( const Gaussian& additiveNoise);
00062
00064 virtual ~NonlinearMobileRobRelRelPdf();
00065
00066
00067 virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, int method=DEFAULT, void * args=NULL) const;
00068
00069 private:
00070 Gaussian _additiveNoise;
00071 };
00072 }
00073
00074 namespace MipAlgorithms {
00076 class NonlinearMeasurementRelRelPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
00077 public:
00080 NonlinearMeasurementRelRelPdf( const Gaussian& measNoise);
00081
00083 virtual ~NonlinearMeasurementRelRelPdf();
00084
00085
00086 virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const;
00087
00088 private:
00089 Gaussian _measNoise;
00090 };
00091 }
00092
00093
00094 namespace MipAlgorithms {
00098 class PaFilBFLRelRel : public PaFilBFL{
00099 public:
00100
00102 PaFilBFLRelRel(PaFilBFLPar par);
00103
00105 ~PaFilBFLRelRel();
00106
00109 void step(MutLocFilInput &input);
00110
00112 void reset();
00113
00115 string print();
00116
00117 private:
00118
00119 Decimal _iniTime;
00120
00121 PaFilBFLPar _par;
00122
00123 NonlinearMobileRobRelRelPdf* _sys_pdf;
00124 SystemModel<ColumnVector>* _sys_model;
00125
00126 NonlinearMeasurementRelRelPdf* _meas_pdf;
00127 MeasurementModel<ColumnVector, ColumnVector>* _meas_model;
00128
00129 Pose _myPrevConf;
00130 Pose _myCurrConf;
00131 Pose _myOdometry;
00132
00133 Pose _hisPrevConf;
00134 Pose _hisCurrConf;
00135 Pose _hisOdometry;
00136
00137 Decimal _myOdometryNorm;
00138 Decimal _hisOdometryNorm;
00139
00140 void _normalStep(MutLocFilInput &input);
00141
00142 void _firstStep(MutLocFilInput &input);
00143
00144 void _computeHisOdometry(MutLocFilInput &input);
00145
00146 };
00147 }
00148
00149 #endif
00150
00151
00152
00153
00154
00155