libKF.h itpp/itbase.h ../stat/libFN.h ../stat/libEF.h ../math/chmat.h work/git/mixpp/bdm/estim/ekf_templ.h work/git/mixpp/bdm/estim/libKF.cpp work/git/mixpp/pmsm/pmsm_sim.cpp work/git/mixpp/pmsm/pmsm_sim2.cpp work/git/mixpp/pmsm/pmsm_sim3.cpp work/git/mixpp/pmsm/pmsm_unkQ.cpp work/git/mixpp/pmsm/pmsm_unkQpf.cpp work/git/mixpp/pmsm/sim_var.cpp work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.h work/git/mixpp/tests/testKF.cpp work/git/mixpp/tests/testKF_QR.cpp work/git/mixpp/tests/testKF_QRexh.cpp libBM.h ../itpp_ext.h ../stat/libEF.h ../stat/libFN.h itpp/itbase.h ../math/chmat.h work/git/mixpp/bdm/estim/libKF.h ../math/libDC.h work/git/mixpp/bdm/estim/ekf_templ.h work/git/mixpp/pmsm/sim_var.cpp work/git/mixpp/bdm/estim/libKF.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.h work/git/mixpp/pmsm/pmsm_mix.cpp KalmanFull Kalman KalmanCh EKFfull EKF EKFCh KFcondQR KFcondR Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions. Vaclav Smidl. ----------------------------------- BDM++ - C++ library for Bayesian Decision Making under UncertaintyUsing IT++ for numerical operations ----------------------------------- #ifndefKF_H #defineKF_H #include<itpp/itbase.h> #include"../stat/libFN.h" #include"../stat/libEF.h" #include"../math/chmat.h" usingnamespaceitpp; classKalmanFull{ protected: intdimx,dimy,dimu; matA,B,C,D,R,Q; //cache mat_Pp,_Ry,_iRy,_K; public: //posterior vecmu; matP; boolevalll; doublell; public: KalmanFull(matA,matB,matC,matD,matR,matQ,matP0,vecmu0); voidbayes(constvec&dt); friendstd::ostream&operator<< (std::ostream&os,constKalmanFull&kf); KalmanFull(){}; }; template<classsq_T> classKalman:publicBM{ protected: RVrvy; RVrvu; intdimx; intdimy; intdimu; matA; matB; matC; matD; sq_TQ; sq_TR; enorm<sq_T>est; enorm<sq_T>fy; mat_K; vec&_yp; sq_T&_Ry; vec&_mu; sq_T&_P; public: Kalman(RVrvx0,RVrvy0,RVrvu0); Kalman(constKalman<sq_T>&K0); voidset_parameters(constmat&A0,constmat&B0,constmat&C0,constmat&D0,constsq_T&R0,constsq_T&Q0); voidset_est(constvec&mu0,constsq_T&P0){ sq_Tpom(dimy); est.set_parameters(mu0,P0); P0.mult_sym(C,pom); fy.set_parameters(C*mu0,pom); }; voidbayes(constvec&dt); epdf&_epdf(){returnest;} mat&__K(){return_K;} vec_dP(){return_P->getD();} }; classKalmanCh:publicKalman<chmat>{ protected: matpreA; matpostA; public: KalmanCh(RVrvx0,RVrvy0,RVrvu0):Kalman<chmat>(rvx0,rvy0,rvu0),preA(dimy+dimx+dimx,dimy+dimx),postA(dimy+dimx,dimy+dimx){}; voidset_parameters(constmat&A0,constmat&B0,constmat&C0,constmat&D0,constchmat&R0,constchmat&Q0); voidset_est(constvec&mu0,constchmat&P0){ est.set_parameters(mu0,P0); }; voidbayes(constvec&dt); }; classEKFfull:publicKalmanFull,publicBM{ diffbifn*pfxu; diffbifn*phxu; enorm<fsqmat>E; public: EKFfull(RVrvx,RVrvy,RVrvu); voidset_parameters(diffbifn*pfxu,diffbifn*phxu,constmatQ0,constmatR0); voidbayes(constvec&dt); voidset_est(vecmu0,matP0){mu=mu0;P=P0;}; epdf&_epdf(){returnE;}; }; template<classsq_T> classEKF:publicKalman<fsqmat>{ diffbifn*pfxu; diffbifn*phxu; public: EKF(RVrvx,RVrvy,RVrvu); voidset_parameters(diffbifn*pfxu,diffbifn*phxu,constsq_TQ0,constsq_TR0); voidbayes(constvec&dt); }; classEKFCh:publicKalmanCh{ diffbifn*pfxu; diffbifn*phxu; public: EKFCh(RVrvx,RVrvy,RVrvu); voidset_parameters(diffbifn*pfxu,diffbifn*phxu,constchmatQ0,constchmatR0); voidbayes(constvec&dt); }; classKFcondQR:publicKalman<ldmat>,publicBMcond{ //protected: public: KFcondQR(RVrvx,RVrvy,RVrvu,RVrvRQ):Kalman<ldmat>(rvx,rvy,rvu),BMcond(rvRQ){}; voidcondition(constvec&RQ); }; classKFcondR:publicKalman<ldmat>,publicBMcond{ //protected: public: KFcondR(RVrvx,RVrvy,RVrvu,RVrvR):Kalman<ldmat>(rvx,rvy,rvu),BMcond(rvR){}; voidcondition(constvec&R); }; template<classsq_T> Kalman<sq_T>::Kalman(constKalman<sq_T>&K0):BM(K0.rv),rvy(K0.rvy),rvu(K0.rvu), dimx(rv.count()),dimy(rvy.count()),dimu(rvu.count()), A(dimx,dimx),B(dimx,dimu),C(dimy,dimx),D(dimy,dimu), Q(dimx),R(dimy), est(rv),fy(rvy),_yp(fy._mu()),_Ry(fy._R()),_mu(est._mu()),_P(est._R()){ this->set_parameters(K0.A,K0.B,K0.C,K0.D,K0.R,K0.Q); //copyvaluesinpointers _mu=K0._mu; _P=K0._P; _yp=K0._yp; _Ry=K0._Ry; } template<classsq_T> Kalman<sq_T>::Kalman(RVrvx,RVrvy0,RVrvu0):BM(rvx),rvy(rvy0),rvu(rvu0), dimx(rvx.count()),dimy(rvy.count()),dimu(rvu.count()), A(dimx,dimx),B(dimx,dimu),C(dimy,dimx),D(dimy,dimu), Q(dimx),R(dimy), est(rvx),fy(rvy),_yp(fy._mu()),_Ry(fy._R()),_mu(est._mu()),_P(est._R()){ }; template<classsq_T> voidKalman<sq_T>::set_parameters(constmat&A0,constmat&B0,constmat&C0,constmat&D0,constsq_T&R0,constsq_T&Q0){ it_assert_debug(A0.cols()==dimx,"Kalman:Aisnotsquare"); it_assert_debug(B0.rows()==dimx,"Kalman:Bisnotcompatible"); it_assert_debug(C0.cols()==dimx,"Kalman:Cisnotsquare"); it_assert_debug((D0.rows()==dimy)||(D0.cols()==dimu),"Kalman:Disnotcompatible"); it_assert_debug((R0.cols()==dimy)||(R0.rows()==dimy),"Kalman:Risnotcompatible"); it_assert_debug((Q0.cols()==dimx)||(Q0.rows()==dimx),"Kalman:Qisnotcompatible"); A=A0; B=B0; C=C0; D=D0; R=R0; Q=Q0; } template<classsq_T> voidKalman<sq_T>::bayes(constvec&dt){ it_assert_debug(dt.length()==(dimy+dimu),"KalmanFull::bayeswrongsizeofdt"); sq_TiRy(dimy); 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;insq_T _P.mult_sym(A); _P+=Q; //Dataupdate //_Ry=C*P*C.transpose()+R;insq_T _P.mult_sym(C,_Ry); _Ry+=R; matPfull=_P.to_mat(); _Ry.inv(iRy);//resultisin_iRy; _K=Pfull*C.transpose()*(iRy.to_mat()); sq_Tpom((int)Pfull.rows()); iRy.mult_sym_t(C*Pfull,pom); (_P)-=pom;//P=P-PC'iRy*CP; (_yp)=C*_mu+D*u;//yprediction (_mu)+=_K*(y-_yp); if(evalll==true){//likelihoodofobservationy ll=fy.evalpdflog(y); } //cout<<"y:"<<y-(*_yp)<<"R:"<<_Ry->to_mat()<<"iR:"<<_iRy->to_mat()<<"ll:"<<ll<<endl; }; //TODOwhynotconstpointer?? template<classsq_T> EKF<sq_T>::EKF(RVrvx0,RVrvy0,RVrvu0):Kalman<sq_T>(rvx0,rvy0,rvu0){} template<classsq_T> voidEKF<sq_T>::set_parameters(diffbifn*pfxu0,diffbifn*phxu0,constsq_TQ0,constsq_TR0){ 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; } template<classsq_T> voidEKF<sq_T>::bayes(constvec&dt){ it_assert_debug(dt.length()==(dimy+dimu),"KalmanFull::bayeswrongsizeofdt"); sq_TiRy(dimy,dimy); vecu=dt.get(dimy,dimy+dimu-1); vecy=dt.get(0,dimy-1); //Timeupdate _mu=pfxu->eval(_mu,u); pfxu->dfdx_cond(_mu,u,A,false);//updateAbyaderivativeoffx //P=A*P*A.transpose()+Q;insq_T _P.mult_sym(A); _P+=Q; //Dataupdate phxu->dfdx_cond(_mu,u,C,false);//updateCbyaderivativehx //_Ry=C*P*C.transpose()+R;insq_T _P.mult_sym(C,_Ry); (_Ry)+=R; matPfull=_P.to_mat(); _Ry.inv(iRy);//resultisin_iRy; _K=Pfull*C.transpose()*(iRy.to_mat()); sq_Tpom((int)Pfull.rows()); iRy.mult_sym_t(C*Pfull,pom); (_P)-=pom;//P=P-PC'iRy*CP; _yp=phxu->eval(_mu,u);//yprediction (_mu)+=_K*(y-_yp); if(evalll==true){ll+=fy.evalpdflog(y);} }; #endif//KF_H