testKF.cpp itpp/itbase.h estim/libKF.h libBM.h ../itpp_ext.h ../stat/libEF.h ../stat/libFN.h itpp/itbase.h ../math/chmat.h estim/libKF.h ../math/libDC.h work/git/mixpp/tests/testKF.cpp int int main () main #include<itpp/itbase.h> #include<estim/libKF.h> usingnamespaceitpp; //Theselinesareneededforuseofcoutandendl usingstd::cout; usingstd::endl; intmain(){ //Kalmanfilter matA,B,C,D,R,Q,P0; vecmu0; matMu0;; //inputfromMatlab it_filefin("testKF.it"); matDt; 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=10;//Dt.cols(); intdimx=A.rows(); //PrepareforKalmanfiltersinBDM: RVrx("{x}",vec_1(A.cols())); RVru("{u}",vec_1(B.cols())); RVry("{y}",vec_1(C.rows())); ////LDMAT //Kalman<ldmat>KF(rx,ry,ru); //KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); //KF.set_est(mu0,ldmat(P0)); //epdf&KFep=KF._epdf(); //matXt(2,Ndat); //Xt.set_col(0,KFep.mean()); //Chol KalmanChKF(rx,ry,ru); KF.set_parameters(A,B,C,D,chmat(R),chmat(Q)); KF.set_est(mu0,chmat(P0));//prediction! epdf&KFep=KF._epdf(); matXt(dimx,Ndat); Xt.set_col(0,KFep.mean()); // //FSQMAT Kalman<ldmat>KFf(rx,ry,ru); KFf.set_parameters(A,B,C,D,ldmat(R),ldmat(Q)); KFf.set_est(mu0,ldmat(P0)); epdf&KFfep=KFf._epdf(); matXtf(dimx,Ndat); Xtf.set_col(0,KFfep.mean()); //FULL KalmanFullKF2(A,B,C,D,R,Q,P0,mu0); matXt2(dimx,Ndat); Xt2.set_col(0,mu0); //EKF bilinfnfxu(rx,ru,A,B); bilinfnhxu(rx,ru,C,D); EKFChKFE(rx,ry,ru); KFE.set_parameters(&fxu,&hxu,Q,R); KFE.set_est(mu0,chmat(P0)); epdf&KFEep=KFE._epdf(); matXtE(dimx,Ndat); XtE.set_col(0,KFEep.mean()); //testperformanceofeachfilter Real_Timertt; vecexec_times(4);//KF,KFfKF2,KFE tt.tic(); for(intt=1;t<Ndat;t++){ KF.bayes(Dt.get_col(t)); Xt.set_col(t,KFep.mean()); } exec_times(0)=tt.toc(); tt.tic(); for(intt=1;t<Ndat;t++){ KFf.bayes(Dt.get_col(t)); Xtf.set_col(t,KFfep.mean()); } exec_times(1)=tt.toc(); tt.tic(); for(intt=1;t<Ndat;t++){ KF2.bayes(Dt.get_col(t)); Xt2.set_col(t,KF2.mu); } exec_times(2)=tt.toc(); tt.tic(); for(intt=1;t<Ndat;t++){ KFE.bayes(Dt.get_col(t)); XtE.set_col(t,KFEep.mean()); } exec_times(3)=tt.toc(); it_filefou("testKF_res.it"); fou<<Name("xth")<<Xt; fou<<Name("xthf")<<Xtf; fou<<Name("xth2")<<Xt2; fou<<Name("xthE")<<XtE; fou<<Name("exec_times")<<exec_times; //Exitprogram: return0; }