ekf_obj.cpp itpp/itbase.h estim/libKF.h ekf_obj.h ../simulator.h libBM.h ../simulator.h ../itpp_ext.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp ../stat/libEF.h ../stat/libFN.h reference.h itpp/itbase.h fixed.h ../math/chmat.h estim/libKF.h ../math/libDC.h matrix.h parametry_motoru.h ekf_obj.h double double minQ (double Q) minQ double Q #include<itpp/itbase.h> #include<estim/libKF.h> #include"ekf_obj.h" #include"../simulator.h" doubleminQ(doubleQ){if(Q>1.0){return1.0;}else{returnQ;};}; voidEKFfixed::bayes(constvec&dt){ ekf(dt(2),dt(3),dt(0),dt(1)); vecxhat(4); //UGLYHACK!!!relianceonapredictor!! xhat(0)=zprevod(x_est[0],Qm)*Iref; xhat(1)=zprevod(x_est[1],Qm)*Iref; xhat(2)=zprevod(x_est[2],Qm)*Wref; xhat(3)=zprevod(x_est[3],15); E.set_mu(xhat); if(BM::evalll){ //fromenorm vecxdif(x,4);//first4ofx //UGLYHACK!!!relianceonapredictor!! /*xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref; xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref; xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref; xdif(3)=x[3]-zprevod(x_pred[3],15);*/ xdif-=xhat;//(xdif=x-xhat) matPfull(4,4); double*Pp=Pfull._data(); for(inti=0;i<16;i++){*(Pp++)=zprevod(P_est[i],15);} BM::ll=-0.5*(4*1.83787706640935+log(det(Pfull))+xdif*(inv(Pfull)*xdif)); } }; voidEKFfixed::update_psi(void) { intt_sin,t_cos,tmp; //implementacevPC t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); PSI[2]=((long)cB*t_sin)>>15; tmp=((long)cH*x_est[2])>>15; PSI[3]=((long)tmp*t_cos)>>15; PSI[6]=-((long)cB*t_cos)>>15; PSI[7]=((long)tmp*t_sin)>>15; } voidEKFfixed::prediction(int*ux) { intt_sin,t_cos,tmp; //implementacevPC t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); tmp=((long)cB*x_est[2])>>15; x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15; x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15; x_pred[2]=x_est[2]; x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; update_psi(); mmult(PSI,P_est,temp15a,3,3,3); //mtrans(PSI,temp15b,5,5); mmultt(temp15a,PSI,P_pred,3,3,3); maddD(P_pred,Q,3,3); } voidEKFfixed::correction(void) { intY_error[2]; longtemp30a[4];/*matrix[2,2]-temporarymatrixforinversion*/ choice_P(P_pred,temp15a,3); maddD(temp15a,R,1,1); minv2(temp15a,temp30a); Ry(0,0)=zprevod(temp15a[0],15); Ry(0,1)=zprevod(temp15a[1],15); Ry(1,0)=zprevod(temp15a[2],15); Ry(1,1)=zprevod(temp15a[3],15); mmultDr(P_pred,temp15a,3,3,1,1); mmult1530(temp15a,temp30a,Kalm,3,1,1); /*estimatethestatesystem*/ choice_x(x_pred,temp15a); msub(Y_mes,temp15a,Y_error,1,0); mmult(Kalm,Y_error,temp15a,3,1,0); madd(x_pred,temp15a,x_est,3,0); /*matrixofcovariances-versionwithoutMMULTDL()*/ /*VersionwithMMULTDL()*/ mmultDl(P_pred,temp15a,1,3,3,1); mmult(Kalm,temp15a,P_est,3,1,3); msub(P_pred,P_est,P_est,3,3); /*END*/ } voidEKFfixed::ekf(doubleux,doubleuy,doubleisxd,doubleisyd) { //vypocetnapetivsystemu(x,y) ukalm[0]=prevod(ux/Uref,Qm); ukalm[1]=prevod(uy/Uref,Qm); //zadanimereni Y_mes[0]=prevod(isxd/Iref,Qm); Y_mes[1]=prevod(isyd/Iref,Qm); prediction(ukalm); correction(); //navratestimovanychhodnotregulatoru vec&mu=E._mu(); (mu)(0)=zprevod(x_est[0],Qm)*Iref; (mu)(1)=zprevod(x_est[1],Qm)*Iref; (mu)(2)=zprevod(x_est[2],Qm)*Wref; (mu)(3)=zprevod(x_est[3],15)*Thetaref; } voidEKFfixed::init_ekf(doubleTv) { //TuningofmatrixQ Q[0]=prevod(.01,15);//0.05 Q[5]=Q[0]; Q[10]=prevod(0.0001,15);//1e-3 Q[15]=prevod(0.0001,15);//1e-3 //TuningofmatrixR R[0]=prevod(0.05,15);//0.05 R[3]=R[0]; //Motormodelparameters cA=prevod(1-Tv*Rs/Ls,15); cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); cC=prevod(Tv/Ls/Iref*Uref,15); //cD=prevod(1-Tv*Bf/J,15); //cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); //cF=prevod(p*Tv*Mref/J/Wref,15); cG=prevod(Tv*Wref*4/Thetaref,15); cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); //cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); /*InitmatrixPSIwithpermanentlyconstantterms*/ PSI[0]=cA; PSI[5]=PSI[0]; PSI[10]=0x7FFF; PSI[14]=cG; PSI[15]=0x7FFF; }