root/library/tests/test_kalman.cpp @ 565

#include <estim/kalman.h>

using namespace bdm;

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

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

        mat Dt;
        int Ndat;

        bool xxx = fin.seek ( "d" );
        if ( !xxx ) {
                bdm_error ( "testKF.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 = 10;//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
//      Kalman<ldmat> KF(rx,ry,ru);
//      KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
//      KF.set_est(mu0,ldmat(P0) );
//      epdf& KFep = KF.posterior();
//      mat Xt(2,Ndat);
//      Xt.set_col( 0,KFep.mean() );

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

        //
        // FSQMAT
        Kalman<ldmat> KFf;
        KFf.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
        KFf.set_est ( mu0, ldmat ( P0 ) );
        const   epdf& KFfep = KFf.posterior();
        mat Xtf ( dimx, Ndat );
        Xtf.set_col ( 0, KFfep.mean() );

        // FULL
        KalmanFull KF2 ( A, B, C, D, R, Q, P0, mu0 );
        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_est ( mu0, chmat ( P0 ) );
        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 ( 4 ); // KF, KFf KF2, KFE

        tt.tic();
        for ( int t = 1; t < Ndat; t++ ) {
                KF.bayes ( Dt.get_col ( t ) );
                Xt.set_col ( t, KFep.mean() );
        }
        exec_times ( 0 ) = tt.toc();

        tt.tic();
        for ( int t = 1; t < Ndat; t++ ) {
                KFf.bayes ( Dt.get_col ( t ) );
                Xtf.set_col ( t, KFfep.mean() );
        }
        exec_times ( 1 ) = tt.toc();

        tt.tic();
        for ( int t = 1; t < Ndat; t++ ) {
                KF2.bayes ( Dt.get_col ( t ) );
                Xt2.set_col ( t, KF2.mu );
        }
        exec_times ( 2 ) = tt.toc();

        tt.tic();
        for ( int t = 1; t < Ndat; t++ ) {
                KFE.bayes ( Dt.get_col ( t ) );
                XtE.set_col ( t, KFEep.mean() );
        }
        exec_times ( 3 ) = tt.toc();


        it_file fou ( "testKF_res.it" );
        fou << Name ( "xth" ) << Xt;
        fou << Name ( "xthf" ) << Xtf;
        fou << Name ( "xth2" ) << Xt2;
        fou << Name ( "xthE" ) << XtE;
        fou << Name ( "exec_times" ) << exec_times;
        //Exit program:
        return 0;

}