EKFilterUnicycle2D.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 __EKF_FILTER_UNICYCLE2D_H
#define __EKF_FILTER_UNICYCLE2D_H

#include <Spaces.h>
#include <armadillo>
#include <math.h>
#include <Gaussian.h>


using namespace MipBaselib;
using namespace arma;

namespace MipAlgorithms{
 
 /* @{ */
 
 enum EKFilterUnicycle2DInputType{
  EKF_UNICYCLE2D_INPUT_NONE,
  EKF_UNICYCLE2D_INPUT_POSE,
  EKF_UNICYCLE2D_INPUT_VELOCITIES,
  EKF_UNICYCLE2D_INPUT_TYPE_NUM
 };
 
 static const char* EKFilterUnicycle2DInputTypeName[EKF_UNICYCLE2D_INPUT_TYPE_NUM] ={
  "EKFUnicycle2DInputNone",
  "EKFUnicycle2DInputPose",
  "EKFUnicycle2DInputVelocities"
 };
 
 enum EKFilterUnicycle2DMeasurementType{
  EKF_UNICYCLE2D_MEASUREMENT_NONE,
  EKF_UNICYCLE2D_MEASUREMENT_POSE,
  EKF_UNICYCLE2D_MEASUREMENT_POSITION,
  EKF_UNICYCLE2D_MEASUREMENT_DISTANCE,
  EKF_UNICYCLE2D_MEASUREMENT_BEARING,
  EKF_UNICYCLE2D_MEASUREMENT_ORIENTATION,
  EKF_UNICYCLE2D_MEASUREMENT_TYPE_NUM
 };
 
 static const char* EKFilterUnicycle2DMeasurementTypeName[EKF_UNICYCLE2D_MEASUREMENT_TYPE_NUM] ={
  "EKFUnicycle2DMeasurementNone",
  "EKFUnicycle2DMeasurementPose",
  "EKFUnicycle2DMeasurementPosition",
  "EKFUnicycle2DMeasurementDistance",
  "EKFUnicycle2DMeasurementBearing",
  "EKFUnicycle2DMeasurementOrientation"
 };
 
 
 class EKFilterUnicycle2DIn{
  public:
   
   // odometries
   EKFilterUnicycle2DInputType itype;
   
   Time hisInputTime;
   Decimal hisLinVel;
   Decimal hisAngVel;
   
   Time myInputTime;
   Decimal myLinVel;
   Decimal myAngVel;
   
   Pose myPose;
   Pose hisPose;
   
   // measures
   EKFilterUnicycle2DMeasurementType mtype;
   Pose pose;
   DMat::fixed<3,3> poseCov;
   
   Position position;
   DMat::fixed<2,2> positionCov;
   
   Decimal distance;
   Decimal distanceCov;
   
   Angle bearing;
   Decimal bearingCov;
   
   Angle orientation;
   Decimal orientationCov;
   
   EKFilterUnicycle2DIn(){
    itype = EKF_UNICYCLE2D_INPUT_NONE;
    mtype = EKF_UNICYCLE2D_MEASUREMENT_NONE;
   }
 };
 
 
 class EKFilterUnicycle2DPars {
  public:
   
   Time timeBetweenUpdates;
   Time timeBetweenPredictions;
   Decimal startingOrientation;
   Mat< Decimal >::fixed< 2, 2 > N_n_x;
   
   EKFilterUnicycle2DPars();
   
   EKFilterUnicycle2DPars(Time tu, Time tp, Decimal so);
   
   EKFilterUnicycle2DPars(const EKFilterUnicycle2DPars& ekp);
   
   void operator=(const EKFilterUnicycle2DPars& ekp);
};
 
 
 class EKFilterUnicycle2DVars {
  public:
   
   MipBaselib::Gaussian _bel;
   bool _firstStep;
   
   // these quantities are used when the time update is made using the already integrated odometries
   Pose _myOdo;
   Pose _hisOdo;
   
   Pose _lastMyPose;
   Pose _lastHisPose;
   
   // these quantities are used when the time update of the filter is made using the control inputs
   vector<Time> _lastMyInputTime;
   vector<Decimal> _lastMyLinVel;
   vector<Decimal> _lastMyAngVel;
   
   vector<Time> _lastHisInputTime;
   vector<Decimal> _lastHisLinVel;
   vector<Decimal> _lastHisAngVel;
   
   int _counterHisInput;
   int _counterMyInput;
   
   Time _lastIntegrationTime;
   bool _lastIntegrationTimeMine;
   int _lastIntegrationTimeIndex;
   
   // these timers take the time from the last prediction and update steps
   Timer _timeFromLastUpdate;
   Timer _timeFromLastPrediction;
   
   EKFilterUnicycle2DVars();
   
   EKFilterUnicycle2DVars(const EKFilterUnicycle2DVars& ekv);
   
   void operator=(const EKFilterUnicycle2DVars& ekv);
 };
 
 
 class EKFilterUnicycle2D {
  protected:
   EKFilterUnicycle2DVars* _vars;
   EKFilterUnicycle2DPars* _pars;
  public:
   EKFilterUnicycle2D();
   
   EKFilterUnicycle2D(EKFilterUnicycle2DPars &inParams);
   
   EKFilterUnicycle2D(const EKFilterUnicycle2D& ekf);
   
   virtual void step(EKFilterUnicycle2DIn &in);
   
   virtual string print() const;
   
   virtual bool initialized();
   
   virtual MipBaselib::Gaussian getState();
   
   virtual void operator=(const EKFilterUnicycle2D& ekf);
   
   virtual EKFilterUnicycle2DPars* getPars();
   
   virtual void reset();
   
  protected:
   
   virtual void _computeOdometries(EKFilterUnicycle2DIn &in);
   virtual void _initialize(EKFilterUnicycle2DIn &in);
   virtual void _normalStep(EKFilterUnicycle2DIn &in);
   
   virtual void _velocitiesPrediction(EKFilterUnicycle2DIn &in);
   virtual void _velocitiesPrediction(Time &t1, Time &t2, Decimal mv, Decimal mw, Decimal hv, Decimal hw);
   virtual void _posePrediction();
   virtual void _updateWithPose(EKFilterUnicycle2DIn &in);
   virtual void _updateWithPosition(EKFilterUnicycle2DIn &in);
   
 };
 
 /* @} */
 
}


#endif