root/tests/testKF_QR.cpp @ 262

Revision 262, 1.7 kB (checked in by smidl, 16 years ago)

cleanup of include files

RevLine 
[262]1
[32]2#include <estim/libKF.h>
3#include <estim/libPF.h>
4
[254]5using namespace bdm;
[32]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
[33]19        mat Dt, XQRt,eR,eQ;
[32]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();
[33]41        XQRt=zeros( 5,Ndat );
42        mat Xt=zeros( 2,Ndat );
[32]43
44//      cout << KF;
[162]45        RV rx("{x }","2");
46        RV ru("{u }","1");
47        RV ry("{y }","1");
48        RV rQR("{Q,R }","3");
[32]49        //
[33]50        KFcondQR KF(rx,ry,ru,rQR);
[32]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        //
[33]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
[32]62       
[33]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);
[170]67        const epdf& mpost=KF_QR._epdf();
68        const epdf& mposttr=KFtr._epdf();
[32]69
[33]70        XQRt.set_col( 0,mpost.mean());
71        Xt.set_col( 0,mposttr.mean());
[32]72        for ( int t=1;t<Ndat;t++ ) {
[33]73                KF_QR.bayes( Dt.get_col( t ));
[32]74                KFtr.bayes( Dt.get_col( t ));
75               
[33]76                XQRt.set_col(t,mpost.mean());
77                Xt.set_col(t,mposttr.mean());
[32]78        }
79       
[33]80        it_file fou( "testKF_QR_res.it" );
81        fou << Name("xqrth") << XQRt;
82        fou << Name("xth") << Xt;
[32]83        //Exit program:
84        return 0;
85
86}
Note: See TracBrowser for help on using the browser.