pmsm_sim.cpp itpp/itbase.h estim/libKF.h estim/libPF.h stat/libFN.h pmsm.h simulator.h libBM.h simulator.h pmsm.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 work/git/mixpp/pmsm/pmsm_sim.cpp EKF_unQ 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"pmsm.h" #include"simulator.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()); }; }; intmain(){ //Kalmanfilter intNdat=30000; doubleh=1e-6; intNsimstep=125; intNpart=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.000010.00001");//zdenek:0.010.010.00010.0001 vecRdiag("0.00050.0005");//var(diff(xth))="0.0340.034" chmatQ(Qdiag); chmatR(Rdiag); EKFChKFE(rx,ry,ru); KFE.set_parameters(&fxu,&hxu,Q,R); KFE.set_est(mu0,chmat(1*ones(4))); RVrQ("{Q}","2"); EKF_unQKFEp(rx,ry,ru,rQ); KFEp.set_parameters(&fxu,&hxu,Q,R); KFEp.set_est(mu0,chmat(1*ones(4))); mgammaevolQ(rQ,rQ); MPF<EKF_unQ>M(rx,rQ,evolQ,evolQ,Npart,KFEp); //initialize evolQ.set_parameters(100.0);//sigma=1/10mu evolQ.condition("0.010.01");//Zdenekdefault epdf&pfinit=evolQ._epdf(); M.set_est(pfinit); evolQ.set_parameters(1000.0); // epdf&KFEep=KFE._epdf(); epdf&Mep=M._epdf(); matXt=zeros(Ndat,9);//truestatefromsimulator matDt=zeros(Ndat,4+2);//observation matXtE=zeros(Ndat,4); matXtM=zeros(Ndat,6);//Q+x //SETSIMULATOR pmsmsim_set_parameters(0.28,0.003465,0.1989,0.0,4,1.5,0.04,200.,3e-6,h); doubleWw=0.0; staticintk_rampa=1; staticlongk_rampa_tmp=0; vecdt(2); vecut(2); for(inttK=1;tK<Ndat;tK++){ //NumberofstepsofasimulatorforonestepofKalman for(intii=0;ii<Nsimstep;ii++){ //simulator Ww+=k_rampa*2.*M_PI*2e-4;//1000Hz/s if(Ww>2.*M_PI*150.){ Ww=2.*M_PI*150.; if(k_rampa_tmp<500000)k_rampa_tmp++; else{k_rampa=-1;k_rampa_tmp=0;} }; if(Ww<-2.*M_PI*150.)Ww=-2.*M_PI*150.;/**/ 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)); Xt.set_row(tK,vec(x,9));//vecfromC-array Dt.set_row(tK,concat(dt,ut,vec_1(sqrt(pow(ut(0),2)+pow(ut(1),2))),vec_1(sqrt(pow(dt(0),2)+pow(dt(1),2))))); XtE.set_row(tK,KFEep.mean()); XtM.set_row(tK,Mep.mean()); } it_filefou("pmsm_sim.it"); fou<<Name("xth")<<Xt; fou<<Name("Dt")<<Dt; fou<<Name("xthE")<<XtE; fou<<Name("xthM")<<XtM; //Exitprogram: return0; }