root/tests/testKF_QR.cpp @ 141

Revision 33, 1.8 kB (checked in by smidl, 17 years ago)

Oprava PF a MPF + jejich implementace pro pmsm system

Line 
1#include <itpp/itbase.h>
2#include <estim/libKF.h>
3#include <estim/libPF.h>
4
5using namespace itpp;
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){ it_error("testKF.it not found");}
24        fin >>Dt;
25        fin.seek( "A" ); 
26        fin >> A;
27        fin.seek( "B" ); 
28        fin >> B;
29        fin.seek( "C" ); 
30        fin >> C;
31        fin.seek( "D" ); 
32        fin >> D;
33        fin.seek( "R" ); 
34        fin >> R;
35        fin.seek( "Q" ); fin >> Q;
36        fin.seek( "P0" ); fin >> P0;
37        fin.seek( "mu0" ); fin >> Mu0; 
38        mu0=Mu0.get_col(0);
39       
40        Ndat = Dt.cols();
41        XQRt=zeros( 5,Ndat );
42        mat Xt=zeros( 2,Ndat );
43
44//      cout << KF;
45        RV rx("1","{x}","2","0");
46        RV ru("2","{u}","1","0");
47        RV ry("3","{y}","1","0");
48        RV rQR("4","{Q,R}","3","0");
49        //
50        KFcondQR KF(rx,ry,ru,rQR);
51        // KF with R unknown
52        KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
53        KF.set_est(mu0,ldmat(P0) );
54        //
55        Kalman<ldmat> KFtr(rx,ry,ru);
56        // KF with R unknown
57        KFtr.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
58        KFtr.set_est(mu0,ldmat(P0) );
59               
60        mgamma evolQR(rQR,rQR);
61        evolQR.set_parameters(10.0); //sigma = 1/10 mu
62       
63        MPF<KFcondQR > KF_QR(rx,rQR,evolQR,evolQR,100,KF);
64        evolQR.condition("1 1 1");
65        epdf& pfinit=evolQR._epdf();
66        KF_QR.set_est(pfinit);
67        epdf& mpost=KF_QR._epdf();
68        epdf& mposttr=KFtr._epdf();
69
70        XQRt.set_col( 0,mpost.mean());
71        Xt.set_col( 0,mposttr.mean());
72        for ( int t=1;t<Ndat;t++ ) {
73                KF_QR.bayes( Dt.get_col( t ));
74                KFtr.bayes( Dt.get_col( t ));
75               
76                XQRt.set_col(t,mpost.mean());
77                Xt.set_col(t,mposttr.mean());
78        }
79       
80        it_file fou( "testKF_QR_res.it" );
81        fou << Name("xqrth") << XQRt;
82        fou << Name("xth") << Xt;
83        //Exit program:
84        return 0;
85
86}
Note: See TracBrowser for help on using the browser.