EKF.h

Go to the documentation of this file.

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

/* Caso EKF */
      // x+ : stima a posteriori
      // x- : stima a priori
      // w : rumore di processo
      // v : rumore di misura
      // f(x+,u,0) -> per la proiezione in avanti dello stato (stima a priori) <- funzione
      // h(x-,0) -> modello per la misura <- funzione
      // A = df/dx -> calcolata per (x+,u,0) <- funzione (se variabile), matrice (se costante)
      // W = df/dw -> calcolata per (x+,u,0) <- funzione (se variabile), matrice (se costante)
      // H = dh/dx -> calcolata per (x-,u,0) <- funzione (se variabile), matrice (se costante)
      // V = dh/dv -> calcolata per (x-,u,0) <- funzione (se variabile), matrice (se costante)

#include <bfl/filter/extendedkalmanfilter.h>
#include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <bfl/pdf/analyticconditionalgaussian.h>
#include <bfl/pdf/linearanalyticconditionalgaussian.h>
#include <bfl/pdf/analyticconditionalgaussian_additivenoise.h>

#include <iostream>
#include <fstream>

#include <baselib.h>
#include <bfl/filter/kalmanfilter.h>
#include <bfl/pdf/conditionalpdf.h>
#include <bfl/pdf/gaussian.h>
# include <map>

//#include <time.h>
#include <bfl/wrappers/rng/rng.h>
#include <LogTrace.h>

#ifndef _EKF_H
#define _EKF_H


using namespace MipBaselib;
using namespace std;

namespace MipAlgorithms{
 
 /* @{ */
  
    class processModel : public BFL::AnalyticConditionalGaussianAdditiveNoise
    {
    private:
      MatrixWrapper::Matrix A;     // jacobian matrix (df/dx) <- an external callback function will compute this
      MatrixWrapper::Matrix W;     // jacobian matrix (df/dw) <- an external callback function will compute this
      MatrixWrapper::ColumnVector preStateEst;   // state projection (a priori) <- an external callback function will compute this
    public:
      processModel(const BFL::Gaussian& additiveNoise,int ConditionalArguments);
      // default costructor copy
      // The redefinition of these methods is mandatory
      virtual MatrixWrapper::ColumnVector    ExpectedValueGet() const; //returns x-
      virtual MatrixWrapper::Matrix dfGet(unsigned int i) const;                 //returns A

      // This method must be redefined to get W*Q*W' instead of Q
      virtual MatrixWrapper::SymmetricMatrix CovarianceGet   () const;
      // brief get/set methods for internal members
      inline void set_A(MatrixWrapper::Matrix df_dx) {A = df_dx;};
      inline MatrixWrapper::Matrix get_A() {return A;};
      inline void set_W(MatrixWrapper::Matrix df_dw) {W = df_dw;};
      inline MatrixWrapper::Matrix get_W() {return W;};
      inline void set_preStateEst(MatrixWrapper::ColumnVector preEst) {preStateEst = preEst;};
      inline MatrixWrapper::ColumnVector get_preStateEst() {return preStateEst;};
    };

    // TODO - this class has to be checked!!! By now this doesn't work!!!
    class measModel : public BFL::AnalyticConditionalGaussianAdditiveNoise
    {
    private:
      BFL::Matrix H;          // jacobian matrix (df/dx) <- an external callback function will compute this
      BFL::ColumnVector zEst; // stima della misura
      BFL::ColumnVector stateEst; // stima dello stato (serve per calcolare la previsione della misura)
      int rows,columns; // righe/colonne di H
    public:
      measModel(const BFL::Matrix& ratio, const BFL::Gaussian& additiveNoise);

      virtual BFL::ColumnVector    ExpectedValueGet() ;
      virtual BFL::Matrix          dfGet(unsigned int i)       const;
      // brief get/set methods for internal members
      inline BFL::Matrix get_H() {return H;};
      inline void set_H(BFL::Matrix dh_dx) {H = dh_dx;};
      inline BFL::ColumnVector get_stateEst() {return stateEst;};
      inline void set_stateEst(BFL::ColumnVector state){printf("prova\n"); stateEst = state;};
    };

