testKF_QRexh.cpp itpp/itbase.h estim/libKF.h estim/libPF.h work/git/mixpp/tests/testKF_QRexh.cpp libBM.h ../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); vecvQ1="0.01:0.04:1"; vecvQ2="0.01:0.04:1"; matLL=zeros(vQ1.length(),vQ2.length()); Ndat=Dt.cols(); RVrx("{x}","2"); RVru("{u}","1"); RVry("{y}","2"); // // Kalman<ldmat>KFtr(rx,ry,ru); for(inti=0;i<vQ1.length();i++){ for(intj=0;j<vQ2.length();j++){ //KFwithRunknown matQj=Q; Qj(0,0)=vQ1(i); Qj(1,1)=vQ2(j); KFtr.set_parameters(A,B,C,D,ldmat(R),ldmat(Qj)); KFtr.set_est(mu0,ldmat(P0)); for(intt=1;t<Ndat;t++){ KFtr.bayes(Dt.get_col(t)); LL(i,j)+=KFtr._ll(); } } } it_filefou("testKF_QR_exh.it"); fou<<Name("LL")<<LL; fou<<Name("Q1")<<vQ1; fou<<Name("Q2")<<vQ2; //Exitprogram: return0; }