Show
Ignore:
Timestamp:
04/12/11 22:30:24 (13 years ago)
Author:
smidl
Message:

fixed point implemnetation of 2d EKF

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

    r1321 r1326  
    585585        { 
    586586                int Ai[4]; 
    587                 for (int i=0;i<4; i++) Ai[i]=(int)(PSIU[i]); 
     587                for (int i=0;i<4; i++) Ai[i]=(int)(PSI[i]); 
    588588                ivec Ad(Ai,4); 
    589589                log_level.store(logA,get_from_ivec(Ad)); 
    … …  
    593593        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,2); 
    594594         
    595  
    596595// implementace v PC 
    597596/*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
    … …  
    617616        imat Ud(Ui,2,2); 
    618617        imat Cd(Ci,2,2); 
    619         imat CU = Cd*Ud/(1<<15); 
    620         log_level.store(logU,get_from_ivec(ivec(CU._data(),CU._datasize()))); 
     618//      imat CU = Cd*Ud/(1<<15); 
     619        log_level.store(logU,get_from_ivec(ivec(Ud._data(),Ud._datasize()))); 
    621620         
    622621        int di[2];      for (int i=0;i<2; i++) di[i]=(int)Df[i]; 
    … …  
    686685         
    687686        // Tuning of matrix R 
    688         R[0]=prevod(0.5,15);         // 0.05 
     687        R[0]=prevod(0.005,15);         // 0.05 
     688        R[3]=R[0]; 
     689         
     690        // Motor model parameters  
     691        cA=prevod(1-Tv*Rs/Ls,15); 
     692        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); 
     693        cC=prevod(Tv/Ls/Iref*Uref,15); 
     694        //  cD=prevod(1-Tv*Bf/J,15); 
     695        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); 
     696        //  cF=prevod(p*Tv*Mref/J/Wref,15); 
     697        cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
     698        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
     699        // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < =========== 
     700        cH=prevod(Tv*Wref*Fmag*Thetaref/Iref/Ls,15); //  
     701        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
     702         
     703        /* Init matrix PSI with permanently constant terms */ 
     704        PSI[0]=0x7FFF; 
     705        PSI[1]=0; 
     706        PSI[2]=cG; 
     707        PSI[3]=0x7FFF; 
     708} 
     709 
     710 
     711void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut) 
     712{ 
     713        ekf(ut[0],ut[1],yt[0],yt[1]); 
     714} 
     715 
     716 
     717void EKFfixedCh::ekf(double ux, double uy, double isxd, double isyd) 
     718{ 
     719        // vypocet napeti v systemu (x,y) 
     720        int16 uf[2]; 
     721        uf[0]=prevod(ux/Uref,15); 
     722        uf[1]=prevod(uy/Uref,15); 
     723         
     724        int16 Y_mes[2]; 
     725        // zadani mereni 
     726        Y_mes[0]=prevod(isxd/Iref,15); 
     727        Y_mes[1]=prevod(isyd/Iref,15); 
     728         
     729        ////// vlastni rutina EKF -- ///////////////////////// 
     730        int16 t_sin,t_cos, tmp; 
     731         
     732        // implementace v PC 
     733        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
     734         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ 
     735        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
     736        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); 
     737         
     738        tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13 
     739        //             q15*q13 +          q13*q15      + q15*q13?? 
     740        x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
     741        x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
     742        x_est[2]=x_est[2]; 
     743        //               q15              + q15*q13 
     744        x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; 
     745         
     746        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15); 
     747        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15); 
     748         
     749        //void EKFfixed::update_psi(void) 
     750        {        
     751                PSI[2]=((int32)cB*t_sin)>>15; 
     752                tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
     753                PSI[3]=((int32)tmp*t_cos)>>15; 
     754                PSI[6]=-((int32)cB*t_cos)>>15; 
     755                PSI[7]=((int32)tmp*t_sin)>>15; 
     756        } 
     757        { 
     758/*              ivec Ad(PSI,16); 
     759                log_level.store(logA,get_from_ivec(Ad));*/ 
     760        } 
     761         
     762        ///////// end of copy /////////////// 
     763        mmultACh(PSI,Chf,PSICh,4,4); 
     764        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
     765        //householder(PSICh,Q,4); 
     766        givens_fast(PSICh,Q,4); 
     767        // COPY  
     768        for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];} 
     769         
     770        { 
     771                 
     772                int Chi[16]; for (int i=0;i<16;i++) Chi[i]=Chf[i]; 
     773                ivec Chd(Chi,16); 
     774                log_level.store(logCh,get_from_ivec(Chd)); 
     775                imat mCh(Chd._data(), 4,4); 
     776                imat P = mCh*mCh.T(); 
     777                ivec iP(P._data(),16); 
     778                log_level.store(logP,get_from_ivec(iP)); 
     779        } 
     780         
     781         
     782        int16 difz[2]; 
     783        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!! 
     784        difz[1]=(Y_mes[1]-x_est[1]);//<<2; 
     785         
     786         
     787        // 
     788        int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
     789        //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];   
     790 
     791        carlson_fast(difz,x_est,Chf,dR,2,4); 
     792        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
     793         
     794        // navrat estimovanych hodnot regulatoru 
     795        vec& mu = E._mu(); 
     796        (mu)(0)=zprevod(x_est[0],15)*Iref; 
     797        (mu)(1)=zprevod(x_est[1],15)*Iref; 
     798        (mu)(2)=zprevod(x_est[2],15)*Wref; 
     799        (mu)(3)=zprevod(x_est[3],15)*Thetaref; 
     800         
     801        //x_est[2]=x[2]*32768/Wref; 
     802        //x_est[3]=x[3]*32768/Thetaref; 
     803        //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref))); 
     804} 
     805 
     806void EKFfixedCh::init_ekf(double Tv) 
     807{ 
     808        // Tuning of matrix Q 
     809        Q[0]=prevod(.01,15);       // 0.05 
     810        Q[5]=Q[0]; 
     811        Q[10]=prevod(0.0005,15);      // 1e-3 
     812        Q[15]=prevod(0.001,15);      // 1e-3 
     813         
     814        Chf[0]=0x3FFF;// !       // 0.05 
     815        Chf[1]=Chf[2]=Chf[3]=Chf[4]=0; 
     816        Chf[5]=0x3FFF;//! 
     817        Chf[6]=Chf[6]=Chf[8]=Chf[9]=0; 
     818        Chf[10]=0x3FFF;//!      // 1e-3 
     819        Chf[11]=Chf[12]=Chf[13]=Chf[4]=0; 
     820        Chf[15]=0x3FFF;      // 1e-3 
     821                 
     822        // Tuning of matrix R 
     823        R[0]=prevod(0.05,15);         // 0.05 
     824        R[3]=R[0]; 
     825         
     826        // Motor model parameters  
     827        cA=prevod(1-Tv*Rs/Ls,15); 
     828        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); 
     829        cC=prevod(Tv/Ls/Iref*Uref,15); 
     830        //  cD=prevod(1-Tv*Bf/J,15); 
     831        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); 
     832        //  cF=prevod(p*Tv*Mref/J/Wref,15); 
     833        cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
     834        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
     835        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  <======= 
     836        //cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); //  
     837        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
     838         
     839        /* Init matrix PSI with permanently constant terms */ 
     840        PSI[0]=cA; 
     841        PSI[5]=PSI[0]; 
     842        PSI[10]=0x7FFF; 
     843        PSI[14]=cG; 
     844        PSI[15]=0x7FFF; 
     845         
     846} 
     847 
     848void EKFfixedCh2::bayes(const itpp::vec& yt, const itpp::vec& ut) 
     849{ 
     850        ekf2(ut[0],ut[1],yt[0],yt[1]); 
     851} 
     852 
     853 
     854void EKFfixedCh2::ekf2(double ux, double uy, double isxd, double isyd) 
     855{ 
     856        // vypocet napeti v systemu (x,y) 
     857        int16 uf[2]; 
     858        uf[0]=prevod(ux/Uref,15); 
     859        uf[1]=prevod(uy/Uref,15); 
     860         
     861        int16 Y_mes[2]; 
     862        // zadani mereni 
     863        Y_mes[0]=prevod(isxd/Iref,15); 
     864        Y_mes[1]=prevod(isyd/Iref,15); 
     865         
     866        ////// vlastni rutina EKF -- ///////////////////////// 
     867        int16 t_sin,t_cos; 
     868        int32 tmp; 
     869         
     870        x_est[0]=x_est[0]; 
     871        //               q15              + q15*q13 
     872        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15; 
     873         
     874        //      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15); 
     875        //      if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15); 
     876         
     877         
     878        ///////// end of copy /////////////// 
     879        mmultACh(PSI,Chf,PSICh,2,2); 
     880         
     881        //void EKFfixed::update_psi(void) 
     882        { 
     883                int Ai[4]; 
     884                for (int i=0;i<4; i++) Ai[i]=(int)(PSICh[i]); 
     885                ivec Ad(Ai,4); 
     886                log_level.store(logA,get_from_ivec(Ad)); 
     887        } 
     888         
     889        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
     890        givens_fast(PSICh,Q,2); 
     891        for (int i=0;i<4;i++) Chf[i]=PSICh[i];   
     892         
     893        // implementace v PC 
     894        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
     895         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ 
     896        tmp=prevod(sin(Thetaref*x_est[1]/32768.),15); 
     897        if (tmp>32767) t_sin =32767; else t_sin=tmp; 
     898        tmp=prevod(cos(Thetaref*x_est[1]/32768.),15); 
     899        if (tmp>32767) t_cos =32767; else t_cos=tmp; 
     900         
     901        {        
     902                C[0]=((int32)cB*t_sin)>>15; 
     903                tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!! 
     904                C[1]=((int32)tmp*t_cos)>>15; 
     905                C[2]=-((int32)cB*t_cos)>>15; 
     906                C[3]=((int32)tmp*t_sin)>>15; 
     907        } 
     908         
     909         
     910        { 
     911                int Chi[4];     for (int i=0;i<4; i++) Chi[i]=(int)Chf[i]; 
     912//              int Ci[4];      for (int i=0;i<4; i++) Ci[i]=(int)C[i]; 
     913                 
     914                imat Chd(Chi,2,2); 
     915        //      imat Cd(Ci,2,2); 
     916                imat CCh = Chd; 
     917                log_level.store(logCh,get_from_ivec(ivec(CCh._data(),CCh._datasize()))); 
     918        } 
     919         
     920         
     921        tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13 
     922        //             q15*q13 +          q13*q15      + q15*q13?? 
     923        y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
     924        y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
     925         
     926         
     927        int16 difz[2]; 
     928        difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!! 
     929        difz[1]=(Y_mes[1]-y_est[1]);//<<2; 
     930         
     931        y_old[0] = Y_mes[0]; 
     932        y_old[1] = Y_mes[1]; 
     933        { 
     934                int Ci[4];               for (int i=0;i<4; i++) Ci[i]=(int)C[i]; 
     935                ivec CC(Ci,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1]; 
     936                log_level.store(logC,get_from_ivec(CC)); 
     937        } 
     938         
     939        //bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 
     940        int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
     941        //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];   
     942        carlson_fastC(difz,x_est,Chf,C,dR,2,2); 
     943        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
     944         
     945        // navrat estimovanych hodnot regulatoru 
     946        vec& mu = E._mu(); 
     947        (mu)(0)=zprevod(x_est[0],15)*Wref; 
     948        (mu)(1)=zprevod(x_est[1],15)*Thetaref; 
     949         
     950        //x_est[2]=x[2]*32768/Wref; 
     951        //x_est[3]=x[3]*32768/Thetaref; 
     952        //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref))); 
     953} 
     954 
     955void EKFfixedCh2::init_ekf2(double Tv) 
     956{ 
     957        // Tuning of matrix Q 
     958        Q[0]=prevod(0.0005,15);      // 1e-3 
     959        Q[3]=prevod(0.0001,15);      // 1e-3 
     960         
     961        Chf[0]=0x1FFF;// !       // 0.05 
     962        Chf[1]=Chf[2]=0; 
     963        Chf[3]=0x1FFF;//! 
     964                 
     965        // Tuning of matrix R 
     966        R[0]=prevod(0.01,15);         // 0.05 
    689967        R[3]=R[0]; 
    690968         
    … …  
    707985        PSI[3]=0x7FFF; 
    708986} 
    709  
    710  
    711 void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut) 
    712 { 
    713         ekf(ut[0],ut[1],yt[0],yt[1]); 
    714 } 
    715  
    716  
    717 void EKFfixedCh::ekf(double ux, double uy, double isxd, double isyd) 
    718 { 
    719         // vypocet napeti v systemu (x,y) 
    720         int16 uf[2]; 
    721         uf[0]=prevod(ux/Uref,15); 
    722         uf[1]=prevod(uy/Uref,15); 
    723          
    724         int16 Y_mes[2]; 
    725         // zadani mereni 
    726         Y_mes[0]=prevod(isxd/Iref,15); 
    727         Y_mes[1]=prevod(isyd/Iref,15); 
    728          
    729         ////// vlastni rutina EKF -- ///////////////////////// 
    730         int16 t_sin,t_cos, tmp; 
    731          
    732         // implementace v PC 
    733         /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
    734          *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ 
    735         t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
    736         t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); 
    737          
    738         tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13 
    739         //             q15*q13 +          q13*q15      + q15*q13?? 
    740         x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
    741         x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
    742         x_est[2]=x_est[2]; 
    743         //               q15              + q15*q13 
    744         x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; 
    745          
    746         if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15); 
    747         if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15); 
    748          
    749         //void EKFfixed::update_psi(void) 
    750         {        
    751                 PSI[2]=((int32)cB*t_sin)>>15; 
    752                 tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
    753                 PSI[3]=((int32)tmp*t_cos)>>15; 
    754                 PSI[6]=-((int32)cB*t_cos)>>15; 
    755                 PSI[7]=((int32)tmp*t_sin)>>15; 
    756         } 
    757         { 
    758 /*              ivec Ad(PSI,16); 
    759                 log_level.store(logA,get_from_ivec(Ad));*/ 
    760         } 
    761          
    762         ///////// end of copy /////////////// 
    763         mmultACh(PSI,Chf,PSICh,4,4); 
    764         //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
    765         //householder(PSICh,Q,4); 
    766         givens_fast(PSICh,Q,4); 
    767         // COPY  
    768         for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];} 
    769          
    770         { 
    771                  
    772                 int Chi[16]; for (int i=0;i<16;i++) Chi[i]=Chf[i]; 
    773                 ivec Chd(Chi,16); 
    774                 log_level.store(logCh,get_from_ivec(Chd)); 
    775                 imat mCh(Chd._data(), 4,4); 
    776                 imat P = mCh*mCh.T(); 
    777                 ivec iP(P._data(),16); 
    778                 log_level.store(logP,get_from_ivec(iP)); 
    779         } 
    780          
    781          
    782         int16 difz[2]; 
    783         difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!! 
    784         difz[1]=(Y_mes[1]-x_est[1]);//<<2; 
    785          
    786          
    787         // 
    788         int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
    789         //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];   
    790  
    791         carlson_fast(difz,x_est,Chf,dR,2,4); 
    792         //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
    793          
    794         // navrat estimovanych hodnot regulatoru 
    795         vec& mu = E._mu(); 
    796         (mu)(0)=zprevod(x_est[0],15)*Iref; 
    797         (mu)(1)=zprevod(x_est[1],15)*Iref; 
    798         (mu)(2)=zprevod(x_est[2],15)*Wref; 
    799         (mu)(3)=zprevod(x_est[3],15)*Thetaref; 
    800          
    801         //x_est[2]=x[2]*32768/Wref; 
    802         //x_est[3]=x[3]*32768/Thetaref; 
    803         //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref))); 
    804 } 
    805  
    806 void EKFfixedCh::init_ekf(double Tv) 
    807 { 
    808         // Tuning of matrix Q 
    809         Q[0]=prevod(.01,15);       // 0.05 
    810         Q[5]=Q[0]; 
    811         Q[10]=prevod(0.0005,15);      // 1e-3 
    812         Q[15]=prevod(0.001,15);      // 1e-3 
    813          
    814         Chf[0]=0x3FFF;// !       // 0.05 
    815         Chf[1]=Chf[2]=Chf[3]=Chf[4]=0; 
    816         Chf[5]=0x3FFF;//! 
    817         Chf[6]=Chf[6]=Chf[8]=Chf[9]=0; 
    818         Chf[10]=0x3FFF;//!      // 1e-3 
    819         Chf[11]=Chf[12]=Chf[13]=Chf[4]=0; 
    820         Chf[15]=0x3FFF;      // 1e-3 
    821                  
    822         // Tuning of matrix R 
    823         R[0]=prevod(0.05,15);         // 0.05 
    824         R[3]=R[0]; 
    825          
    826         // Motor model parameters  
    827         cA=prevod(1-Tv*Rs/Ls,15); 
    828         cB=prevod(Tv*Wref*Fmag/Iref/Ls,15); 
    829         cC=prevod(Tv/Ls/Iref*Uref,15); 
    830         //  cD=prevod(1-Tv*Bf/J,15); 
    831         //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15); 
    832         //  cF=prevod(p*Tv*Mref/J/Wref,15); 
    833         cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
    834         //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
    835         cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  <======= 
    836         //cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); //  
    837         //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
    838          
    839         /* Init matrix PSI with permanently constant terms */ 
    840         PSI[0]=cA; 
    841         PSI[5]=PSI[0]; 
    842         PSI[10]=0x7FFF; 
    843         PSI[14]=cG; 
    844         PSI[15]=0x7FFF; 
    845          
    846 } 
    847