Changeset 477 for library/tests/test_kalman.cpp

Timestamp:

08/05/09 14:40:03 (15 years ago)

Author:

mido

Message:

panove, vite, jak jsem peclivej na upravu kodu.. snad se vam bude libit:) konfigurace je v souboru /system/astylerc

Files:

• library/tests/test_kalman.cpp (modified) (2 diffs)

library/tests/test_kalman.cpp

@@ -10 +10 @@
 int main() {
         // Kalman filter
-        mat A, B,C,D,R,Q,P0;
+        mat A, B, C, D, R, Q, P0;
         vec mu0;
         mat Mu0;;
         // input from Matlab
-        it_file fin( "testKF.it" );
+        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" ); 
+        bool xxx = fin.seek ( "d" );
+        if ( !xxx ) {
+                it_error ( "testKF.it not found" );
+        }
+        fin >> Dt;
+        fin.seek ( "A" );
         fin >> A;
-        fin.seek( "B" ); 
+        fin.seek ( "B" );
         fin >> B;
-        fin.seek( "C" ); 
+        fin.seek ( "C" );
         fin >> C;
-        fin.seek( "D" ); 
+        fin.seek ( "D" );
         fin >> D;
-        fin.seek( "R" ); 
+        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);
-        
+        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()));
-        
+        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);
@@ -50 +55 @@
 //      KF.set_est(mu0,ldmat(P0) );
 //      epdf& KFep = KF.posterior();
-//      mat Xt(2,Ndat); 
+//      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!
+        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() );
-        
-        //      
+        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) );
+        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() );
-        
+        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);
+        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);
+        bilinfn fxu ( A, B );
+        bilinfn hxu ( C, D );
         EKFCh KFE;
-        KFE.set_parameters(&fxu,&hxu,Q,R);
-        KFE.set_est(mu0,chmat(P0));
+        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() );
+        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();
+        vec exec_times ( 4 ); // KF, KFf KF2, KFE
 
         tt.tic();
-        for ( int t=1;t<Ndat;t++ ) {
-                KF2.bayes( Dt.get_col( t ));
-                Xt2.set_col( t,KF2.mu);
+        for ( int t = 1; t < Ndat; t++ ) {
+                KF.bayes ( Dt.get_col ( t ) );
+                Xt.set_col ( t, KFep.mean() );
         }
-        exec_times(2) = tt.toc();
+        exec_times ( 0 ) = tt.toc();
 
         tt.tic();
-        for ( int t=1;t<Ndat;t++ ) {
-                KFE.bayes( Dt.get_col( t ));
-                XtE.set_col( t,KFEep.mean() );
+        for ( int t = 1; t < Ndat; t++ ) {
+                KFf.bayes ( Dt.get_col ( t ) );
+                Xtf.set_col ( t, KFfep.mean() );
         }
-        exec_times(3) = tt.toc();
+        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;
+        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;