root/library/tests/stresssuite/kalman_stress.cpp @ 1353

#include <estim/kalman.h>
#include "../mat_checks.h"

using namespace bdm;

//These lines are needed for use of cout and endl
using std::cout;
using std::endl;

TEST ( kalman_stress ) {
    // Kalman filter
    mat A, B, C, D, R, Q, P0;
    vec mu0;
    mat Mu0;;
    // input from Matlab
    it_file fin ( "kalman_stress.it" );

    mat Dt;
    int Ndat;

    bool xxx = fin.seek ( "d" );
    if ( !xxx ) {
        bdm_error ( "kalman_stress.it not found" );
    }
    fin >> Dt;
    fin.seek ( "A" );
    fin >> A;
    fin.seek ( "B" );
    fin >> B;
    fin.seek ( "C" );
    fin >> C;
    fin.seek ( "D" );
    fin >> D;
    fin.seek ( "R" );
    fin >> R;
    fin.seek ( "Q" );
    fin >> Q;
    fin.seek ( "P0" );
    fin >> P0;
    fin.seek ( "mu0" );
    fin >> Mu0;
    mu0 = Mu0.get_col ( 0 );

    Ndat = Dt.cols();
    int dimx = A.rows();

    // Prepare for Kalman filters in BDM:
    RV rx ( "{x }", vec_1 ( A.cols() ) );
    RV ru ( "{u }", vec_1 ( B.cols() ) );
    RV ry ( "{y }", vec_1 ( C.rows() ) );

        // LDMAT
        EKF_UD KFu;
        KFu.set_rv(rx);
        KFu.set_yrv(ry);
        KFu.set_rvc(ru);
        shared_ptr<bilinfn> f=new bilinfn; f->set_parameters(A,B);
        shared_ptr<bilinfn> h=new bilinfn; h->set_parameters(C,D);
        KFu.set_parameters(f,h,Q,diag(R));
        KFu.prior()._mu()=mu0;
        KFu.prior()._R()=ldmat(P0);
        const epdf& KFuep = KFu.posterior();
        mat Xtu(dimx,Ndat);
        Xtu.set_col( 0,KFuep.mean() );

    //Chol
    KalmanCh KF;
    KF.set_parameters ( A, B, C, D, chmat ( Q ), chmat ( R ) );
    KF.set_statistics ( mu0, chmat ( P0 ) ); //prediction!
    KF.set_evalll ( false );
    KF.validate();
    const epdf& KFep = KF.posterior();
    mat Xt ( dimx, Ndat );
    Xt.set_col ( 0, KFep.mean() );

    // FULL
    KalmanFull KF2;
    KF2.set_parameters ( A, B, C, D,  Q, R );
    KF2.set_statistics ( mu0, P0 );
    KF2.set_evalll ( false );
    KF2.validate();
    mat Xt2 ( dimx, Ndat );
    Xt2.set_col ( 0, mu0 );


    // EKF
    shared_ptr<bilinfn> fxu = new bilinfn ( A, B );
    shared_ptr<bilinfn> hxu = new bilinfn ( C, D );
    EKFCh KFE;
    KFE.set_parameters ( fxu, hxu, Q, R );
    KFE.set_statistics ( mu0, chmat ( P0 ) );
    KFE.set_evalll ( false );
    KFE.validate();
    const epdf& KFEep = KFE.posterior();
    mat XtE ( dimx, Ndat );
    XtE.set_col ( 0, KFEep.mean() );

    //test performance of each filter
    Real_Timer tt;
    vec exec_times ( 3 ); // KF, KF2, KFE

    vec dt;
    tt.tic();
    vec mu=mu0;
    mat iRy;
    mat Ry;
    mat P=P0;
    mat K;
    vec ut;
    vec yt;
    for ( int t = 1; t < Ndat; t++ ) {
        dt = Dt.get_col ( t );
        yt= dt.get ( 0, C.rows() - 1 );
        ut = dt.get ( C.rows(), dt.length() - 1 ) ;
        
        mu = A*mu + B*ut;
        P  = A*P*A.T() + Q;

        //Data update
        Ry = C*P*C.T() + R;
        iRy = inv(Ry);
        K = P*C.T()*iRy; 
        P = P- K*C*P; // P = P -KCP;
        mu = mu + K*(yt-C*mu-D*ut);
        Xtu.set_col ( t, KFuep.mean() );
    }
    exec_times ( 0 ) = tt.toc();

    tt.tic();
    for ( int t = 1; t < Ndat; t++ ) {
        dt = Dt.get_col ( t );
        KF2.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
        Xt2.set_col ( t, KF2.posterior().mean() );
    }
    exec_times ( 1 ) = tt.toc();

    tt.tic();
    for ( int t = 1; t < Ndat; t++ ) {
        dt = Dt.get_col ( t );
        KFE.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
        XtE.set_col ( t, KFEep.mean() );
    }
    exec_times ( 2 ) = tt.toc();


    it_file fou ( "kalman_stress_res.it" );
    fou << Name ( "xthu" ) << Xtu;
    fou << Name ( "xth2" ) << Xt2;
    fou << Name ( "xthE" ) << XtE;
    fou << Name ( "exec_times" ) << exec_times;
}