pmsm_sim3.cpp itpp/itbase.h estim/libKF.h estim/libPF.h stat/libFN.h pmsm.h simulator.h ../simulator_zdenek/ekf_example/ekf_obj.h iopom.h ../simulator_zdenek/ekf_example/ekf_obj.h libBM.h simulator.h pmsm.h iopom.h ../itpp_ext.h ../stat/libEF.h estim/libPF.h ../stat/libFN.h itpp/itbase.h work/git/mixpp/pmsm/pmsm_sim3.cpp ../math/chmat.h estim/libKF.h ../math/libDC.h void void set_simulator_t (double &Ww) set_simulator_t double & Ww Extended Kalman filter with unknown Q. 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" #include"../simulator_zdenek/ekf_example/ekf_obj.h" #include"iopom.h" usingnamespaceitpp; voidset_simulator_t(double&Ww){ if(t>0.0002)x[8]=1.2;//1A//0.2ZP if(t>0.4)x[8]=10.8;//9A if(t>0.6)x[8]=25.2;//21A if(t>0.7)Ww=2.*M_PI*10.; if(t>1.0)x[8]=1.2;//1A if(t>1.2)x[8]=10.8;//9A if(t>1.4)x[8]=25.2;//21A if(t>1.6)Ww=2.*M_PI*50.; if(t>1.9)x[8]=1.2;//1A if(t>2.1)x[8]=10.8;//9A if(t>2.3)x[8]=25.2;//21A if(t>2.5)Ww=2.*M_PI*100; if(t>2.8)x[8]=1.2;//1A if(t>3.0)x[8]=10.8;//9A if(t>3.2)x[8]=25.2;//21A if(t>3.4)Ww=2.*M_PI*150; if(t>3.7)x[8]=1.2;//1A if(t>3.9)x[8]=10.8;//9A if(t>4.1)x[8]=25.2;//21A if(t>4.3)Ww=2.*M_PI*0; if(t>4.8)x[8]=-1.2;//1A if(t>5.0)x[8]=-10.8;//9A if(t>5.2)x[8]=-25.2;//21A if(t>5.4)Ww=2.*M_PI*(-10.); if(t>5.7)x[8]=-1.2;//1A if(t>5.9)x[8]=-10.8;//9A if(t>6.1)x[8]=-25.2;//21A if(t>6.3)Ww=2.*M_PI*(-50.); if(t>6.7)x[8]=-1.2;//1A if(t>6.9)x[8]=-10.8;//9A if(t>7.1)x[8]=-25.2;//21A if(t>7.3)Ww=2.*M_PI*(-100.); if(t>7.7)x[8]=-1.2;//1A if(t>7.9)x[8]=-10.8;//9A if(t>8.1)x[8]=-25.2;//21A if(t>8.3)x[8]=10.8;//9A if(t>8.5)x[8]=25.2;//21A x[8]=0.0; } intmain(){ //Kalmanfilter intNdat=90000; doubleh=1e-6; intNsimstep=125; intNpart=100; vecmu0="0.00.00.00.0"; vecQdiag("0.050.050.0020.001");//zdenek:0.010.010.00010.0001 vecRdiag("0.050.05");//var(diff(xth))="0.0340.034" chmatQ(Qdiag); chmatR(Rdiag); RVrQ("100","{Q}","4","0"); EKFfixedKFE(rx,rQ); KFE.init_ekf(Nsimstep*h); mgamma_fixevolQ(rQ,rQ); MPF<EKFfixed>M(rx,rQ,evolQ,evolQ,Npart,KFE); //initialize evolQ.set_parameters(10.0,Qdiag,1.0);//sigma=1/10mu evolQ.condition(Qdiag);//Zdenekdefault epdf&pfinit=evolQ._epdf(); M.set_est(pfinit); evolQ.set_parameters(500000.0,Qdiag,0.9999); epdf&KFEep=KFE._epdf(); epdf&Mep=M._epdf(); matXt=zeros(Ndat,9);//truestatefromsimulator matDt=zeros(Ndat,4);//observation matXtE=zeros(Ndat,4); matXtM=zeros(Ndat,4+4);//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.;/**/ //set_simulator_t(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)); Xt.set_row(tK,vec(x,9));//vecfromC-array Dt.set_row(tK,concat(dt,ut)); XtE.set_row(tK,KFEep.mean()); XtM.set_row(tK,Mep.mean()); } chartmpstr[200]; sprintf(tmpstr,"%s/%s","herez/","format"); ofstreamform(tmpstr); form<<"#Experimetalheaderfile"<<endl; dirfile_write(form,"herez/",Xt,"X","{isaisbomth}"); dirfile_write(form,"herez",XtM,"XtM","{q1q2q3q4isaisbomth}"); dirfile_write(form,"herez",XtE,"XE","{isaisbomth}"); dirfile_write(form,"herez",Dt,"Dt","{isaisbuaub}"); return0; }