Changeset 1326

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

fixed point implemnetation of 2d EKF

Location:
applications/pmsm/simulator_zdenek/ekf_example
Files:
4 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  

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

    r1321 r1326  
    332332 
    333333UIREGISTER(EKFfixedCh); 
     334 
     335/*! 
     336 * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic 
     337 *  
     338 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
     339 */ 
     340class EKFfixedCh2 : public BM { 
     341public: 
     342        LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP); 
     343         
     344        void init_ekf2(double Tv); 
     345        void ekf2(double ux, double uy, double isxd, double isyd); 
     346         
     347        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ 
     348        int16 Q[4]; /* matrix [4,4] */ 
     349        int16 R[4]; /* matrix [2,2] */ 
     350         
     351        int16 x_est[2]; /* estimate and prediction */ 
     352        int16 y_est[2]; /* estimate and prediction */ 
     353        int16 y_old[2]; /* estimate and prediction */ 
     354         
     355        int16 PSI[4]; /* matrix [4,4] */ 
     356        int16 PSICh[4]; /* matrix PIS*U, [4,4] */ 
     357        int16 C[4]; /* matrix [4,4] */ 
     358         
     359        int16 Chf[4]; // upper triangular of covariance (inplace) 
     360         
     361        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
     362         
     363        enorm<fsqmat> E; 
     364        mat Ry; 
     365         
     366public: 
     367        //! Default constructor 
     368        EKFfixedCh2 ():BM(),E(),Ry(2,2){ 
     369                int16 i; 
     370                for(i=0;i<4;i++){Q[i]=0;} 
     371                for(i=0;i<4;i++){R[i]=0;} 
     372                 
     373                for(i=0;i<2;i++){x_est[i]=0;} 
     374                for(i=0;i<2;i++){y_est[i]=0;} 
     375                for(i=0;i<2;i++){y_old[i]=0;} 
     376                for(i=0;i<4;i++){Chf[i]=0;} 
     377                 
     378                for(i=0;i<4;i++){PSI[i]=0;} 
     379                for(i=0;i<4;i++){C[i]=0;} 
     380                 
     381                set_dim(2); 
     382                dimc = 2; 
     383                dimy = 2; 
     384                E._mu()=zeros(2); 
     385                E._R()=zeros(2,2); 
     386                init_ekf2(0.000125); 
     387        }; 
     388        //! Here dt = [yt;ut] of appropriate dimensions 
     389        void bayes ( const vec &yt, const vec &ut ); 
     390        //!dummy! 
     391        const epdf& posterior() const {return E;}; 
     392        void log_register(logger &L, const string &prefix){ 
     393                BM::log_register ( L, prefix ); 
     394                 
     395                L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix ); 
     396                L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); 
     397                L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); 
     398                L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); 
     399                 
     400        }; 
     401        //void from_setting(); 
     402}; 
     403 
     404UIREGISTER(EKFfixedCh2); 
    334405 
    335406 

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

    r1321 r1326  
    206206                                *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni 
    207207                                 *b_i  +=  ((int32)beta*(*b_j))>>15; 
    208                                  printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta); 
     208                                 //printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta); 
    209209                        } 
    210210                } 
    … …  
    451451                tau=Ch[i*dimx+j]; 
    452452                                tmp_long=((int32)beta*Ch[i*dimx+j] -(int32)sigma*w[i])/gamma; 
    453                                 if (tmp_long>32767) { 
     453                /*              if (tmp_long>32767) { 
    454454                                        Ch[i*dimx+j]=32767; 
    455455                                } else { 
    … …  
    457457                                                Ch[i*dimx+j]=-32767; 
    458458                                        } else { 
    459                                                 Ch[i*dimx+j]=tmp_long; 
    460                                         } 
     459                */                              Ch[i*dimx+j]=tmp_long; 
     460                        /*              } 
    461461                                } 
    462                                  
     462                        */       
    463463                w[i]+=((int32)tau*sigma)>>15; 
    464464            } 
    … …  
    537537                        } 
    538538                        m1pom-=(j+1); // shift back to first element 
    539                          
    540                         *respom++=tmp_sum>>15; 
    541                          
     539         
     540                /*      if (tmp_sum>(1<<29)-1)  
     541                                *respom++=(1<<14); 
     542                        else 
     543                */       
     544                if (tmp_sum>0) 
     545                        *respom++=(tmp_sum+(1<<14)-1)>>15; 
     546                else  
     547                        *respom++=(tmp_sum-(1<<14)+1)>>15; 
     548                 
    542549                        tmp_sum=0; 
    543550                } 
    … …  
    552559        int16 rho,s,c,tau; 
    553560        int32  tmp_long; 
     561         
     562        //c,s in q14!! 
    554563         
    555564        int16 A[25];//beware 
    … …  
    573582                for (j=0; j<dimx; j++)  
    574583                { 
    575                         //                      tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j]; 
    576                          
    577                         tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij; 
    578                          
    579                         if (tmp_long>0) 
     584                        if (*A_ij!=0) 
    580585                        { 
     586                                tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij; 
    581587                                //                              rho=qsqrt(tmp_long);                   // verze pro DSP 
    582588                                rho=(int16)(sqrt((double)tmp_long));     // verze pro PC 
    583                                 s=((int32)*A_ij<<15)/rho; 
    584                                 c=((int32)*Ch_ii<<15)/rho; 
     589                                s=(((int32)*A_ij)<<14)/rho; 
     590                                c=(((int32)*Ch_ii)<<14)/rho; 
    585591                                 
    586592                                Ch_ki=Ch+i; 
    … …  
    589595                                for (k=0;k<=i; k++) 
    590596                                { 
    591                                         tau=((int32)c**A_kj-(int32)s**Ch_ki)>>15; 
    592                                         *Ch_ki=((int32)s**A_kj+(int32)c**Ch_ki)>>15; 
     597                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14; 
     598                                        tmp_long=(int32)s**A_kj+(int32)c**Ch_ki; 
     599                                        if (tmp_long>(1<<28)) //q14 + q15 
     600                                                *Ch_ki = (1<<14)-1; 
     601                                        else  
     602                                                *Ch_ki=tmp_long>>14; 
    593603                                        *A_kj=tau; 
    594604                                         
    … …  
    604614                for (j=0; j<i; j++) 
    605615                { 
    606                         tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij; 
    607                          
    608                         if (tmp_long>0) 
     616                         
     617                        if (*Ch_ij>0) 
    609618                        { 
     619                                tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij; 
    610620                                //                      rho=qsqrt(tmp_long);                     // verze pro DSP 
    611                                 rho=(int16)(sqrt((double)tmp_long));       // verze pro PC 
    612                                 s=((int32)*Ch_ij<<15)/rho; 
    613                                 c=((int32)*Ch_ii<<15)/rho; 
     621                                if (tmp_long>(1<<30)-1) 
     622                                        rho=(1<<15)-1; 
     623                                else 
     624                                        rho=(int16)(sqrt((double)tmp_long));       // verze pro PC 
     625                                                 
     626                                s=(((int32)*Ch_ij)<<14)/rho; 
     627                                c=(((int32)*Ch_ii)<<14)/rho; 
    614628                                 
    615629                                Ch_kj = Ch + j; 
    … …  
    618632                                for (k=0; k<=i; k++) 
    619633                                { 
    620                                         tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>15; 
    621                                         *Ch_ki =((int32)s**Ch_kj+(int32)c**Ch_ki)>>15; 
     634                                        tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>14; 
     635                                        tmp_long =((int32)s**Ch_kj+(int32)c**Ch_ki); 
     636                                        if (tmp_long>(1<<28)) 
     637                                                *Ch_ki = (1<<14)-1; 
     638                                        else  
     639                                                *Ch_ki=tmp_long>>14; 
    622640                                        *Ch_kj=tau; 
    623641                                         
    … …  
    720738                         
    721739                        //sigma=Ch[iy*dimx+j]; 
    722                         beta=alpha; 
     740                        beta=alpha; // in q15 
    723741                        //            alpha+=((long)sigma*sigma)>>15; 
    724                         alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;                              // vyssi presnost 
     742                        tmp_long=((int32)alpha<<15)+(((int32)sigma*sigma)<<(30-2*qCh)); 
     743                        if (tmp_long>0) 
     744                                alpha=(tmp_long+(1<<14)-1)>>15;                         // vyssi presnost 
     745                        else 
     746                                alpha=(tmp_long-(1<<14)+1)>>15;                         // vyssi presnost 
     747                                         
    725748                        //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP 
    726749                        gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC 
    727                          
     750                               // in q15 
    728751                        w[j]=0; 
    729752                         
    730753                        Ch_ij=Ch+j; 
    731                         w_i=w; 
     754                        w_i=w; // in q15 
    732755                         
    733756                        for (i=0;i<=j;i++) 
    … …  
    736759                                tau=*Ch_ij; 
    737760                                //                              tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma; 
    738                                 tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma; 
    739                                  
    740                                 if (tmp_long>32767) 
     761                                tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma; // in qCh 
     762                                 
     763/*                              if (tmp_long>32767) 
    741764                                        tmp_long=32767; 
    742765                                if (tmp_long<-32768) 
    743                                         tmp_long=-32768; 
     766                                        tmp_long=-32768;*/ 
    744767                                *Ch_ij=tmp_long; 
    745768                                 
    746769                                //                w_i+=((long)tau*sigma)>>15; 
    747                                 *w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15; 
     770                                tmp_long = ((int32)*w_i<<15)+((int32)tau*sigma<<(30-2*qCh)); 
     771                                /*if (tmp_long>0) 
     772                                        *w_i=(tmp_long+(1<<14)-1)>>15; 
     773                                else  
     774                                        *w_i=(tmp_long-(1<<14)+1)>>15; 
     775                                */ *w_i=(tmp_long)>>15; 
    748776                                 
    749777                                w_i++; 
    … …  
    761789                        w_i++; 
    762790                } 
    763         } 
    764 } 
    765  
     791                printf("Kg: %d %d\n",w[0],w[1]); 
     792        } 
     793} 
     794 

  • applications/pmsm/simulator_zdenek/ekf_example/reference_Q15.h

    r1232 r1326  
    2020 
    2121#define Uref            (600.0   *4) 
    22 #define Iref            ((30.0*1.4142)  *4) 
    23 #define Wref            ((6.283185*200.)  *4) 
     22#define Iref            ((30.0*1.4142)  *4) //170 
     23#define Wref            ((6.283185*200.)  *4) //5026 
    2424#define Thetaref        3.141593 
    2525#define Mref            (34.0 *4)