Show
Ignore:
Timestamp:
04/28/11 22:18:55 (13 years ago)
Author:
smidl
Message:

extended reduced model for unknown Torque

Files:
1 modified

Legend:

Unmodified
Added
Removed
  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

    r1330 r1341  
    573573        x_est[0]=x_est[0]; 
    574574        //               q15              + q15*q13 
    575         x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15; 
     575        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0]+(1<<14))>>15; 
    576576         
    577577//      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15); 
     
    636636tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13 
    637637//             q15*q13 +          q13*q15      + q15*q13?? 
    638 y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
    639 y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
     638y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13 
     639y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13 
    640640 
    641641         
     
    674674{ 
    675675        // Tuning of matrix Q 
    676         Q[0]=prevod(0.0005,15);      // 1e-3 
    677         Q[3]=prevod(0.0001,15);      // 1e-3 
     676        Q[0]=prevod(0.01,15);      // 1e-3 
     677        Q[3]=prevod(0.01,15);      // 1e-3 
    678678         
    679679        Uf[0]=0x7FFF;// !       // 0.05 
     
    685685         
    686686        // Tuning of matrix R 
    687         R[0]=prevod(0.005,15);         // 0.05 
     687        R[0]=prevod(0.01,15);         // 0.05 
    688688        R[3]=R[0]; 
    689689         
     
    708708} 
    709709 
     710void EKFfixedUD3::bayes(const itpp::vec& yt, const itpp::vec& ut) 
     711{ 
     712        ekf3(ut[0],ut[1],yt[0],yt[1]); 
     713} 
     714 
     715 
     716void EKFfixedUD3::ekf3(double ux, double uy, double isxd, double isyd) 
     717{ 
     718        // vypocet napeti v systemu (x,y) 
     719        int16 uf[2]; 
     720        uf[0]=prevod(ux/Uref,15); 
     721        uf[1]=prevod(uy/Uref,15); 
     722         
     723        int16 Y_mes[2]; 
     724        // zadani mereni 
     725        Y_mes[0]=prevod(isxd/Iref,15); 
     726        Y_mes[1]=prevod(isyd/Iref,15); 
     727         
     728        ////// vlastni rutina EKF -- ///////////////////////// 
     729        int16 t_sin,t_cos; 
     730        int32 tmp; 
     731         
     732        int16 Mm=x_est[2]; 
     733        x_est[0]=(((int32)x_est[0]<<15) + (int32)cF*x_est[2] + (1<<14))>>15; 
     734        //               q15              + q15*q13 
     735        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0] + (1<<14))>>15; 
     736        x_est[2]=x_est[2]; 
     737         
     738        //      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15); 
     739        //      if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15); 
     740         
     741         
     742        ///////// end of copy /////////////// 
     743        mmultAU(PSI,Uf,PSIU,3,3); 
     744         
     745         
     746        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
     747        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,3); 
     748         
     749        // implementace v PC 
     750        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
     751         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ 
     752        tmp=prevod(sin(Thetaref*x_est[1]/32768.),15); 
     753        if (tmp>32767) t_sin =32767; else t_sin=tmp; 
     754        tmp=prevod(cos(Thetaref*x_est[1]/32768.),15); 
     755        if (tmp>32767) t_cos =32767; else t_cos=tmp; 
     756         
     757        {        
     758                C[0]=((int32)cB*t_sin)>>15; 
     759                tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!! 
     760                C[1]=((int32)tmp*t_cos)>>15; 
     761                C[2] = 0; 
     762                 
     763                C[3]=-((int32)cB*t_cos)>>15; 
     764                C[4]=((int32)tmp*t_sin)>>15; 
     765                C[5]=0; 
     766        }        
     767         
     768        tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13 
     769        //             q15*q13 +          q13*q15      + q15*q13?? 
     770        y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13 
     771        y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13 
     772         
     773         
     774        int16 difz[2]; 
     775        difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!! 
     776        difz[1]=(Y_mes[1]-y_est[1]);//<<2; 
     777         
     778        y_old[0] = Y_mes[0]; 
     779        y_old[1] = Y_mes[1]; 
     780         
     781        //bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 
     782        int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
     783        //int16 xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];   
     784        bierman_fastC(difz,x_est,Uf,Df,C,dR,2,3); 
     785        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
     786         
     787        // navrat estimovanych hodnot regulatoru 
     788        vec& mu = E._mu(); 
     789        (mu)(0)=zprevod(x_est[0],15)*Wref; 
     790        (mu)(1)=zprevod(x_est[1],15)*Thetaref; 
     791        (mu)(2)=zprevod(x_est[2],15)*Mref; 
     792         
     793        //x_est[2]=x[2]*32768/Wref; 
     794        //x_est[3]=x[3]*32768/Thetaref; 
     795        //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref))); 
     796} 
     797 
     798void EKFfixedUD3::init_ekf3(double Tv) 
     799{ 
     800        // Tuning of matrix Q 
     801        Q[0]=prevod(0.001,15);      // 1e-3 
     802        Q[4]=prevod(0.001,15);      // 1e-3 
     803        Q[8]=prevod(0.001,15);      // 1e-3 
     804         
     805        Uf[0]=0x7FFF;// !       // 0.05 
     806        Uf[1]=Uf[2]=Uf[3]=0; 
     807        Uf[4]=0x7FFF;//! 
     808        Uf[5]=Uf[6]=Uf[7]=0; 
     809        Uf[8]=0x7FFF;//! 
     810         
     811        Df[0]=1<<14; 
     812        Df[1]=1<<14; 
     813        Df[2]=1<<14; 
     814         
     815        // Tuning of matrix R 
     816        R[0]=prevod(0.01,15);         // 0.05 
     817        R[3]=R[0]; 
     818         
     819        // Motor model parameters  
     820        cA=prevod(1-Tv*Rs/Ls,15); 
     821        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); 
     822        cC=prevod(Tv/Ls/Iref*Uref,15); 
     823        //  cD=prevod(1-Tv*Bf/J,15); 
     824        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); 
     825        cF=prevod(-p*Tv*Mref/J/Wref,15); 
     826        cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
     827        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
     828        // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < =========== 
     829        cH=prevod(Tv*Wref*Fmag*Thetaref/Iref/Ls,15); //  
     830        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
     831         
     832        /* Init matrix PSI with permanently constant terms */ 
     833        PSI[0]=0x7FFF; 
     834        PSI[1]=0; 
     835        PSI[2]= cF; 
     836         
     837        PSI[3]=cG; 
     838        PSI[4]=0x7FFF; 
     839        PSI[5]=0; 
     840         
     841        PSI[6]=0; 
     842        PSI[7]=0; 
     843        PSI[8]=0x7FFF; 
     844} 
     845 
    710846 
    711847void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)