root/library/tests/test_kalman_QR.cpp @ 700

Revision 565, 1.9 kB (checked in by vbarta, 15 years ago)

using own error macros (basically copied from IT++, but never aborting)

  • Property svn:eol-style set to native
Line 
1
2#include <estim/kalman.h>
3#include <estim/particles.h>
4
5using namespace bdm;
6
7//These lines are needed for use of cout and endl
8using std::cout;
9using std::endl;
10
11int main() {
12        // Klaman filter
13        mat A, B, C, D, R, Q, P0;
14        vec mu0;
15        mat Mu0;// read from matlab
16        // input from Matlab
17        it_file fin ( "testKF.it" );
18
19        mat Dt, XQRt, eR, eQ;
20        int Ndat;
21
22        bool xxx = fin.seek ( "d" );
23        if ( !xxx ) {
24                bdm_error ( "testKF.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        XQRt = zeros ( 5, Ndat );
47        mat Xt = zeros ( 2, Ndat );
48
49//      cout << KF;
50        RV rx ( "{x }", "2" );
51        RV ru ( "{u }", "1" );
52        RV ry ( "{y }", "1" );
53        RV rQR ( "{Q,R }", "3" );
54        //
55        KFcondQR KF;
56        // KF with R unknown
57        KF.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
58        KF.set_est ( mu0, ldmat ( P0 ) );
59        //
60        Kalman<ldmat> KFtr;
61        // KF with R unknown
62        KFtr.set_parameters ( A, B, C, D, ldmat ( R ), ldmat ( Q ) );
63        KFtr.set_est ( mu0, ldmat ( P0 ) );
64
65        mgamma evolQR;
66        evolQR.set_parameters ( 10.0, "1 1 1" ); //sigma = 1/10 mu
67
68        MPF<KFcondQR> KF_QR;
69        KF_QR.set_parameters ( &evolQR, &evolQR, 100 );
70        evolQR.condition ( "1 1 1" );
71        egamma kfinit=egamma("10 10 10","1 1 1");
72        KF_QR.set_statistics ( egamma("10 10 10","1 1 1"), &KF );
73        const epdf& mpost = KF_QR.posterior();
74        const epdf& mposttr = KFtr.posterior();
75
76        XQRt.set_col ( 0, mpost.mean() );
77        Xt.set_col ( 0, mposttr.mean() );
78        for ( int t = 1; t < Ndat; t++ ) {
79                KF_QR.bayes ( Dt.get_col ( t ) );
80                KFtr.bayes ( Dt.get_col ( t ) );
81
82                XQRt.set_col ( t, mpost.mean() );
83                Xt.set_col ( t, mposttr.mean() );
84        }
85
86        it_file fou ( "testKF_QR_res.it" );
87        fou << Name ( "xqrth" ) << XQRt;
88        fou << Name ( "xth" ) << Xt;
89        //Exit program:
90        return 0;
91
92}
Note: See TracBrowser for help on using the browser.