root/library/tests/stresssuite/kalman_stress.cpp @ 1353

Revision 1190, 3.7 kB (checked in by smidl, 14 years ago)

stress

Line 
1
2#include <estim/kalman.h>
3#include "../mat_checks.h"
4
5using namespace bdm;
6
7//These lines are needed for use of cout and endl
8using std::cout;
9using std::endl;
10
11TEST ( kalman_stress ) {
12    // Kalman filter
13    mat A, B, C, D, R, Q, P0;
14    vec mu0;
15    mat Mu0;;
16    // input from Matlab
17    it_file fin ( "kalman_stress.it" );
18
19    mat Dt;
20    int Ndat;
21
22    bool xxx = fin.seek ( "d" );
23    if ( !xxx ) {
24        bdm_error ( "kalman_stress.it not found" );
25    }
26    fin >> Dt;
27    fin.seek ( "A" );
28    fin >> A;
29    fin.seek ( "B" );
30    fin >> B;
31    fin.seek ( "C" );
32    fin >> C;
33    fin.seek ( "D" );
34    fin >> D;
35    fin.seek ( "R" );
36    fin >> R;
37    fin.seek ( "Q" );
38    fin >> Q;
39    fin.seek ( "P0" );
40    fin >> P0;
41    fin.seek ( "mu0" );
42    fin >> Mu0;
43    mu0 = Mu0.get_col ( 0 );
44
45    Ndat = Dt.cols();
46    int dimx = A.rows();
47
48    // Prepare for Kalman filters in BDM:
49    RV rx ( "{x }", vec_1 ( A.cols() ) );
50    RV ru ( "{u }", vec_1 ( B.cols() ) );
51    RV ry ( "{y }", vec_1 ( C.rows() ) );
52
53        // LDMAT
54        EKF_UD KFu;
55        KFu.set_rv(rx);
56        KFu.set_yrv(ry);
57        KFu.set_rvc(ru);
58        shared_ptr<bilinfn> f=new bilinfn; f->set_parameters(A,B);
59        shared_ptr<bilinfn> h=new bilinfn; h->set_parameters(C,D);
60        KFu.set_parameters(f,h,Q,diag(R));
61        KFu.prior()._mu()=mu0;
62        KFu.prior()._R()=ldmat(P0);
63        const epdf& KFuep = KFu.posterior();
64        mat Xtu(dimx,Ndat);
65        Xtu.set_col( 0,KFuep.mean() );
66
67    //Chol
68    KalmanCh KF;
69    KF.set_parameters ( A, B, C, D, chmat ( Q ), chmat ( R ) );
70    KF.set_statistics ( mu0, chmat ( P0 ) ); //prediction!
71    KF.set_evalll ( false );
72    KF.validate();
73    const epdf& KFep = KF.posterior();
74    mat Xt ( dimx, Ndat );
75    Xt.set_col ( 0, KFep.mean() );
76
77    // FULL
78    KalmanFull KF2;
79    KF2.set_parameters ( A, B, C, D,  Q, R );
80    KF2.set_statistics ( mu0, P0 );
81    KF2.set_evalll ( false );
82    KF2.validate();
83    mat Xt2 ( dimx, Ndat );
84    Xt2.set_col ( 0, mu0 );
85
86
87    // EKF
88    shared_ptr<bilinfn> fxu = new bilinfn ( A, B );
89    shared_ptr<bilinfn> hxu = new bilinfn ( C, D );
90    EKFCh KFE;
91    KFE.set_parameters ( fxu, hxu, Q, R );
92    KFE.set_statistics ( mu0, chmat ( P0 ) );
93    KFE.set_evalll ( false );
94    KFE.validate();
95    const epdf& KFEep = KFE.posterior();
96    mat XtE ( dimx, Ndat );
97    XtE.set_col ( 0, KFEep.mean() );
98
99    //test performance of each filter
100    Real_Timer tt;
101    vec exec_times ( 3 ); // KF, KF2, KFE
102
103    vec dt;
104    tt.tic();
105    vec mu=mu0;
106    mat iRy;
107    mat Ry;
108    mat P=P0;
109    mat K;
110    vec ut;
111    vec yt;
112    for ( int t = 1; t < Ndat; t++ ) {
113        dt = Dt.get_col ( t );
114        yt= dt.get ( 0, C.rows() - 1 );
115        ut = dt.get ( C.rows(), dt.length() - 1 ) ;
116       
117        mu = A*mu + B*ut;
118        P  = A*P*A.T() + Q;
119
120        //Data update
121        Ry = C*P*C.T() + R;
122        iRy = inv(Ry);
123        K = P*C.T()*iRy; 
124        P = P- K*C*P; // P = P -KCP;
125        mu = mu + K*(yt-C*mu-D*ut);
126        Xtu.set_col ( t, KFuep.mean() );
127    }
128    exec_times ( 0 ) = tt.toc();
129
130    tt.tic();
131    for ( int t = 1; t < Ndat; t++ ) {
132        dt = Dt.get_col ( t );
133        KF2.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
134        Xt2.set_col ( t, KF2.posterior().mean() );
135    }
136    exec_times ( 1 ) = tt.toc();
137
138    tt.tic();
139    for ( int t = 1; t < Ndat; t++ ) {
140        dt = Dt.get_col ( t );
141        KFE.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
142        XtE.set_col ( t, KFEep.mean() );
143    }
144    exec_times ( 2 ) = tt.toc();
145
146
147    it_file fou ( "kalman_stress_res.it" );
148    fou << Name ( "xthu" ) << Xtu;
149    fou << Name ( "xth2" ) << Xt2;
150    fou << Name ( "xthE" ) << XtE;
151    fou << Name ( "exec_times" ) << exec_times;
152}
Note: See TracBrowser for help on using the browser.