pmsm_sim2.cpp itpp/itbase.h estim/libKF.h estim/libPF.h stat/libFN.h stat/loggers.h pmsm.h simulator.h sim_profiles.h EKF_unQ EKF_unQful int int main () main /* \file \briefModelsforsynchronouselectricdriveusingIT++andBDM \authorVaclavSmidl. ----------------------------------- BDM++-C++libraryforBayesianDecisionMakingunderUncertainty UsingIT++fornumericaloperations ----------------------------------- */ #include<itpp/itbase.h> #include<estim/libKF.h> #include<estim/libPF.h> #include<stat/libFN.h> #include<stat/loggers.h> #include"pmsm.h" #include"simulator.h" #include"sim_profiles.h" usingnamespaceitpp; classEKF_unQ:publicEKFCh,publicBMcond{ public: EKF_unQ(RVrx,RVry,RVru,RVrQ):EKFCh(rx,ry,ru),BMcond(rQ){}; voidcondition(constvec&Q0){ Q.setD(Q0,0); //fromEKF preA.set_submatrix(dimy+dimx,dimy,Q._Ch()); }; voidbayes(constvecdt){ EKFCh::bayes(dt); vecxtrue(4); //UGLYHACK!!!relianceonapredictor!! xtrue(0)=x[0]; xtrue(1)=x[1]; xtrue(2)=x[2]; xtrue(3)=x[3]; ll=-0.5*(4*1.83787706640935+_P.logdet()+_P.invqform(xtrue)); } }; classEKF_unQful:publicEKFfull,publicBMcond{ public: EKF_unQful(RVrx,RVry,RVru,RVrQ):EKFfull(rx,ry,ru),BMcond(rQ){}; voidcondition(constvec&Q0){ Q=diag(Q0); }; voidbayes(constvecdt){ EKFfull::bayes(dt); vecxtrue(4); //UGLYHACK!!!relianceonapredictor!! xtrue(0)=x[0]; xtrue(1)=x[1]; xtrue(2)=x[2]; xtrue(3)=x[3]; BM::ll=-0.5*(4*1.83787706640935+log(det(P))+xtrue*(inv(P)*xtrue)); } }; intmain(){ //Kalmanfilter intNdat=90000; doubleh=1e-6; intNsimstep=125; intNpart=50; dirfilelogL("exp/pmsm_sim2",1000); //internalmodel IMpmsmfxu; //RsLsdtFmag(Ypm)kppJBf(Mz) fxu.set_parameters(0.28,0.003465,Nsimstep*h,0.1989,1.5,4.0,0.04,0.0); //observationmodel OMpmsmhxu; vecmu0="0.00.00.00.0"; //vecQdiag("0.010.010.010.0001");//zdenek:0.010.010.00010.0001 vecQdiag("1010100.001");//zdenek:0.010.010.00010.0001 vecRdiag("100100");//var(diff(xth))="0.0340.034" chmatQ(Qdiag); chmatR(Rdiag); EKFChKFE(rx,ry,ru); KFE.set_est(mu0,chmat(1*eye(4))); KFE.set_parameters(&fxu,&hxu,Q,R); RVrQ("{Q}","4"); EKF_unQfulKFEp(rx,ry,ru,rQ); KFEp.set_est(mu0,1*ones(4)); KFEp.set_parameters(&fxu,&hxu,diag(Qdiag),diag(Rdiag)); mgamma_fixevolQ(rQ,rQ); MPF<EKF_unQful>M(rx,rQ,evolQ,evolQ,Npart,KFEp); //initialize evolQ.set_parameters(1000.0,Qdiag,0.5);//sigma=1/10mu evolQ.condition(Qdiag);//Zdenekdefault epdf&pfinit=evolQ._epdf(); M.set_est(pfinit); evolQ.set_parameters(100000.0,Qdiag,0.9999); // epdf&KFEep=KFE._epdf(); epdf&Mep=M._epdf(); intX_log=L.add(rx,"X"); intEfix_log=L.add(rx,"XF"); RVtmp=concat(rQ,rx); intM_log=L.add(tmp,"M"); L.init(); //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); for(inttK=1;tK<Ndat;tK++){ //NumberofstepsofasimulatorforonestepofKalman for(intii=0;ii<Nsimstep;ii++){ //simulator sim_profile_steps1(Ww); pmsmsim_step(Ww); }; //collectdata ut(0)=KalmanObs[0]; ut(1)=KalmanObs[1]; dt(0)=KalmanObs[2]; dt(1)=KalmanObs[3]; //estimator KFE.bayes(concat(dt,ut)); M.bayes(concat(dt,ut)); L.logit(X_log,vec(x,4));//vecfromC-array L.logit(Efix_log,KFEep.mean()); L.logit(M_log,Mep.mean()); L.step(); } L.finalize(); return0; }