Changeset 22 for tests/testKF.cpp
- Timestamp:
- 02/18/08 17:50:37 (16 years ago)
- Files:
-
- 1 modified
-
tests/testKF.cpp (modified) (4 diffs)
Legend:
- Unmodified
- Added
- Removed
-
tests/testKF.cpp
r19 r22 1 1 #include <itpp/itbase.h> 2 #include "../bdm/estim/libKF.h"2 #include <estim/libKF.h> 3 3 4 4 using namespace itpp; … … 18 18 it_file fin( "testKF.it" ); 19 19 20 mat Dt, Xt,Xt2 ;20 mat Dt, Xt,Xt2,XtE; 21 21 int Ndat; 22 22 … … 41 41 Xt=zeros( 2,Ndat ); 42 42 Xt2=zeros( 2,Ndat ); 43 XtE=zeros( 2,Ndat ); 43 44 44 Kalman< ldmat> KF( A,B,C,D,ldmat(diag(R)),ldmat(diag(Q)),ldmat(diag(P0)),mu0 );45 Kalman<fsqmat> KF( A,B,C,D,fsqmat(R),fsqmat(Q),fsqmat(P0),mu0 ); 45 46 // cout << KF; 46 47 KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 ); 48 RV rx("1","{x}","2","0","0"); 49 RV ru("2","{u}","1","0","0"); 50 RV ry("3","{y}","1","0","0"); 51 bilinfn fxu(rx,ru,A,B); 52 bilinfn hxu(rx,ru,C,D); 53 EKF<fsqmat> KFE(&fxu,&hxu,Q,R,mu0,P0); 47 54 48 55 Xt.set_col( 0,KF.mu ); 49 56 Xt2.set_col( 0,KF.mu ); 57 XtE.set_col( 0,KF.mu ); 50 58 for ( int t=1;t<Ndat;t++ ) { 51 59 KF.bayes( Dt.get_col( t )); 52 60 KF2.bayes( Dt.get_col( t )); 61 KFE.bayes( Dt.get_col( t )); 53 62 Xt.set_col(t,KF.mu); 54 63 Xt2.set_col(t,KF2.mu); 64 XtE.set_col(t,KFE.mu); 55 65 // Kmu = KF.mu; 56 66 // cout << "t:" <<t<< " " << dt<<" "<<Kmu <<endl; … … 60 70 fou << Name("xth") << Xt; 61 71 fou << Name("xth2") << Xt2; 72 fou << Name("xthE") << XtE; 62 73 //Exit program: 63 74 return 0;
