work/mixpp/bdm/estim/libKF.h

Go to the documentation of this file.

#ifndef KF_H
#define KF_H

#include <itpp/itbase.h>
#include "../stat/libFN.h"
#include "../math/libDC.h"


using namespace itpp;

class KalmanFull : public BM { 
        int dimx, dimy, dimu;
        mat A, B, C, D, R, Q;
        
        //cache 
        mat _Pp, _Ry, _iRy, _K;
public:
        //posterior 
        vec mu;
        mat P;

public:
        KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0);
        void bayes(const vec &dt, bool evalll=true); 

        friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );

};


template<class sq_T>
class Kalman : public BM { 
protected:
        int dimx, dimy, dimu;
        mat A, B, C, D;
        sq_T R, Q;
        
        //cache
        mat _K;
        vec _yp;
        sq_T _Ry,_iRy;
public:
        //posterior 
        vec mu;
        sq_T P;

public:
        Kalman (int dimx, int dimu, int dimy);
        Kalman ( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 );
        void bayes(const vec &dt, bool evalll=true); 

        friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );

};

template<class sq_T>
class EKF : public Kalman<fsqmat> {
        diffbifn* pfxu;
        diffbifn* phxu;
public: 
        EKF (diffbifn* pfxu, diffbifn* phxu, sq_T Q0, sq_T R0, vec mu0, mat P0);
        void bayes(const vec &dt, bool evalll=true);    
};


template<class sq_T>
Kalman<sq_T>::Kalman( int dx, int du, int dy): BM(), dimx(dx),dimy(dy),dimu(du){
        A = mat(dimx,dimx);
        B = mat(dimx,dimu);
        C = mat(dimy,dimx);
        D = mat(dimy,dimu);

        mu = vec(dimx);
        //TODO Initialize the rest?
};

template<class sq_T>
Kalman<sq_T>::Kalman(const  mat A0,const  mat B0, const mat C0, const mat D0, const sq_T R0, const sq_T Q0, const sq_T P0, const vec mu0 ): BM() {
        dimx = A0.rows();
        dimu = B0.cols();
        dimy = C0.rows();

        it_assert_debug( A0.cols()==dimx, "Kalman: A is not square" );
        it_assert_debug( B0.rows()==dimx, "Kalman: B is not compatible" );
        it_assert_debug( C0.cols()==dimx, "Kalman: C is not square" );
        it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     "Kalman: D is not compatible" );
        it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "Kalman: R is not compatible" );
        it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "Kalman: Q is not compatible" );

        A = A0;
        B = B0;
        C = C0;
        D = D0;
        R = R0;
        Q = Q0;
        mu = mu0;
        P = P0;

//Fixme should we assign cache??
        _iRy = eye(dimy); // needed in inv(_iRy)
        _Ry = eye(dimy); // needed in inv(_iRy)
}

template<class sq_T>
void Kalman<sq_T>::bayes( const vec &dt , bool evalll) {
        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );

        vec u = dt.get( dimy,dimy+dimu-1 );
        vec y = dt.get( 0,dimy-1 );
        //Time update
        mu = A*mu + B*u;
        //P  = A*P*A.transpose() + Q; in sq_T
        P.mult_sym( A );
        P+=Q;

        //Data update
        //_Ry = C*P*C.transpose() + R; in sq_T
        P.mult_sym( C, _Ry);
        _Ry+=R;

        mat Pfull = P.to_mat();
        
        _Ry.inv( _iRy ); // result is in _iRy;
        _K = Pfull*C.transpose()*(_iRy.to_mat());
        P -= _K*C*Pfull; // P = P -KCP;
        _yp = y-C*mu-D*u; //y prediction
        mu += _K*( _yp );
        
        if (evalll==true) {
        ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \
        +_Ry.logdet() +_iRy.qform(_yp));
        }
};

//TODO why not const pointer??

template<class sq_T>
EKF<sq_T>::EKF( diffbifn* pfxu0,  diffbifn* phxu0, sq_T Q0, sq_T R0, vec mu0, mat P0): pfxu(pfxu0), phxu(phxu0), Kalman<fsqmat>(pfxu0->_dimx(),pfxu0->_dimu(),phxu0->_dimy()) {
                
                //initialize matrices A C, later, these will be only updated!
                pfxu->dfdx_cond(mu,zeros(dimu),A,true);
                pfxu->dfdu_cond(mu,zeros(dimu),B,true);
                phxu->dfdx_cond(mu,zeros(dimu),C,true);
                phxu->dfdu_cond(mu,zeros(dimu),D,true);
                

                R = R0;
                Q = Q0;
                mu = mu0;
                P = P0;

        using std::cout;
        cout<<A<<std::endl;
        cout<<B<<std::endl;
        cout<<C<<std::endl;
        cout<<D<<std::endl;

}

template<class sq_T>
void EKF<sq_T>::bayes( const vec &dt , bool evalll) {
        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );

        vec u = dt.get( dimy,dimy+dimu-1 );
        vec y = dt.get( 0,dimy-1 );
        //Time update
        mu = pfxu->eval(mu, u);
        pfxu->dfdx_cond(mu,u,A,false); //update A by a derivative of fx
        
        //P  = A*P*A.transpose() + Q; in sq_T
        P.mult_sym( A );
        P+=Q;

        //Data update
        phxu->dfdx_cond(mu,u,C,false); //update C by a derivative hx
        //_Ry = C*P*C.transpose() + R; in sq_T
        P.mult_sym( C, _Ry);
        _Ry+=R;

        mat Pfull = P.to_mat();
        
        _Ry.inv( _iRy ); // result is in _iRy;
        _K = Pfull*C.transpose()*(_iRy.to_mat());
        P -= _K*C*Pfull; // P = P -KCP;
        _yp = y-phxu->eval(mu,u); //y prediction
        mu += _K*( _yp );
        
        if (evalll==true) {
        ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \
        +_Ry.logdet() +_iRy.qform(_yp));
        }
};


#endif // KF_H

---