[7] | 1 | #include <itpp/itbase.h> |
---|
[22] | 2 | #include <estim/libKF.h> |
---|
[7] | 3 | |
---|
| 4 | using namespace itpp; |
---|
| 5 | |
---|
| 6 | //These lines are needed for use of cout and endl |
---|
| 7 | using std::cout; |
---|
| 8 | using std::endl; |
---|
| 9 | |
---|
| 10 | int main() { |
---|
[33] | 11 | // Kalman filter |
---|
[8] | 12 | mat A, B,C,D,R,Q,P0; |
---|
| 13 | vec mu0; |
---|
| 14 | mat Mu0;; |
---|
| 15 | // input from Matlab |
---|
[9] | 16 | it_file fin( "testKF.it" ); |
---|
[11] | 17 | |
---|
[37] | 18 | mat Dt; |
---|
[8] | 19 | int Ndat; |
---|
[7] | 20 | |
---|
[18] | 21 | bool xxx= fin.seek( "d" ); |
---|
[28] | 22 | if (!xxx){ it_error("testKF.it not found");} |
---|
[18] | 23 | fin >>Dt; |
---|
[8] | 24 | fin.seek( "A" ); |
---|
| 25 | fin >> A; |
---|
| 26 | fin.seek( "B" ); |
---|
| 27 | fin >> B; |
---|
| 28 | fin.seek( "C" ); |
---|
| 29 | fin >> C; |
---|
| 30 | fin.seek( "D" ); |
---|
| 31 | fin >> D; |
---|
| 32 | fin.seek( "R" ); |
---|
| 33 | fin >> R; |
---|
| 34 | fin.seek( "Q" ); fin >> Q; |
---|
| 35 | fin.seek( "P0" ); fin >> P0; |
---|
[18] | 36 | fin.seek( "mu0" ); fin >> Mu0; |
---|
| 37 | mu0=Mu0.get_col(0); |
---|
[7] | 38 | |
---|
[8] | 39 | Ndat = Dt.cols(); |
---|
[37] | 40 | int dimx = A.rows(); |
---|
| 41 | int dimy = C.rows(); |
---|
| 42 | int dimu = B.cols(); |
---|
| 43 | |
---|
| 44 | // Prepare for Kalman filters in BDM: |
---|
| 45 | ivec tmpsize(1); |
---|
| 46 | tmpsize(0) = A.cols(); |
---|
| 47 | RV rx("1","{x}",tmpsize,"0"); |
---|
| 48 | tmpsize(0) = B.cols(); |
---|
| 49 | RV ru("2","{u}",tmpsize,"0"); |
---|
| 50 | tmpsize(0) = C.rows(); |
---|
| 51 | RV ry("3","{y}",tmpsize,"0"); |
---|
| 52 | |
---|
| 53 | // // LDMAT |
---|
| 54 | // Kalman<ldmat> KF(rx,ry,ru); |
---|
| 55 | // KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); |
---|
| 56 | // KF.set_est(mu0,ldmat(P0) ); |
---|
| 57 | // epdf& KFep = KF._epdf(); |
---|
| 58 | // mat Xt(2,Ndat); |
---|
| 59 | // Xt.set_col( 0,KFep.mean() ); |
---|
[7] | 60 | |
---|
[37] | 61 | //Chol |
---|
| 62 | KalmanCh KF(rx,ry,ru); |
---|
| 63 | KF.set_parameters(A,B,C,D,chmat(R),chmat(Q)); |
---|
| 64 | KF.set_est(mu0,chmat(P0) ); //prediction! |
---|
| 65 | epdf& KFep = KF._epdf(); |
---|
| 66 | mat Xt(dimx,Ndat); |
---|
| 67 | Xt.set_col( 0,KFep.mean() ); |
---|
| 68 | |
---|
| 69 | // |
---|
| 70 | // FSQMAT |
---|
| 71 | Kalman<ldmat> KFf(rx,ry,ru); |
---|
| 72 | KFf.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); |
---|
| 73 | KFf.set_est(mu0,ldmat(P0) ); |
---|
| 74 | epdf& KFfep = KFf._epdf(); |
---|
| 75 | mat Xtf(dimx,Ndat); |
---|
| 76 | Xtf.set_col( 0,KFfep.mean() ); |
---|
| 77 | |
---|
| 78 | // FULL |
---|
[28] | 79 | KalmanFull KF2( A,B,C,D,R,Q,P0,mu0 ); |
---|
[37] | 80 | mat Xt2(dimx,Ndat); |
---|
| 81 | Xt2.set_col( 0,mu0); |
---|
| 82 | |
---|
| 83 | |
---|
| 84 | // EKF |
---|
[22] | 85 | bilinfn fxu(rx,ru,A,B); |
---|
| 86 | bilinfn hxu(rx,ru,C,D); |
---|
[37] | 87 | EKFCh KFE(rx,ry,ru); |
---|
[28] | 88 | KFE.set_parameters(&fxu,&hxu,Q,R); |
---|
[37] | 89 | KFE.set_est(mu0,chmat(P0)); |
---|
| 90 | epdf& KFEep = KFE._epdf(); |
---|
| 91 | mat XtE(dimx,Ndat); |
---|
| 92 | XtE.set_col( 0,KFEep.mean() ); |
---|
[8] | 93 | |
---|
[37] | 94 | //test performance of each filter |
---|
| 95 | Real_Timer tt; |
---|
| 96 | vec exec_times(4); // KF, KFf KF2, KFE |
---|
[32] | 97 | |
---|
[37] | 98 | tt.tic(); |
---|
[7] | 99 | for ( int t=1;t<Ndat;t++ ) { |
---|
[37] | 100 | KF.bayes( Dt.get_col( t )); |
---|
| 101 | Xt.set_col( t,KFep.mean() ); |
---|
| 102 | } |
---|
| 103 | exec_times(0) = tt.toc(); |
---|
| 104 | |
---|
| 105 | tt.tic(); |
---|
| 106 | for ( int t=1;t<Ndat;t++ ) { |
---|
[28] | 107 | KFf.bayes( Dt.get_col( t )); |
---|
[37] | 108 | Xtf.set_col( t,KFfep.mean() ); |
---|
| 109 | } |
---|
| 110 | exec_times(1) = tt.toc(); |
---|
| 111 | |
---|
| 112 | tt.tic(); |
---|
| 113 | for ( int t=1;t<Ndat;t++ ) { |
---|
[8] | 114 | KF2.bayes( Dt.get_col( t )); |
---|
[37] | 115 | Xt2.set_col( t,KF2.mu); |
---|
| 116 | } |
---|
| 117 | exec_times(2) = tt.toc(); |
---|
| 118 | |
---|
| 119 | tt.tic(); |
---|
| 120 | for ( int t=1;t<Ndat;t++ ) { |
---|
[32] | 121 | KFE.bayes( Dt.get_col( t )); |
---|
| 122 | XtE.set_col( t,KFEep.mean() ); |
---|
[7] | 123 | } |
---|
[37] | 124 | exec_times(3) = tt.toc(); |
---|
[7] | 125 | |
---|
[37] | 126 | |
---|
[9] | 127 | it_file fou( "testKF_res.it" ); |
---|
[7] | 128 | fou << Name("xth") << Xt; |
---|
[32] | 129 | fou << Name("xthf") << Xtf; |
---|
[8] | 130 | fou << Name("xth2") << Xt2; |
---|
[22] | 131 | fou << Name("xthE") << XtE; |
---|
[37] | 132 | fou << Name("exec_times") << exec_times; |
---|
[7] | 133 | //Exit program: |
---|
| 134 | return 0; |
---|
| 135 | |
---|
| 136 | } |
---|