1 | #include <itpp/itbase.h> |
---|
2 | #include <estim/libKF.h> |
---|
3 | #include <estim/libPF.h> |
---|
4 | |
---|
5 | using namespace itpp; |
---|
6 | |
---|
7 | //These lines are needed for use of cout and endl |
---|
8 | using std::cout; |
---|
9 | using std::endl; |
---|
10 | |
---|
11 | int 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, XRt,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 | XRt=zeros( 3,Ndat ); |
---|
42 | |
---|
43 | // cout << KF; |
---|
44 | RV rx("1","{x}","2","0"); |
---|
45 | RV ru("2","{u}","1","0"); |
---|
46 | RV ry("3","{y}","1","0"); |
---|
47 | RV rR("4","{R}","1","0"); |
---|
48 | // |
---|
49 | KFcondR KF(rx,ry,ru,rR); |
---|
50 | Kalman<fsqmat> KFtr(rx,ry,ru); |
---|
51 | Kalman<fsqmat> KFtr2(rx,ry,ru); |
---|
52 | //correct KF |
---|
53 | KFtr.set_parameters(A,B,C,D,fsqmat(R),fsqmat(Q)); |
---|
54 | KFtr.set_est(mu0,fsqmat(P0) ); |
---|
55 | // KF with lower R |
---|
56 | mat Q2=Q/1.0; |
---|
57 | mat R2=R/20.0; |
---|
58 | KFtr2.set_parameters(A,B,C,D,fsqmat(R2),fsqmat(Q2)); |
---|
59 | KFtr2.set_est(mu0,fsqmat(P0) ); |
---|
60 | // KF with R unknown |
---|
61 | KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); |
---|
62 | KF.set_est(mu0,ldmat(P0) ); |
---|
63 | // |
---|
64 | mgamma evolR(rR,rR); |
---|
65 | evolR.set_parameters(10.0); //sigma = 1/10 mu |
---|
66 | |
---|
67 | MPF<KFcondR > KF_R(rx,rR,evolR,evolR,10,KF); |
---|
68 | evolR.condition("0.1"); |
---|
69 | epdf& pfinit=evolR._epdf(); |
---|
70 | KF_R.set_est(pfinit); |
---|
71 | epdf& mpost=KF_R._epdf(); |
---|
72 | |
---|
73 | cout << mpost.mean()<<endl; |
---|
74 | |
---|
75 | XRt.set_col( 0,mpost.mean()); |
---|
76 | double ll1=0.0; |
---|
77 | double ll2=0.0; |
---|
78 | for ( int t=1;t<Ndat;t++ ) { |
---|
79 | // KF_R.bayes( Dt.get_col( t )); |
---|
80 | KFtr.bayes( Dt.get_col( t )); |
---|
81 | KFtr2.bayes( Dt.get_col( t )); |
---|
82 | |
---|
83 | ll1+=KFtr._ll(); |
---|
84 | ll2+=KFtr2._ll(); |
---|
85 | |
---|
86 | XRt.set_col(t,mpost.mean()); |
---|
87 | } |
---|
88 | |
---|
89 | cout << ll1 << " " << ll2 <<endl; |
---|
90 | |
---|
91 | it_file fou( "testKF_R_res.it" ); |
---|
92 | fou << Name("xqrth") << XRt; |
---|
93 | //Exit program: |
---|
94 | return 0; |
---|
95 | |
---|
96 | } |
---|