root/bdm/estim/libKF.cpp @ 67

#include <itpp/itbase.h>
#include "libKF.h"

using namespace itpp;

using std::endl;

KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
        dimx = A0.rows();
        dimu = B0.cols();
        dimy = C0.rows();

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

        A = A0;
        B = B0;
        C = C0;
        D = D0;
        R = R0;
        Q = Q0;
        mu = mu0;
        P = P0;
        
        ll = 0.0;
        evalll=true;
}

void KalmanFull::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;

        //Data update
        _Ry = C*P*C.transpose() + R;
        _iRy = inv ( _Ry );
        _K = P*C.transpose() *_iRy;
        P -= _K*C*P; // P = P -KCP;
        mu += _K* ( y-C*mu-D*u );

        if ( evalll ) {
                //from enorm
                vec x=y-C*mu-D*u;
                ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) );
        }
};

std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) {
        os << "mu:" << kf.mu << endl << "P:" << kf.P <<endl;
        return os;
}



 /////////////////////////////// EKFS
EKFfull::EKFfull ( RV rvx0, RV rvy0, RV rvu0 ) : BM ( rvx0 ),E(rvx0) {};

void EKFfull::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const mat Q0,const mat R0 ) {
        pfxu = pfxu0;
        phxu = phxu0;

        dimx = rv.count();
        dimy = phxu0->_dimy();
        dimu = phxu0->_dimu();

        A.set_size(dimx,dimx);
        C.set_size(dimy,dimx);
        //initialize matrices A C, later, these will be only updated!
        pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true );
        B.clear();
        phxu->dfdx_cond ( mu,zeros ( dimu ),C,true );
        D.clear();

        R = R0;
        Q = Q0;
}

void EKFfull::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 );
        
        pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true );
        phxu->dfdx_cond ( mu,zeros ( dimu ),C,true );

        //Time update
        mu = pfxu->eval(mu,u);// A*mu + B*u;
        P  = A*P*A.transpose() + Q;

        //Data update
        _Ry = C*P*C.transpose() + R;
        _iRy = inv ( _Ry );
        _K = P*C.transpose() *_iRy;
        P -= _K*C*P; // P = P -KCP;
        vec yp = phxu->eval(mu,u);
        mu += _K* ( y-yp );

        E.set_mu(mu);

        if ( BM::evalll ) {
                //from enorm
                vec x=y-yp;
                BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) );
        }
};



void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) {

        ( ( Kalman<chmat>* ) this )->set_parameters ( A0,B0,C0,D0,R0,Q0 );
        // Cholesky special!
        preA.clear();
        preA.set_submatrix ( 0,0,R._Ch() );
        preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
}


void KalmanCh::bayes ( const vec &dt ) {

        vec u = dt.get ( dimy,dimy+dimu-1 );
        vec y = dt.get ( 0,dimy-1 );

        //TODO get rid of Q in qr()!
//      mat Q;

        //R and Q are already set in set_parameters()
        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
        //Fixme can be more efficient if .T() is not used
        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );

//      if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" );
        if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );

        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
        _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
        _Ry->inv ( *_iRy );
        fy._cached ( true );
        *_mu = A* ( *_mu ) + B*u + ( _K ) * ( _iRy->_Ch() ).T() * ( y-C* ( *_mu )-D*u );

        /*      cout << "P:" <<_P->to_mat() <<endl;
                cout << "Ry:" <<_Ry->to_mat() <<endl;
                cout << "iRy:" <<_iRy->to_mat() <<endl;*/
        if ( evalll==true ) { //likelihood of observation y
                ll=fy.evalpdflog ( y );
        }
}


EKFCh::EKFCh ( RV rvx0, RV rvy0, RV rvu0 ) : KalmanCh ( rvx0,rvy0,rvu0 ) {}

void EKFCh::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const chmat Q0,const chmat 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 );
        B.clear();
        phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
        D.clear();

        R = R0;
        Q = Q0;

        // Cholesky special!
        preA.clear();
        preA.set_submatrix ( 0,0,R._Ch() );
        preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
}


void EKFCh::bayes ( const vec &dt ) {

        vec u = dt.get ( dimy,dimy+dimu-1 );
        vec y = dt.get ( 0,dimy-1 );

        pfxu->dfdx_cond ( *_mu,u,A,false ); //update A by a derivative of fx
        phxu->dfdx_cond ( *_mu,u,C,false ); //update A by a derivative of fx

        //R and Q are already set in set_parameters()
        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
        //Fixme can be more efficient if .T() is not used
        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );

//      mat Sttm = _P->to_mat();

//      if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" );
        if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );

        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
        _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
        _Ry->inv ( *_iRy );
        fy._cached ( true );
        
//      mat iRY = inv(_Ry->to_mat());
//      mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm;
//      mat _K2 = Stt*C.T()*inv(R.to_mat());

        // prediction
        *_mu = pfxu->eval ( *_mu ,u );

        *_yp = phxu->eval ( *_mu,u );
        
/*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/
        
        //correction //= initial value is already prediction!
        *_mu += ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );

/*      _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() );
        *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/
        
        /*      cout << "P:" <<_P->to_mat() <<endl;
                cout << "Ry:" <<_Ry->to_mat() <<endl;
                cout << "iRy:" <<_iRy->to_mat() <<endl;*/

        if ( evalll==true ) { //likelihood of observation y
                ll=fy.evalpdflog ( y );
        }
}

void KFcondQR::condition ( const vec &QR ) {
        it_assert_debug ( QR.length() == ( rvc.count() ),"KFcondRQ: conditioning by incompatible vector" );

        Q.setD ( QR ( 0, dimx-1 ) );
        R.setD ( QR ( dimx, -1 ) );
};

void KFcondR::condition ( const vec &R0 ) {
        it_assert_debug ( R0.length() == ( rvc.count() ),"KFcondR: conditioning by incompatible vector" );

        R.setD ( R0 );
};