ekf.cpp math.h fixed.h reference.h matrix.h ekf.h ekf.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf.cpp math.h reference.h fixed.h matrix.h K_PREVOD_FORM 4 double double ladeni_EKF[10] [10] ladeni_EKF int int H[8] [8] H {0x7FFF,0,0,0,0,0x7FFF,0,0} int int Ht[8] [8] Ht {0x7FFF,0,0,0x7FFF,0,0,0,0} int int Q[16] [16] Q {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} int int R[4] [4] R {0,0,0,0} int int x_est[4] [4] x_est {0,0,0,0} int int x_pred[4] [4] x_pred {0,0,0,0} int int P_pred[16] [16] P_pred {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} int int P_est[16] [16] P_est {0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF} int int Y_mes[2] [2] Y_mes {0,0} int int ukalm[2] [2] ukalm {0,0} int int Kalm[8] [8] Kalm int int PSI[16] [16] PSI {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} int int temp15a[16] [16] temp15a int int cA cA int int cB cB int int cC cC int int cG cG int int cH cH void void init_ekf (double Tv, double *param) init_ekf double Tv double * param KONEC inicializace pro PC - az sem vyhodit v DSP /// void void ekf (double *x_estimation, double Umd, double beta, double Ucnd, double Ucd, double isxd, double isyd) ekf double * x_estimation double Umd double beta double Ucnd double Ucd double isxd double isyd prechod ze simulace do fixed-pointu - vyradit v DSP ///////////KONEC emulace FIXED-POINTu - az sem vyradit v DSP //////////// void void prediction (int *ux) prediction int * ux void void correction (void) correction void void static void update_psi (void) update_psi void /************************************ ExtendedKalmanFilter KalmanObserver Z.Peroutka Rev.15.3.2008 EKFpocitansdatyveformatuQ15,zatimcoregulacepracujevQ13-> resenimjevynasobitreferencnihodnotyvEKF4-mi(timjeautomaticky zajistenprechodmeziformatyQ13aQ15).RealizovanokonstantouK_PREVOD_FORM. 15.3.2008Kontrolakodu+zamenadatovychtypuq15->intaq30->long. *************************************/ #include<math.h> #include"fixed.h" //#include"parametry_motoru.h"//aktivovatvDSP #include"reference.h" #include"matrix.h" #include"ekf.h" #defineK_PREVOD_FORM4//2^(15-Qm) /*Declarationofglobalfunctions*/ voidinit_ekf(doubleTv,double*param); voidekf(double*x_estimation,doubleUmd,doublebeta,doubleUcnd,doubleUcd,doubleisxd,doubleisyd); /*Declarationoflocalfunctions*/ staticvoidprediction(int*ux); staticvoidcorrection(void); staticvoidupdate_psi(void); //globalvariables doubleladeni_EKF[10]; /*Constants-definovatjakokonstanty???kdejevyhodnejsiabyvpametibyli?*/ staticintH[8]={0x7FFF,0,0,0,0,0x7FFF,0,0};/*matrix[2,4]*/ staticintHt[8]={0x7FFF,0,0,0x7FFF,0,0,0,0};/*matrix[4,2]*/ //staticintQ[4][4]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};/*matrix[4,4]*/ //staticintR[2][2]={0,0,0,0};/*matrix[2,2]*/ staticintQ[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};/*matrix[4,4]*/ staticintR[4]={0,0,0,0};/*matrix[2,2]*/ /*Initialconditionsandvariables*/ staticintx_est[4]={0,0,0,0}; staticintx_pred[4]={0,0,0,0}; staticintP_pred[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};/*matrix[4,4]*/ //staticintP_est[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};/*matrix[4,4]*/ staticintP_est[16]={0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF,0,0,0,0,0x7FFF};/*matrix[4,4]*/ staticintY_mes[2]={0,0}; staticintukalm[2]={0,0}; staticintKalm[8];/*matrix[5,2]*/ staticintPSI[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};/*matrix[4,4]*/ /*Temporaryvariables*/ staticinttemp15a[16]; //constantsofPMSMmathematicalmodel staticintcA,cB,cC,cG,cH;//cD,cE,cF,cI...nepouzivane voidinit_ekf(doubleTv,double*param) { //parametrysimulace-vDSPvyhodit(nahrazeno"parametry_motoru.h") doubleRs,Ls,Fmag,kp,p; //param=[Rs,Ls,Fmag,Bf,p,kp,J=0.04?]; Rs=*param; Ls=*(param+1); Fmag=*(param+2); p=*(param+4); kp=*(param+5); //TuningofmatrixQ /*Q[0][0]=prevod(.05,15);//1e-2 Q[1][1]=Q[0][0]; Q[2][2]=prevod(1e-3,15);//10e-5 Q[3][3]=prevod(1e-3,15);//model(ureg)...4e-3 //TuningofmatrixR R[0][0]=prevod(0.05,15);//model(ureg)...0.1 R[1][1]=R[0][0]; /**/ //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]; /*TuningofmatrixP_est-initialvalues*/ /*P_est[0][0]=prevod(0.999,15); P_est[1][1]=prevod(0.999,15); P_est[2][2]=prevod(0.999,15); P_est[3][3]=prevod(0.999,15); P_est[4][4]=prevod(0.999,15); /**/ //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*K_PREVOD_FORM/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; } staticvoidupdate_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); //implementacevDSP //t_sin=qsin(x_est[3]); //t_cos=qcos(x_est[3]); 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; } voidprediction(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); //implementacevDSP //t_sin=qsin(x_est[3]); //t_cos=qcos(x_est[3]); 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); } voidcorrection(void) { intY_error[2]; longtemp30a[4];/*matrix[2,2]-temporarymatrixforinversion*/ /*Kalmangaincalculation*/ //mmult(P_pred,Ht,temp15a,5,5,2); /*mmultt(P_pred,H,temp15a,4,4,1); mmult(H,temp15a,temp15b,1,4,1);theselinesarereplacedbychoice_P*/ choice_P(P_pred,temp15a,3); maddD(temp15a,R,1,1); minv2(temp15a,temp30a); /*mmultt(P_pred,H,temp15a,4,4,1);/*removethislineifchoice_Pisnotused*/ //mmultDr15(P_pred,Ht,temp15a,4,4,1,1); mmultDr(P_pred,temp15a,3,3,1,1); mmult1530(temp15a,temp30a,Kalm,3,1,1); /*estimatethestatesystem*/ //mmult(H,x_pred,temp15a,1,4,0); 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()*/ /*mmult(Kalm,H,temp15a,4,1,4); mmult(temp15a,P_pred,P_est,4,4,4); msub(P_pred,P_est,P_est,4,4); /*END*/ /*VersionwithMMULTDL()*/ mmultDl(P_pred,temp15a,1,3,3,1); /*ifresultmatrixhasmoretermsthanDIAGmatrix,itisnecessarytoenable erasesequenceindefinitionoffunctionMMULTDL()thatiscurrentlydisabled.*/ mmult(Kalm,temp15a,P_est,3,1,3); msub(P_pred,P_est,P_est,3,3); /*END*/ } voidekf(double*x_estimation,doubleUmd,doublebeta,doubleUcnd,doubleUcd,doubleisxd,doubleisyd) { intUmk,ua,ub; intUm,Ucn,Uc,isx,isy,t_cos; Um=prevod(Umd/Uref,Qm); Ucn=prevod(Ucnd/Uref,Qm); Uc=prevod(Ucd/Uref,Qm); isx=prevod(isxd/Iref,Qm); isy=prevod(isyd/Iref,Qm); //prepocetnapetiproEKFdleskutecnehonapetivssobvodu Umk=((long)Um*Uc)/Ucn; //vypocetfazovychnapetistridace ua=((long)Umk*prevod(cos(beta),15))>>15; ub=((long)Umk*prevod(cos(beta-2./3.*M_PI),15))>>15; //uc=((long)Umk*prevod(cos(beta+2./3.*M_PI),15))>>15; //vypocetnapetivsystemu(x,y) ukalm[0]=ua; ukalm[1]=(((long)ua+ub<<1)*18917)>>15;//usy=(ua+2*ub)/sqrt(3) //zadanimereni Y_mes[0]=isx; Y_mes[1]=isy; prediction(ukalm); correction(); //navratestimovanychhodnotregulatoru *x_estimation=zprevod(x_est[2],Qm)*Wref; //x_est[3]=(shortint)x_est[3]; *(x_estimation+1)=zprevod((shortint)x_est[3],15)*Thetaref; }