testKF_QR.cpp itpp/itbase.h estim/libKF.h estim/libPF.h libBM.h work/git/mixpp/tests/testKF_QR.cpp ../itpp_ext.h ../stat/libEF.h estim/libPF.h ../stat/libFN.h itpp/itbase.h ../math/chmat.h estim/libKF.h ../math/libDC.h int int main () main #include<itpp/itbase.h> #include<estim/libKF.h> #include<estim/libPF.h> usingnamespaceitpp; //Theselinesareneededforuseofcoutandendl usingstd::cout; usingstd::endl; intmain(){ //Klamanfilter matA,B,C,D,R,Q,P0; vecmu0; matMu0;//readfrommatlab //inputfromMatlab it_filefin("testKF.it"); matDt,XQRt,eR,eQ; intNdat; boolxxx=fin.seek("d"); if(!xxx){it_error("testKF.itnotfound");} fin>>Dt; fin.seek("A"); fin>>A; fin.seek("B"); fin>>B; fin.seek("C"); fin>>C; fin.seek("D"); fin>>D; fin.seek("R"); fin>>R; fin.seek("Q");fin>>Q; fin.seek("P0");fin>>P0; fin.seek("mu0");fin>>Mu0; mu0=Mu0.get_col(0); Ndat=Dt.cols(); XQRt=zeros(5,Ndat); matXt=zeros(2,Ndat); //cout<<KF; RVrx("{x}","2"); RVru("{u}","1"); RVry("{y}","1"); RVrQR("{Q,R}","3"); // KFcondQRKF(rx,ry,ru,rQR); //KFwithRunknown KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); KF.set_est(mu0,ldmat(P0)); // Kalman<ldmat>KFtr(rx,ry,ru); //KFwithRunknown KFtr.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); KFtr.set_est(mu0,ldmat(P0)); mgammaevolQR(rQR,rQR); evolQR.set_parameters(10.0);//sigma=1/10mu MPF<KFcondQR >KF_QR(rx,rQR,evolQR,evolQR,100,KF); evolQR.condition("111"); epdf&pfinit=evolQR._epdf(); KF_QR.set_est(pfinit); epdf&mpost=KF_QR._epdf(); epdf&mposttr=KFtr._epdf(); XQRt.set_col(0,mpost.mean()); Xt.set_col(0,mposttr.mean()); for(intt=1;t<Ndat;t++){ KF_QR.bayes(Dt.get_col(t)); KFtr.bayes(Dt.get_col(t)); XQRt.set_col(t,mpost.mean()); Xt.set_col(t,mposttr.mean()); } it_filefou("testKF_QR_res.it"); fou<<Name("xqrth")<<XQRt; fou<<Name("xth")<<Xt; //Exitprogram: return0; }