    class EKF : public BFL::ExtendedKalmanFilter
    {
    public:
        EKF(BFL::Gaussian* prior);
        void SystemUpdate(BFL::SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
                         const MatrixWrapper::ColumnVector& u);
        void MeasurementUpdate(BFL::MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
                          const MatrixWrapper::ColumnVector& z,
     const MatrixWrapper::ColumnVector& s);
    };

    enum filterTypes{
  NLIN_PROCESS_LIN_MEASURE,
  LIN_PROCESS_NLIN_MEASURE,
                NLIN_PROCESS_NLIN_MEASURE,
                LIN_PROCESS_LIN_MEASURE
    };
    class genericKF
    {
    private:
        filterTypes   filterType;   // The filter type
        MatrixWrapper::ColumnVector preStateEst;  // current state estimation ("a priori")
        MatrixWrapper::ColumnVector postStateEst; // current state estimation ("a posteriori")
        int num_conditional_arguments; // conditional arguments of the conditional pdf
        // NON LINEAR MODELS
        processModel* process;      // Non linear process model
        measModel*    meas;         // Non linear measurement model
        // LINEAR MODELS
        BFL::LinearAnalyticConditionalGaussian * linear_process; // Linear process model
        BFL::LinearAnalyticConditionalGaussian * linear_meas;    // Linear measurement model

        BFL::AnalyticSystemModelGaussianUncertainty* _processModelNonLin;       // arguments of the filter process non lin
        BFL::AnalyticMeasurementModelGaussianUncertainty* _measModelNonLin;     // arguments of the filter meas non lin
        BFL::LinearAnalyticSystemModelGaussianUncertainty* _processModelLin;    // arguments of the filter meas lin
        BFL::LinearAnalyticMeasurementModelGaussianUncertainty* _measModelLin;  // arguments of the filter process lin

        BFL::Gaussian* _priorProb;          // Gaussian associated with prior state knowledge
        MatrixWrapper::ColumnVector s;  // Internal orocos implementation argument
        // External callback functions to update internal Matrices (A = df/dx, W = df/dw)
        MatrixWrapper::Matrix (*func_W)(MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector, double);
        MatrixWrapper::Matrix (*func_A)(MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector, double);
        MatrixWrapper::ColumnVector (*projState)(MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector,double);
    public:
        genericKF(){};
        // Non linear process model, linear measure model, non additive process noise (EKF)
        genericKF(MatrixWrapper::ColumnVector prior_mu,MatrixWrapper::SymmetricMatrix prior_sigma,
                MatrixWrapper::ColumnVector Pro_mu,MatrixWrapper::SymmetricMatrix Pro_sigma,
                MatrixWrapper::ColumnVector Meas_mu,MatrixWrapper::SymmetricMatrix Meas_sigma,
                int numConditionalArguments,
                MatrixWrapper::Matrix (*eval_A)(MatrixWrapper::ColumnVector , MatrixWrapper::ColumnVector , double ),
                MatrixWrapper::Matrix (*eval_W)(MatrixWrapper::ColumnVector , MatrixWrapper::ColumnVector , double ),
                MatrixWrapper::Matrix dh_dx,
                MatrixWrapper::ColumnVector (*funcState)(MatrixWrapper::ColumnVector , MatrixWrapper::ColumnVector , double ));
        // Linear process model, linear measure model, additive process noise (KF)
        genericKF(MatrixWrapper::ColumnVector prior_mu,MatrixWrapper::SymmetricMatrix prior_sigma,
                MatrixWrapper::ColumnVector Pro_mu,MatrixWrapper::SymmetricMatrix Pro_sigma,
                MatrixWrapper::ColumnVector Meas_mu,MatrixWrapper::SymmetricMatrix Meas_sigma,
                vector<MatrixWrapper::Matrix> AB,
                MatrixWrapper::Matrix dh_dx);
        // TODO - constructors for the other types

        // Filter main loop
        bool Update(MatrixWrapper::ColumnVector u, MatrixWrapper::ColumnVector z, double T_sample);      // Time update --> Measurement update
        bool revUpdate(MatrixWrapper::ColumnVector u, MatrixWrapper::ColumnVector z, double T_sample);   // Measurement update --> Time update

