RootEKF.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
//
// ----------------------------------------------------------------------------



//  /// \defgroup EKF Generic EKF implementation
//  /// \details  This class provides the basic methods of the EKF algorithm. 
//  /// The system model has to be specified in a subclass and it's passed as argument to the constructor.
//  /// Please refer to SimpleFilter.h and testEKF/testSimpleFilter.cpp for an example of how to create and use a specific EKF.
//  /// \ingroup algorithms_Comp


#ifndef ROOTEKF_H
#define ROOTEKF_H

#include <Spaces.h>
#include <math.h>
#include <PDF.h>

#define DEBUG

using namespace MipBaselib;
using namespace arma;


namespace MipAlgorithms
{
 /* @{ */
 
  class RootEKFvars
  {
    public:
      RootEKFvars(){};
      virtual void evalA(DCol input, DCol state, Decimal sTime) = 0;
      virtual void evalW(DCol input, DCol state, Decimal sTime) = 0;  
      virtual void evalV(DCol state) = 0;
      virtual void evalH(DCol state) = 0;
      virtual DCol projectState(DCol input, DCol state, Decimal sTime) = 0;
      virtual DCol predictMeas(DCol state) = 0;
      inline DMat getA() const {return A;};
      inline DMat getV() const {return V;};
      inline DMat getH() const {return H;};
      inline DMat getW() const {return W;};  
    protected:
      DMat A;    // Jacobian df/dx
      DMat V;    // Jacobian df/dv
      DMat H;    // Jacobian dh/dx
      DMat W;    // Jacobian dh/dw
  };

  class RootEKFparams
  {
    public:
      RootEKFparams(){};
//       virtual void initializeParam() = 0;
      inline DCol getInitialEst() const {return initEst;}
      inline DMat getInitialCov() const {return initCov;}
      inline DMat getInitialProcNoise() const {return initProcNoise;} 
      inline DMat getInitialMeasNoise() const {return initMeasNoise;} 
    protected:
      DCol initEst;   // initial state estimate
      DMat initCov;   // initial covariance estimate
      DMat initProcNoise;
      DMat initMeasNoise;
  };

  class RootEKF
  {
    public:
      RootEKF(RootEKFparams* myParam, RootEKFvars* myVars);
      DCol getStateEst() const;
      DMat getStateCov() const;
      inline DMat getProcNoise() const {return procNoise;};
      inline void setProcNoise(DMat value){procNoise = value;};
      inline DMat getMeasNoise() const {return measNoise;};
      inline void setMeasNoise(DMat value){measNoise = value;};
      inline DMat getK(){return K;};
      void doTimeUpd(DCol input, Decimal sTime);
      void doMeasUpd(DCol measure, Decimal sTime);
      void doUpd(DCol input, DCol measure, Decimal sTime){doTimeUpd(input, sTime); doMeasUpd(measure, sTime);}
      void doRevUpd(DCol input, DCol measure, Decimal sTime){doMeasUpd(measure, sTime); doTimeUpd(input, sTime);}
    private:   
      RootEKFparams* _param;
      RootEKFvars* _vars; 
      DCol stateEst;   // current state estimate 
      DMat stateCov;   // current state covariance
      DMat procNoise;   // process noise 
      DMat measNoise;   // measurements noise
      DMat K;    // kalman gain
      void projectCov();  // evaluate "a priori" covariance
      void evalK();   // compute Kalman gain
      void updState(DCol measure); // evaluate "a posteriori" estimate
      void updCov();   // evaluate "a posteriori" covariance
      void CovSymmetryCheck();  
  };
};// end namespace MipAlgorithms

/* @} */

#endif // ROOTEKF_H