libKF.cpp itpp/itbase.h libKF.h libBM.h ../itpp_ext.h ../stat/libEF.h ../stat/libFN.h itpp/itbase.h work/git/mixpp/bdm/estim/libKF.cpp ../math/chmat.h libKF.h ../math/libDC.h std::ostream & std::ostream& operator<< (std::ostream &os, const KalmanFull &kf) operator<< std::ostream & os const KalmanFull & kf #include<itpp/itbase.h> #include"libKF.h" usingnamespaceitpp; usingstd::endl; KalmanFull::KalmanFull(matA0,matB0,matC0,matD0,matR0,matQ0,matP0,vecmu0){ dimx=A0.rows(); dimu=B0.cols(); dimy=C0.rows(); it_assert_debug(A0.cols()==dimx,"KalmanFull:Aisnotsquare"); it_assert_debug(B0.rows()==dimx,"KalmanFull:Bisnotcompatible"); it_assert_debug(C0.cols()==dimx,"KalmanFull:Cisnotsquare"); it_assert_debug((D0.rows()==dimy)||(D0.cols()==dimu),"KalmanFull:Disnotcompatible"); it_assert_debug((R0.cols()==dimy)||(R0.rows()==dimy),"KalmanFull:Risnotcompatible"); it_assert_debug((Q0.cols()==dimx)||(Q0.rows()==dimx),"KalmanFull:Qisnotcompatible"); A=A0; B=B0; C=C0; D=D0; R=R0; Q=Q0; mu=mu0; P=P0; ll=0.0; evalll=true; } voidKalmanFull::bayes(constvec&dt){ it_assert_debug(dt.length()==(dimy+dimu),"KalmanFull::bayeswrongsizeofdt"); vecu=dt.get(dimy,dimy+dimu-1); vecy=dt.get(0,dimy-1); //Timeupdate mu=A*mu+B*u; P=A*P*A.transpose()+Q; //Dataupdate _Ry=C*P*C.transpose()+R; _iRy=inv(_Ry); _K=P*C.transpose()*_iRy; P-=_K*C*P;//P=P-KCP; mu+=_K*(y-C*mu-D*u); if(evalll){ //fromenorm vecx=y-C*mu-D*u; ll=-0.5*(R.cols()*1.83787706640935+log(det(_Ry))+x*(_iRy*x)); } }; std::ostream&operator<< (std::ostream&os,constKalmanFull&kf){ os<<"mu:"<<kf.mu<<endl<<"P:"<<kf.P<<endl; returnos; } EKFfull::EKFfull(RVrvx0,RVrvy0,RVrvu0):BM(rvx0),E(rvx0){}; voidEKFfull::set_parameters(diffbifn*pfxu0,diffbifn*phxu0,constmatQ0,constmatR0){ pfxu=pfxu0; phxu=phxu0; dimx=rv.count(); dimy=phxu0->_dimy(); dimu=phxu0->_dimu(); A.set_size(dimx,dimx); C.set_size(dimy,dimx); //initializematricesAC,later,thesewillbeonlyupdated! pfxu->dfdx_cond(mu,zeros(dimu),A,true); B.clear(); phxu->dfdx_cond(mu,zeros(dimu),C,true); D.clear(); R=R0; Q=Q0; } voidEKFfull::bayes(constvec&dt){ it_assert_debug(dt.length()==(dimy+dimu),"KalmanFull::bayeswrongsizeofdt"); vecu=dt.get(dimy,dimy+dimu-1); vecy=dt.get(0,dimy-1); pfxu->dfdx_cond(mu,zeros(dimu),A,true); phxu->dfdx_cond(mu,zeros(dimu),C,true); //Timeupdate mu=pfxu->eval(mu,u);//A*mu+B*u; P=A*P*A.transpose()+Q; //Dataupdate _Ry=C*P*C.transpose()+R; _iRy=inv(_Ry); _K=P*C.transpose()*_iRy; P-=_K*C*P;//P=P-KCP; vecyp=phxu->eval(mu,u); mu+=_K*(y-yp); E.set_mu(mu); if(BM::evalll){ //fromenorm vecx=y-yp; BM::ll=-0.5*(R.cols()*1.83787706640935+log(det(_Ry))+x*(_iRy*x)); } }; voidKalmanCh::set_parameters(constmat&A0,constmat&B0,constmat&C0,constmat&D0,constchmat&R0,constchmat&Q0){ ((Kalman<chmat>*)this)->set_parameters(A0,B0,C0,D0,R0,Q0); //Choleskyspecial! preA.clear(); preA.set_submatrix(0,0,R._Ch()); preA.set_submatrix(dimy+dimx,dimy,Q._Ch()); } voidKalmanCh::bayes(constvec&dt){ vecu=dt.get(dimy,dimy+dimu-1); vecy=dt.get(0,dimy-1); vecpom(dimy); //TODOgetridofQinqr()! //matQ; //RandQarealreadysetinset_parameters() preA.set_submatrix(dimy,0,(_P._Ch())*C.T()); //Fixmecanbemoreefficientif.T()isnotused preA.set_submatrix(dimy,dimy,(_P._Ch())*A.T()); //if(!qr(preA,Q,postA))it_warning("QRinkalmanunstable!"); if(!qr(preA,postA)){it_warning("QRinkalmanunstable!");} (_Ry._Ch())=postA(0,dimy-1,0,dimy-1); _K=inv(A)*(postA(0,dimy-1,dimy,dimy+dimx-1)).T(); (_P._Ch())=postA(dimy,dimy+dimx-1,dimy,dimy+dimx-1); _mu=A*(_mu)+B*u; _yp=C*_mu-D*u; backward_substitution(_Ry._Ch(),(y-_yp),pom); _mu+=(_K)*pom; cout<<"P:"<<_P.to_mat()<<endl; cout<<"Ry:"<<_Ry.to_mat()<<endl; cout<<"_K:"<<_K<<endl; if(evalll==true){//likelihoodofobservationy ll=fy.evalpdflog(y); } } EKFCh::EKFCh(RVrvx0,RVrvy0,RVrvu0):KalmanCh(rvx0,rvy0,rvu0){} voidEKFCh::set_parameters(diffbifn*pfxu0,diffbifn*phxu0,constchmatQ0,constchmatR0){ pfxu=pfxu0; phxu=phxu0; //initializematricesAC,later,thesewillbeonlyupdated! pfxu->dfdx_cond(_mu,zeros(dimu),A,true); //pfxu->dfdu_cond(*_mu,zeros(dimu),B,true); B.clear(); phxu->dfdx_cond(_mu,zeros(dimu),C,true); //phxu->dfdu_cond(*_mu,zeros(dimu),D,true); D.clear(); R=R0; Q=Q0; //Choleskyspecial! preA.clear(); preA.set_submatrix(0,0,R._Ch()); preA.set_submatrix(dimy+dimx,dimy,Q._Ch()); } voidEKFCh::bayes(constvec&dt){ vecpom(dimy); vecu=dt.get(dimy,dimy+dimu-1); vecy=dt.get(0,dimy-1); pfxu->dfdx_cond(_mu,u,A,false);//updateAbyaderivativeoffx phxu->dfdx_cond(_mu,u,C,false);//updateAbyaderivativeoffx //RandQarealreadysetinset_parameters() preA.set_submatrix(dimy,0,(_P._Ch())*C.T()); //Fixmecanbemoreefficientif.T()isnotused preA.set_submatrix(dimy,dimy,(_P._Ch())*A.T()); //matSttm=_P->to_mat(); //if(!qr(preA,Q,postA))it_warning("QRinkalmanunstable!"); if(!qr(preA,postA)){it_warning("QRinkalmanunstable!");} (_Ry._Ch())=postA(0,dimy-1,0,dimy-1); _K=inv(A)*(postA(0,dimy-1,dimy,dimy+dimx-1)).T(); (_P._Ch())=postA(dimy,dimy+dimx-1,dimy,dimy+dimx-1); //matiRY=inv(_Ry->to_mat()); //matStt=Sttm-Sttm*C.T()*iRY*C*Sttm; //mat_K2=Stt*C.T()*inv(R.to_mat()); //prediction _mu=pfxu->eval(_mu,u); _yp=phxu->eval(_mu,u); /*vecmu2=*_mu+(_K2)*(y-*_yp);*/ //correction//=initialvalueisalreadyprediction! backward_substitution(_Ry._Ch(),(y-_yp),pom); _mu+=(_K)*pom; /*_K=(_P->to_mat())*C.transpose()*(_iRy->to_mat()); *_mu=pfxu->eval(*_mu,u)+(_K)*(y-*_yp);*/ cout<<"P:"<<_P.to_mat()<<endl; cout<<"Ry:"<<_Ry.to_mat()<<endl; cout<<"_K:"<<_K<<endl; if(evalll==true){//likelihoodofobservationy ll=fy.evalpdflog(y); } } voidKFcondQR::condition(constvec&QR){ it_assert_debug(QR.length()==(rvc.count()),"KFcondRQ:conditioningbyincompatiblevector"); Q.setD(QR(0,dimx-1)); R.setD(QR(dimx,-1)); }; voidKFcondR::condition(constvec&R0){ it_assert_debug(R0.length()==(rvc.count()),"KFcondR:conditioningbyincompatiblevector"); R.setD(R0); };