PaFilBFLRelRelBearOnly.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __PA_FIL_BFL_RR_BEAR_ONLY_H_
#define __PA_FIL_BFL_RR_BEAR_ONLY_H_

#include <filter/bootstrapfilter.h>

#include <model/systemmodel.h>
#include <model/measurementmodel.h>
#include "time.h"

#include <iostream>
#include <fstream>

#include <pdf/conditionalpdf.h>
#include <pdf/gaussian.h>
#include "PaFilBFL.h"

using namespace MatrixWrapper;
using namespace BFL;
using namespace std;
using namespace MipBaselib;
//Pose          _bestParticlePose;
//Decimal       _bestParticleProb;


namespace MipAlgorithms {
        
 class NonlinearMobileRobRelRelBearOnlyPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
  public:
   NonlinearMobileRobRelRelBearOnlyPdf( const Gaussian& additiveNoise);
   
   virtual ~NonlinearMobileRobRelRelBearOnlyPdf();
   
   // implement this virtual function for system model of a particle filter
   virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, int method=DEFAULT, void * args=NULL) const;
  
  private:
   Gaussian _additiveNoise;
 };
 
 
 class NonlinearMeasurementRelRelBearOnlyPdf : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
  public:
   NonlinearMeasurementRelRelBearOnlyPdf( const Gaussian& measNoise);
   
   virtual ~NonlinearMeasurementRelRelBearOnlyPdf();
   
   // implement this virtual function for measurement model of a particle filter
   virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const;
  
  private:
   Gaussian _measNoise;
 };
 
 
 class PaFilBFLRelRelBearOnly : public PaFilBFL{
  public:
   
   PaFilBFLRelRelBearOnly(PaFilBFLPar par);
   
   ~PaFilBFLRelRelBearOnly();
   
   void step(MutLocFilInput &input);
   
   void reset();
    
   string print();
   
  private:
   // Timer:
                        Timer   timer_measures;
                        Decimal iniTime;

                        int iteration;
                        
   PaFilBFLPar _par;
   
   NonlinearMobileRobRelRelBearOnlyPdf*     _sys_pdf;
   SystemModel<ColumnVector>* _sys_model;
   
   NonlinearMeasurementRelRelBearOnlyPdf*           _meas_pdf;
   MeasurementModel<ColumnVector, ColumnVector>* _meas_model;
   
   Pose _myPrevConf;
   Pose _myCurrConf;
   Pose _myOdometry;
   
   Pose _hisPrevConf;
   Pose _hisCurrConf;
   Pose _hisOdometry;
   
   Decimal _myOdometryNorm;
   Decimal _hisOdometryNorm;
   
   void _normalStep(MutLocFilInput &input);
   
   void _firstStep(MutLocFilInput &input);
   
   void _computeHisOdometry(MutLocFilInput &input);
   
 };
}

#endif