        EKF* _EKF; // A simple wrapper of EKF to get acces to protected members
        BFL::KalmanFilter* _KF;
        // brief get/set methods for internal members
        inline BFL::AnalyticSystemModelGaussianUncertainty* getProcessModel(){return _processModelNonLin;};
        inline BFL::AnalyticMeasurementModelGaussianUncertainty* getMeasModel(){return _measModelNonLin;};
        inline BFL::LinearAnalyticSystemModelGaussianUncertainty* getProcessModelLin(){return _processModelLin;};
        inline BFL::LinearAnalyticMeasurementModelGaussianUncertainty* getMeasModelLin(){return _measModelLin;};
        inline MatrixWrapper::ColumnVector getPreStateEst(){return preStateEst;};
        inline void setPreStateEst(MatrixWrapper::ColumnVector input){preStateEst = input;};
        inline MatrixWrapper::ColumnVector getPostStateEst(){return postStateEst;};
        inline void setPostStateEst(MatrixWrapper::ColumnVector input){postStateEst = input;};
        //tramite selector scelgo cosa modificare:
        //1)sel=0 model uncertainties
        //2)sel=1 meas uncertainties
        //3)sel=2 prior
        static void update_uncertainties(MatrixWrapper::ColumnVector in_mu,MatrixWrapper::SymmetricMatrix in_Cov,
                                        std::vector<MatrixWrapper::ColumnVector> & mu,std::vector<MatrixWrapper::SymmetricMatrix> & sigma,int selctor){};
        static void update_uncertainties(MatrixWrapper::ColumnVector in_mu,std::vector<MatrixWrapper::ColumnVector> & mu,int selctor){};
        static void update_uncertainties(MatrixWrapper::SymmetricMatrix in_Cov,std::vector<MatrixWrapper::SymmetricMatrix> & sigma,int selctor){};

         genericKF & operator = (const genericKF & copy)
{
     printf("sono dentro l operatore =\n");
     if (this != &copy)
   {
     switch(copy.filterType)
    {
     case NLIN_PROCESS_LIN_MEASURE :
     delete process;
     delete linear_meas;
     delete _processModelNonLin;
     delete _measModelLin;
     delete _priorProb;
     delete _EKF;
     

     num_conditional_arguments=copy.num_conditional_arguments;
     process = new processModel(*(copy.process));
     _processModelNonLin = new BFL::AnalyticSystemModelGaussianUncertainty(process);
     linear_meas = new BFL::LinearAnalyticConditionalGaussian(*(copy.linear_meas));
     _measModelLin = new BFL::LinearAnalyticMeasurementModelGaussianUncertainty(linear_meas);
     _priorProb = new BFL::Gaussian(*(copy._priorProb));
     _EKF = new EKF(_priorProb);
     func_W=copy.func_W;
     func_A=copy.func_A;
     projState=copy.projState;
     break;
     case LIN_PROCESS_LIN_MEASURE :
     delete linear_process;
     delete linear_meas;
     delete _processModelLin;
     delete _measModelLin;
     delete _priorProb;
     delete _EKF;
     num_conditional_arguments=copy.num_conditional_arguments;
     linear_process = new BFL::LinearAnalyticConditionalGaussian(*(copy.linear_process));
     _processModelNonLin = new BFL::LinearAnalyticSystemModelGaussianUncertainty(linear_process);
     linear_meas = new BFL::LinearAnalyticConditionalGaussian(*(copy.linear_meas));
     _measModelLin = new BFL::LinearAnalyticMeasurementModelGaussianUncertainty(linear_meas);
     _priorProb = new BFL::Gaussian(*(copy._priorProb));
     _EKF = new EKF(_priorProb);
     break;

     case LIN_PROCESS_NLIN_MEASURE :

     break;

     case NLIN_PROCESS_NLIN_MEASURE :

     break;
    }

   }
    return  *this;
}
    };

    


 /* @} */
}; //end namespace MipAlgorithms

#endif