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 "../stat/libEF.h"


using namespace itpp;

class KalmanFull { 
        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); 

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

};


template<class sq_T>
class Kalman : public BM { 
protected:
        RV rvy;
        RV rvu;
        int dimx, dimy, dimu;
        mat A, B, C, D;
        sq_T R, Q;
        
        enorm<sq_T> est;
        enorm<sq_T> fy;
        
        mat _K;
        //cache of fy
        vec* _yp; 
        sq_T* _Ry,*_iRy;
        //cache of est
        vec* _mu;
        sq_T* _P, *_iP;
        
public:
        Kalman (RV rvx0, RV rvy0, RV rvu0);
        Kalman (const Kalman<sq_T> &K0);
        void set_parameters (const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &R0,const sq_T &Q0);
        void set_est(const vec &mu0, const sq_T &P0 ){est.set_parameters(mu0,P0);};
        void bayes(const vec &dt); 
        epdf& _epdf(){return est;}
//      friend std::ostream &operator<< ( std::ostream &os, const Kalman<sq_T> &kf );

};

template<class sq_T>
class EKF : public Kalman<ldmat> {
        diffbifn* pfxu;
        diffbifn* phxu;
public: 
        EKF (RV rvx, RV rvy, RV rvu);
        void set_parameters(diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0);
        void bayes(const vec &dt);      
};

class KFcondQR : public Kalman<ldmat>, public BMcond {
//protected:
public:
KFcondQR(RV rvx, RV rvy, RV rvu): Kalman<ldmat>(rvx, rvy,rvu){};
void condition(const vec &RQ);
};


template<class sq_T>
Kalman<sq_T>::Kalman(const Kalman<sq_T> &K0): BM(K0.rv),rvy(K0.rvy),rvu(K0.rvu),
dimx(rv.count()), dimy(rvy.count()),dimu(rvu.count()), 
 A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu),est(rv), fy(rvy){
this->set_parameters(K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q);
//reset copy values in pointers
*_mu = *K0._mu;
*_P = *K0._P;
*_iP = *K0._iP;
*_yp = *K0._yp;
*_iRy = *K0._iRy;
*_Ry = *K0._Ry;

}

template<class sq_T>
Kalman<sq_T>::Kalman(RV rvx, RV rvy0, RV rvu0): BM(rvx),rvy(rvy0),rvu(rvu0),
dimx(rvx.count()), dimy(rvy.count()),dimu(rvu.count()), 
 A(dimx,dimx), B(dimx,dimu), C(dimy,dimx), D(dimy,dimu),est(rvx), fy(rvy){
//assign cache
//est
_mu = est._mu();
est._R(_P,_iP);

//fy
_yp = fy._mu();
fy._R(_Ry,_iRy);
};

template<class sq_T>
void Kalman<sq_T>::set_parameters(const mat &A0,const  mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0) {
        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; 
}

template<class sq_T>
void Kalman<sq_T>::bayes( const vec &dt) {
        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());
        
        sq_T pom((int)Pfull.rows());
        _iRy->mult_sym_t(C*Pfull,pom);
        (*_P) -= pom; // P = P -PC'iRy*CP;
        (*_yp) = C*(*_mu)+D*u; //y prediction
        (*_mu) += _K*( y-(*_yp) );
        
        if (evalll==true) {
                ll+=fy.evalpdflog(*_yp);
        }
};

//TODO why not const pointer??

template<class sq_T>
EKF<sq_T>::EKF(RV rvx0, RV rvy0, RV rvu0): Kalman<ldmat>(rvx0,rvy0,rvu0){}

template<class sq_T>
void EKF<sq_T>::set_parameters(diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0) {
                pfxu = pfxu0;
                phxu = phxu0;
                
                //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;

        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) {
        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());
        
        sq_T pom((int)Pfull.rows());
        _iRy->mult_sym_t(C*Pfull,pom);
        (*_P) -= pom; // P = P -PC'iRy*CP;
        *_yp = phxu->eval(*_mu,u); //y prediction
        (*_mu) += _K*( y-*_yp );
        
        if (evalll==true) {ll+=est.evalpdflog(y);}
};


#endif // KF_H

---