root/tests/testKF.cpp @ 271

#include <estim/libKF.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){ it_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
        bilinfn fxu(A,B);
        bilinfn hxu(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;

}