00001 // ---------------------------------------------------------------------------- 00002 // 00003 // $Id$ 00004 // 00005 // Copyright 2008, 2009, 2010, 2011, 2012 Antonio Franchi and Paolo Stegagno 00006 // 00007 // This file is part of MIP. 00008 // 00009 // MIP is free software: you can redistribute it and/or modify 00010 // it under the terms of the GNU General Public License as published by 00011 // the Free Software Foundation, either version 3 of the License, or 00012 // (at your option) any later version. 00013 // 00014 // MIP is distributed in the hope that it will be useful, 00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 // GNU General Public License for more details. 00018 // 00019 // You should have received a copy of the GNU General Public License 00020 // along with MIP. If not, see <http://www.gnu.org/licenses/>. 00021 // 00022 // Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it 00023 // 00024 // ---------------------------------------------------------------------------- 00025 00026 00030 00032 /* @{ */ 00033 00034 #ifndef __PA_FIL_BFL_RR_3D_H_ 00035 #define __PA_FIL_BFL_RR_3D_H_ 00036 00037 #include <filter/bootstrapfilter.h> 00038 #include <model/systemmodel.h> 00039 #include <model/measurementmodel.h> 00040 #include "time.h" 00041 #include <iostream> 00042 #include <fstream> 00043 #include <pdf/conditionalpdf.h> 00044 #include <pdf/gaussian.h> 00045 #include "PaFilBFL.h" 00046 #include "LogTrace.h" 00047 #include "PaFilBFLRelRelBearOnly3D.h" 00048 00049 using namespace MatrixWrapper; 00050 using namespace BFL; 00051 using namespace std; 00052 using namespace MipBaselib; 00053 00054 00055 namespace MipAlgorithms { 00056 00058 class NonlinearMobileRobRelRel3DPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> { 00059 public: 00062 NonlinearMobileRobRelRel3DPdf( const Gaussian& additiveNoise); 00063 00065 virtual ~NonlinearMobileRobRelRel3DPdf(); 00066 00068 virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, int method=DEFAULT, void * args=NULL) const; 00069 00070 private: 00071 Gaussian _additiveNoise; 00072 }; 00073 00074 00076 class NonlinearMeasurementRelRel3DPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> { 00077 public: 00080 NonlinearMeasurementRelRel3DPdf( const Gaussian& measNoise); 00081 00083 virtual ~NonlinearMeasurementRelRel3DPdf(); 00084 00086 virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const; 00087 00088 private: 00089 Gaussian _measNoise; 00090 }; 00091 00092 00096 class PaFilBFLRelRel3D : public PaFilBFL3D, public MIPObject{ 00097 public: 00098 00100 PaFilBFLRelRel3D(PaFilBFLPar par); 00101 00103 ~PaFilBFLRelRel3D(); 00104 00107 void step(MutLocFilInput3D &input); 00108 00110 void reset(); 00111 00113 string print(); 00114 00115 Decimal getLastOdomUpdateTime(){ 00116 return iniTime; 00117 } 00118 00119 private: 00120 // All time structures referred in seconds 00121 // Timer to measure past time 00122 Timer timer_measures; 00123 // Time elapsed from the last motion update 00124 Decimal dt; 00125 // Current time of execution 00126 Decimal iniTime; 00127 // Last time when a measurement update is performed 00128 Decimal lastMeasUpdate; 00129 // Number of accepted measurements 00130 int iteration; 00131 // Parameter of the filter 00132 PaFilBFLPar _par; 00133 // Pointer to system probability function 00134 NonlinearMobileRobRelRel3DPdf* _sys_pdf; 00135 // Pointer to system model 00136 SystemModel<ColumnVector>* _sys_model; 00137 // Pointer to measurement probability function 00138 NonlinearMeasurementRelRel3DPdf* _meas_pdf; 00139 // Pointer to measurement model 00140 MeasurementModel<ColumnVector, ColumnVector>* _meas_model; 00141 // Last valid IMU reading of the filter owner 00142 IMUOdometry _myOdometry; 00143 // Last valid IMU reading of the robot target 00144 IMUOdometry _hisOdometry; 00145 // Previous valid IMU reading of the filter owner 00146 IMUOdometry _myPrevOdometry; 00147 // Previous valid IMU reading of the robot target 00148 IMUOdometry _hisPrevOdometry; 00149 // Angular velocity of the filter owner in horizonthal frame 00150 Velocity3D _myOmegaVel; 00151 // Angular velocity of other target robot in horizonthal frame 00152 Velocity3D _myPrevOmegaVel; 00153 00154 // Methods 00155 // Standard method called 00156 void _normalStep(MutLocFilInput3D &input); 00157 00158 // Inizialization with first measurement 00159 void _firstStep(MutLocFilInput3D &input); 00160 00161 string getObjectName() const { 00162 return "PaFilBFLRelRel3D"; 00163 } 00164 }; 00165 } 00166 00167 #endif 00168 00169 00170 00171 00172 00173