sim_var.cpp itpp/itbase.h stat/libFN.h estim/libKF.h math/chmat.h pmsm.h simulator.h sim_profiles.h stat/loggers.h sim_profiles.h stat/loggers.h libBM.h simulator.h pmsm.h ../itpp_ext.h ../stat/libEF.h stat/libFN.h work/git/mixpp/pmsm/sim_var.cpp itpp/itbase.h ../math/chmat.h estim/libKF.h ../math/libDC.h int int main () main Extended Kalman filter with unknown Q. EKFfull::_epdf logger::add EKFfull::bayes OMpmsm::eval IMpmsm::eval dirfilelog::finalize dirfilelog::init memlog::logit epdf::mean EKFfull::set_est EKFfull::set_parameters IMpmsm::set_parameters dirfilelog::step Models for synchronous electric drive using IT++ and BDM. Vaclav Smidl. ----------------------------------- BDM++ - C++ library for Bayesian Decision Making under UncertaintyUsing IT++ for numerical operations ----------------------------------- #include<itpp/itbase.h> #include<stat/libFN.h> #include<estim/libKF.h> //#include<estim/libPF.h> #include<math/chmat.h> #include"pmsm.h" #include"simulator.h" #include"sim_profiles.h" #include<stat/loggers.h> usingnamespaceitpp; intmain(){ //Kalmanfilter intNdat=90000; doubleh=1e-6; intNsimstep=125; dirfilelogL("exp/sim_var",1000); //memlogL(Ndat); //SETSIMULATOR pmsmsim_set_parameters(0.28,0.003465,0.1989,0.0,4,1.5,0.04,200.,3e-6,h); doubleWw=0.0; vecdt(2); vecut(2); vecdut(2); vecdit(2); vecxtm=zeros(4); vecxte=zeros(4); vecxdif=zeros(4); vecxt(4); vecddif=zeros(2); IMpmsmfxu; //RsLsdtFmag(Ypm)kppJBf(Mz) fxu.set_parameters(0.28,0.003465,Nsimstep*h,0.1989,1.5,4.0,0.04,0.0); OMpmsmhxu; matQt=zeros(4,4); matRt=zeros(2,2); //ESTIMATORS vecmu0="0.00.00.00.0"; vecQdiag("62664540.03");//zdenek:0.010.010.00010.0001 vecRdiag("100100");//var(diff(xth))="0.0340.034" matQ=diag(Qdiag); matR=diag(Rdiag); EKFfullEfix(rx,ry,ru); Efix.set_est(mu0,1*eye(4)); Efix.set_parameters(&fxu,&hxu,Q,R); EKFfullEop(rx,ry,ru); Eop.set_est(mu0,1*eye(4)); Eop.set_parameters(&fxu,&hxu,Q,R); EKFfullEdi(rx,ry,ru); Edi.set_est(mu0,1*eye(4)); Edi.set_parameters(&fxu,&hxu,Q,R); epdf&Efix_ep=Efix._epdf(); epdf&Eop_ep=Eop._epdf(); epdf&Edi_ep=Edi._epdf(); //LOG RVrQ("{Q}","16"); RVrR("{R}","4"); RVrUD("{u_isau_isbi_isai_isb}",ones_i(4)); RVrDu("{duxduyduxfduyf}",ones_i(4)); RVrDi("{disadisb}",ones_i(2)); intX_log=L.add(rx,"X"); intEfix_log=L.add(rx,"XF"); intEop_log=L.add(rx,"XO"); intEdi_log=L.add(rx,"XD"); intQ_log=L.add(rQ,"Q"); intR_log=L.add(rR,"R"); intD_log=L.add(rUD,"D"); intDu_log=L.add(rDu,"Du"); intDi_log=L.add(rDi,"Di"); L.init(); for(inttK=1;tK<Ndat;tK++){ //NumberofstepsofasimulatorforonestepofKalman for(intii=0;ii<Nsimstep;ii++){ sim_profile_steps1(Ww,false); pmsmsim_step(Ww); }; //simulationviadeterministicmodel ut(0)=KalmanObs[0]; ut(1)=KalmanObs[1]; dt(0)=KalmanObs[2]; dt(1)=KalmanObs[3]; dut(0)=KalmanObs[4]; dut(1)=KalmanObs[5]; dit(0)=KalmanObs[8]; dit(1)=KalmanObs[9]; xte=fxu.eval(xtm,ut); //Results: //inxtwehavesimulationaccordingtothemodel //inxwehave"reality" xt(0)=x[0];xt(1)=x[1];xt(2)=x[2];xt(3)=x[3]; xdif=xt-xte;//xtmisacopyofx[] if(xdif(0)>3.0){ cout<<"here"<<endl; } if(xdif(3)>pi)xdif(3)-=2*pi; if(xdif(3)<-pi)xdif(3)+=2*pi; ddif=hxu.eval(xt,ut)-dit; //Rt=0.9*Rt+xdif^2 Qt*=0.1; Qt+=10*outer_product(xdif,xdif);//(x-xt)^2 if(Qt(0,0)>3.0){ cout<<"here"<<endl; } //Forfutureref. xtm=xt; Rt*=0.1; //Rt+=10*outer_product(ddif,ddif);//(x-xt)^2 //ESTIMATE Efix.bayes(concat(dt,ut)); // Eop.set_parameters(&fxu,&hxu,(Qt+1e-8*eye(4)),(Rt+1e-6*eye(2))); Eop.bayes(concat(dt,ut)); // Edi.set_parameters(&fxu,&hxu,(diag(diag(Qt))+1e-16*eye(4)),(diag(diag(Rt))+1e-3*eye(2))); Edi.bayes(concat(dt,ut)); //LOG L.logit(X_log,vec(x,4));//vecfromC-array L.logit(Efix_log,Efix_ep.mean()); L.logit(Eop_log,Eop_ep.mean()); L.logit(Edi_log,Edi_ep.mean()); L.logit(Q_log,vec(Qt._data(),16)); L.logit(R_log,vec(Rt._data(),4)); L.logit(D_log,vec(KalmanObs,4)); L.logit(Du_log,dut); L.logit(Di_log,dit); L.step(); } L.finalize(); //L.itsave("sim_var.it"); return0; }