PaFilBFLRelRelBearOnly3DvelUpdate.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_3D_VEL_UPDATE_H_
#define __PA_FIL_BFL_RR_BEAR_ONLY_3D_VEL_UPDATE_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"
#include "LogTrace.h"
#include <Types/structQuad.h>

using namespace MatrixWrapper;
using namespace BFL;
using namespace std;
using namespace MipBaselib;
 
 
namespace MipAlgorithms {
 
 class NonlinearMobileRobRelRelBearOnly3DPdfvelUpdate : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
  public:
   NonlinearMobileRobRelRelBearOnly3DPdfvelUpdate( const Gaussian& additiveNoise);
   
   virtual ~NonlinearMobileRobRelRelBearOnly3DPdfvelUpdate();
   
   virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, int method=DEFAULT, void * args=NULL) const;
  
  private:
   Gaussian _additiveNoise;
 };
 
 
 class NonlinearMeasurementRelRelBearOnly3DPdfvelUpdate : public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector> {
  public:
   NonlinearMeasurementRelRelBearOnly3DPdfvelUpdate( const Gaussian& measNoise);
   
   virtual ~NonlinearMeasurementRelRelBearOnly3DPdfvelUpdate();
   
   virtual Probability ProbabilityGet(const MatrixWrapper::ColumnVector& measurement) const;
  
  private:
   Gaussian _measNoise;
 };
 
 
 class PaFilBFLRelRelBearOnly3DvelUpdate : public PaFilBFL3D, public MIPObject{
  public:
   
   PaFilBFLRelRelBearOnly3DvelUpdate(PaFilBFLPar par);
   
   ~PaFilBFLRelRelBearOnly3DvelUpdate();
   
   void step(MutLocFilInput3D &input);
   
   void reset();
    
   string print();
   
   Decimal getLastOdomUpdateTime(){
    return iniTime;
   }
  
  private:
   // All time structures referred in seconds
   // Timer to measure past time
   Timer                     timer_measures;
   // Time elapsed from the last motion update
   Decimal                    dt;
   // Current time of execution
   Decimal                    iniTime;
   // Last time when a measurement update is performed
   Decimal                    lastMeasUpdate;
   // Number of accepted measurements
   int                      iteration;
   // Parameter of the filter
   PaFilBFLPar                  _par;
   // Pointer to system probability function
   NonlinearMobileRobRelRelBearOnly3DPdfvelUpdate*    _sys_pdf;
   // Pointer to system model
   SystemModel<ColumnVector>*          _sys_model;
   // Pointer to measurement probability function
   NonlinearMeasurementRelRelBearOnly3DPdfvelUpdate*   _meas_pdf;
   // Pointer to measurement model
   MeasurementModel<ColumnVector, ColumnVector>* _meas_model;
   // Last valid velocity reading of the filter owner
   VelOdometry                  _myOdometry;
   // Last valid velocity reading of the robot target
   VelOdometry                  _hisOdometry;
   // Previous valid velocity reading of the filter owner
   VelOdometry                  _myPrevOdometry;
   // Previous valid velocity reading of the robot target
   VelOdometry                  _hisPrevOdometry;
   // Angular velocity of the filter owner in horizonthal frame 
   Velocity3D _myOmegaVel;
   // Angular velocity of other target robot in horizonthal frame
   Velocity3D _myPrevOmegaVel;
   
   // Methods
   // Standard method called
   void _normalStep(MutLocFilInput3D &input);
   // Inizialization with first measurement
   void _firstStep(MutLocFilInput3D &input);
   
   int getEstimate(Pose3D* ,Velocity3D* bestVel=0);
   
   int getAllEstimates(vector<Pose3D> &estimates, vector<Decimal> &weights,vector<Velocity3D> &velEstimates);
   
   string getObjectName() const {
    return "PaFilBFLRelRelBearOnly3DvelUpdate";
   }
 };
}

#endif