Changeset 37 for tests/testKF.cpp

Timestamp:

03/14/08 18:11:21 (16 years ago)

Author:

smidl

Message:

Matrix in Cholesky decomposition, Square-root Kalman and many bug fixes

Files:

• tests/testKF.cpp (modified) (4 diffs)

tests/testKF.cpp

@@ -9 +9 @@
 
 int main() {
-
-
         // Kalman filter
         mat A, B,C,D,R,Q,P0;
@@ -18 +16 @@
         it_file fin( "testKF.it" );
 
-        mat Dt, Xt,Xt2,XtE,Xtf;
+        mat Dt;
         int Ndat;
 
@@ -40 +38 @@
         
         Ndat = Dt.cols();
-        Xt=zeros( 2,Ndat );
-        Xt2=zeros( 2,Ndat );
-        Xtf=zeros( 2,Ndat );
-        XtE=zeros( 2,Ndat );
+        int dimx = A.rows();
+        int dimy = C.rows();
+        int dimu = B.cols();
+        
+        // Prepare for Kalman filters in BDM:
+        ivec tmpsize(1);
+        tmpsize(0) = A.cols();
+        RV rx("1","{x}",tmpsize,"0");
+        tmpsize(0) = B.cols();
+        RV ru("2","{u}",tmpsize,"0");
+        tmpsize(0) = C.rows();
+        RV ry("3","{y}",tmpsize,"0");
+        
+//      // 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._epdf();
+//      mat Xt(2,Ndat); 
+//      Xt.set_col( 0,KFep.mean() );
 
-//      cout << KF;
-        RV rx("1","{x}","2","0");
-        RV ru("2","{u}","1","0");
-        RV ry("3","{y}","1","0");
-        //
-        Kalman<ldmat> KF(rx,ry,ru);
-        KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
-        KF.set_est(mu0,ldmat(P0) );
-        //
-        Kalman<fsqmat> KFf(rx,ry,ru);
-        KFf.set_parameters(A,B,C,D,fsqmat(R),fsqmat(Q));
-        KFf.set_est(mu0,fsqmat(P0) );
-        //
+        //Chol
+        KalmanCh KF(rx,ry,ru);
+        KF.set_parameters(A,B,C,D,chmat(R),chmat(Q));
+        KF.set_est(mu0,chmat(P0) ); //prediction!
+        epdf& KFep = KF._epdf();
+        mat Xt(dimx,Ndat);      
+        Xt.set_col( 0,KFep.mean() );
+        
+        //      
+        // FSQMAT
+        Kalman<ldmat> KFf(rx,ry,ru);
+        KFf.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
+        KFf.set_est(mu0,ldmat(P0) );
+        epdf& KFfep = KFf._epdf();
+        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(rx,ru,A,B);
         bilinfn hxu(rx,ru,C,D);
-        EKF<ldmat> KFE(rx,ry,ru);
+        EKFCh KFE(rx,ry,ru);
         KFE.set_parameters(&fxu,&hxu,Q,R);
-        KFE.set_est(mu0,P0);
+        KFE.set_est(mu0,chmat(P0));
+        epdf& KFEep = KFE._epdf();
+        mat XtE(dimx,Ndat);     
+        XtE.set_col( 0,KFEep.mean() );
 
-        epdf& KFep = KF._epdf();
-        epdf& KFfep = KFf._epdf();
-        epdf& KFEep = KFE._epdf();
+        //test performance of each filter
+        Real_Timer tt;
+        vec exec_times(4); // KF, KFf KF2, KFE
         
-        Xt.set_col( 0,KFep.mean() );
-        Xtf.set_col( 0,KFfep.mean() );
-        Xt2.set_col( 0,KF2.mu );
-        XtE.set_col( 0,KFEep.mean() );
+        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 ));
-                KF.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 ));
-                Xt.set_col( t,KFep.mean() );
-                Xtf.set_col( t,KFfep.mean() );
-                Xt2.set_col(t,KF2.mu);
                 XtE.set_col( t,KFEep.mean() );
         }
+        exec_times(3) = tt.toc();
+
 
         it_file fou( "testKF_res.it" );
@@ -90 +130 @@
         fou << Name("xth2") << Xt2;
         fou << Name("xthE") << XtE;
+        fou << Name("exec_times") << exec_times;
         //Exit program:
         return 0;