Changeset 1240

Show
Ignore:
Timestamp:
10/29/10 19:10:03 (14 years ago)
Author:
smidl
Message:

prechod na int16

Location:
applications/pmsm/simulator_zdenek/ekf_example
Files:
4 modified

Legend:

Unmodified
Added
Removed

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

    r1236 r1240  
    77double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};}; 
    88 
    9 void mat_to_int(const imat &M, int *I){ 
    10         for (int i=0;i<M.rows(); i++){ 
    11                 for (int j=0;j<M.cols(); j++){ 
     9void mat_to_int16(const imat &M, int16 *I){ 
     10        for (int16 i=0;i<M.rows(); i++){ 
     11                for (int16 j=0;j<M.cols(); j++){ 
    1212                        *I++ = M(i,j); 
    1313                } 
    1414        } 
    1515} 
    16 void vec_to_int(const ivec &v, int *I){ 
    17         for (int i=0;i<v.length(); i++){ 
     16void vec_to_int16(const ivec &v, int16 *I){ 
     17        for (int16 i=0;i<v.length(); i++){ 
    1818                        *I++ = v(i); 
    1919        } 
    2020} 
    2121 
     22#ifdef XXX 
    2223/////////////// 
    2324void EKFfixed::bayes(const vec &yt, const vec &ut){ 
    … …  
    4647                mat Pfull(4,4); 
    4748                double* Pp=Pfull._data(); 
    48                 for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);} 
     49                for(int16 i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);} 
    4950                 
    5051                E._R()._M()=Pfull; 
    … …  
    5859void EKFfixed::update_psi(void) 
    5960{ 
    60   int t_sin,t_cos,tmp; 
     61  int16 t_sin,t_cos,tmp; 
    6162 
    6263  // implementace v PC 
    … …  
    6465  t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); 
    6566 
    66   PSI[2]=((long)cB*t_sin)>>15; 
    67   tmp=((long)cH*x_est[2])>>15; 
    68   PSI[3]=((long)tmp*t_cos)>>15; 
    69   PSI[6]=-((long)cB*t_cos)>>15; 
    70   PSI[7]=((long)tmp*t_sin)>>15; 
    71 } 
    72  
    73  
    74 void EKFfixed::prediction(int *ux) 
    75 { 
    76   int t_sin,t_cos, tmp; 
     67  PSI[2]=((int32)cB*t_sin)>>15; 
     68  tmp=((int32)cH*x_est[2])>>15; 
     69  PSI[3]=((int32)tmp*t_cos)>>15; 
     70  PSI[6]=-((int32)cB*t_cos)>>15; 
     71  PSI[7]=((int32)tmp*t_sin)>>15; 
     72} 
     73 
     74 
     75void EKFfixed::prediction(int16 *ux) 
     76{ 
     77  int16 t_sin,t_cos, tmp; 
    7778 
    7879  // implementace v PC 
    … …  
    8384  t_cos=prevod(cos(Thetaref*x_est[3]/Qm),15); 
    8485   
    85   tmp=((long)cB*x_est[2])>>15; 
    86   x_pred[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*ux[0])>>15; 
    87   x_pred[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*ux[1])>>15; 
     86  tmp=((int32)cB*x_est[2])>>15; 
     87  x_pred[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*ux[0])>>15; 
     88  x_pred[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*ux[1])>>15; 
    8889  x_pred[2]=x_est[2]; 
    89   x_pred[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; 
     90  x_pred[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; 
    9091 
    9192  update_psi(); 
    … …  
    99100void EKFfixed::correction(void) 
    100101{ 
    101   int Y_error[2]; 
    102   long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ 
     102  int16 Y_error[2]; 
     103  int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ 
    103104 
    104105  choice_P(P_pred,temp15a,3); 
    … …  
    189190  P_est[15]=0x7FFF; 
    190191} 
    191  
     192#endif 
    192193 
    193194void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) { 
    … …  
    228229        Df=round_i(Pld._D()*(1<<15)); 
    229230        ivec zer=find(Df==0); 
    230         for(int i=0; i<zer.length(); i++) Df(zer(i))=1; 
     231        for(int16 i=0; i<zer.length(); i++) Df(zer(i))=1; 
    231232} 
    232233 
    … …  
    236237        vec &_mu=est._mu(); 
    237238        const vec &u=cond; 
    238         int dim = dimension(); 
     239        int16 dim = dimension(); 
    239240        ///// !!!!!!!!!!!!!!!! 
    240241        U = est._R()._L().T(); 
    … …  
    258259         
    259260        vec Din = D; 
    260         int i,j,k; 
     261        int16 i,j,k; 
    261262        double sigma; 
    262263        mat G = eye(dim); 
    … …  
    323324         
    324325         
    325         for (int iy=0; iy<dimy; iy++){ 
     326        for (int16 iy=0; iy<dimy; iy++){ 
    326327                a     = U.T()*C.get_row(iy);    // a is not modified, but  
    327328                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain.  
    … …  
    366367         
    367368        //statistics 
    368         int dim = IM->dimension(); 
     369        int16 dim = IM->dimension(); 
    369370        vec mu0; 
    370371        if ( !UI::get ( mu0, set, "mu0" ) ) 
    … …  
    400401{ 
    401402        // vypocet napeti v systemu (x,y) 
    402         int uf[2]; 
     403        int16 uf[2]; 
    403404        uf[0]=prevod(ux/Uref,15); 
    404405        uf[1]=prevod(uy/Uref,15); 
    405406         
    406         int Y_mes[2]; 
     407        int16 Y_mes[2]; 
    407408        // zadani mereni 
    408409        Y_mes[0]=prevod(isxd/Iref,15); 
    … …  
    410411                 
    411412        ////// vlastni rutina EKF -- ///////////////////////// 
    412         int t_sin,t_cos, tmp; 
     413        int16 t_sin,t_cos; 
     414        int32 tmp; 
    413415         
    414416        // implementace v PC 
    415417/*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
    416418        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ 
    417         t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); 
    418         t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); 
    419          
    420         tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13 
     419        tmp=prevod(sin(Thetaref*x_est[3]/32768.),15); 
     420        if (tmp>32767) t_sin =32767; else t_sin=tmp; 
     421        tmp=prevod(cos(Thetaref*x_est[3]/32768.),15); 
     422        if (tmp>32767) t_cos =32767; else t_cos=tmp; 
     423         
     424        tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13 
    421425        //             q15*q13 +          q13*q15      + q15*q13?? 
    422         x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13 
    423         x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13 
     426        x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
     427        x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
    424428        x_est[2]=x_est[2]; 
    425429        //               q15              + q15*q13 
    426         x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; 
     430        x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; 
    427431         
    428432        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15); 
    … …  
    431435        //void EKFfixed::update_psi(void) 
    432436        {        
    433                 PSI[2]=((long)cB*t_sin)>>15; 
    434                 tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
    435                 PSI[3]=((long)tmp*t_cos)>>15; 
    436                 PSI[6]=-((long)cB*t_cos)>>15; 
    437                 PSI[7]=((long)tmp*t_sin)>>15; 
     437                PSI[2]=((int32)cB*t_sin)>>15; 
     438                tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
     439                PSI[3]=((int32)tmp*t_cos)>>15; 
     440                PSI[6]=-((int32)cB*t_cos)>>15; 
     441                PSI[7]=((int32)tmp*t_sin)>>15; 
    438442        } 
    439443        { 
    440                 ivec Ad(PSI,16); 
     444                int Ai[16]; 
     445                for (int i=0;i<16; i++) Ai[i]=(int)(PSI[i]); 
     446                ivec Ad(Ai,16); 
    441447                log_level.store(logA,get_from_ivec(Ad)); 
    442448        } 
    … …  
    444450        ///////// end of copy /////////////// 
    445451        mmultAU(PSI,Uf,PSIU,4,4); 
    446         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
     452        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
    447453        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4); 
    448454         
    449455         { 
    450                 ivec Ud(Uf,16); 
     456                 int Ui[16];    for (int i=0;i<16; i++) Ui[i]=(int)Uf[i]; 
     457                  
     458                ivec Ud(Ui,16); 
    451459                log_level.store(logU,get_from_ivec(Ud)); 
    452                 ivec dd(Df,4); 
    453                 imat U(Uf,4,4); 
     460                 
     461                int di[16];     for (int i=0;i<16; i++) di[i]=(int)Df[i]; 
     462                ivec dd(di,4); 
     463                imat U(Ui,4,4); 
    454464                mat U2=to_mat(U)/32768; 
    455465                mat PP=U2*diag(to_vec(dd))*U2.T(); 
    … …  
    458468         } 
    459469         { 
    460                 ivec Gd(G,16); 
     470                 int Gi[16];    for (int i=0;i<16; i++) Gi[i]=(int)G[i]; 
     471                 ivec Gd(Gi,16); 
    461472                log_level.store(logG,get_from_ivec(Gd)); 
    462473        } 
    463474         
    464475         
    465         int difz[2]; 
     476        int16 difz[2]; 
    466477        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!! 
    467478        difz[1]=(Y_mes[1]-x_est[1]);//<<2; 
    468479 
    469480        { 
    470                 ivec dd(Df,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1]; 
     481                int Di[4];               for (int i=0;i<4; i++) Di[i]=(int)Df[i]; 
     482                ivec dd(Di,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1]; 
    471483                log_level.store(logD,get_from_ivec(dd)); 
    472484        } 
    473485         
    474         //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ); 
    475         int dR[2];dR[0]=R[0];dR[1]=R[3]; 
    476         //int 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];   
     486        //bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 
     487        int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
     488        //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];   
    477489        bierman_fast(difz,x_est,Uf,Df,dR,2,4); 
    478490        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
    … …  
    495507        Q[0]=prevod(.01,15);       // 0.05 
    496508        Q[5]=Q[0]; 
    497         Q[10]=prevod(0.01,15);      // 1e-3 
    498         Q[15]=prevod(0.01,15);      // 1e-3 
     509        Q[10]=prevod(0.0005,15);      // 1e-3 
     510        Q[15]=prevod(0.001,15);      // 1e-3 
    499511 
    500512        Uf[0]=0x7FFF;// !       // 0.05 
    … …  
    524536        cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
    525537        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
     538        // cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // < =========== 
    526539        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  
    527540        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
    … …  
    545558{ 
    546559        // vypocet napeti v systemu (x,y) 
    547         int uf[2]; 
     560        int16 uf[2]; 
    548561        uf[0]=prevod(ux/Uref,15); 
    549562        uf[1]=prevod(uy/Uref,15); 
    550563         
    551         int Y_mes[2]; 
     564        int16 Y_mes[2]; 
    552565        // zadani mereni 
    553566        Y_mes[0]=prevod(isxd/Iref,15); 
    … …  
    555568         
    556569        ////// vlastni rutina EKF -- ///////////////////////// 
    557         int t_sin,t_cos, tmp; 
     570        int16 t_sin,t_cos, tmp; 
    558571         
    559572        // implementace v PC 
    … …  
    563576        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15); 
    564577         
    565         tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13 
     578        tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13 
    566579        //             q15*q13 +          q13*q15      + q15*q13?? 
    567         x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13 
    568         x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13 
     580        x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 
     581        x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 
    569582        x_est[2]=x_est[2]; 
    570583        //               q15              + q15*q13 
    571         x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15; 
     584        x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; 
    572585         
    573586        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15); 
    … …  
    576589        //void EKFfixed::update_psi(void) 
    577590        {        
    578                 PSI[2]=((long)cB*t_sin)>>15; 
    579                 tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
    580                 PSI[3]=((long)tmp*t_cos)>>15; 
    581                 PSI[6]=-((long)cB*t_cos)>>15; 
    582                 PSI[7]=((long)tmp*t_sin)>>15; 
     591                PSI[2]=((int32)cB*t_sin)>>15; 
     592                tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!! 
     593                PSI[3]=((int32)tmp*t_cos)>>15; 
     594                PSI[6]=-((int32)cB*t_cos)>>15; 
     595                PSI[7]=((int32)tmp*t_sin)>>15; 
    583596        } 
    584597        { 
    585                 ivec Ad(PSI,16); 
    586                 log_level.store(logA,get_from_ivec(Ad)); 
     598/*              ivec Ad(PSI,16); 
     599                log_level.store(logA,get_from_ivec(Ad));*/ 
    587600        } 
    588601         
    589602        ///////// end of copy /////////////// 
    590603        mmultACh(PSI,Chf,PSICh,4,4); 
    591         //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
     604        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
    592605        householder(PSICh,Q,4); 
    593606        // COPY  
    594         for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];} 
     607        for (int16 ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];} 
    595608         
    596609        { 
    597                 ivec Chd(Chf,16); 
    598                 log_level.store(logCh,get_from_ivec(Chd)); 
    599         } 
    600          
    601          
    602         int difz[2]; 
     610/*              ivec Chd(Chf,16); 
     611                log_level.store(logCh,get_from_ivec(Chd));*/ 
     612        } 
     613         
     614         
     615        int16 difz[2]; 
    603616        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!! 
    604617        difz[1]=(Y_mes[1]-x_est[1]);//<<2; 
    … …  
    606619         
    607620        // 
    608         int dR[2];dR[0]=R[0];dR[1]=R[3]; 
    609         //int 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];   
     621        int16 dR[2];dR[0]=R[0];dR[1]=R[3]; 
     622        //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];   
    610623 
    611624        carlson(difz,x_est,Chf,dR,2,4); 
    … …  
    653666        cG=prevod(Tv*Wref/Thetaref,15); //in q15! 
    654667        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15); 
    655         cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  
     668        //      cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); //  <======= 
     669        cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); //  
    656670        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref); 
    657671         

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

    r1226 r1240  
    55 
    66  ----------------------------------- 
    7   BDM++ - C++ library for Bayesian Decision Making under Uncertainty 
     7  BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y 
    88 
    99  Using IT++ for numerical operations 
    … …  
    2626double minQ(double Q); 
    2727 
    28 void mat_to_int(const imat &M, int *I); 
    29 void vec_to_int(const ivec &v, int *I); 
     28void mat_to_int16(const imat &M, int16 *I); 
     29void vec_to_int16(const ivec &v, int16 *I); 
    3030void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref); 
    31          
     31 
     32#ifdef XXX 
    3233/*! 
    33 \brief Extended Kalman Filter with full matrices in fixed point arithmetic 
     34\brief Extended Kalman Filter with full matrices in fixed point16 arithmetic 
    3435 
    3536An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    … …  
    4142 
    4243/* Declaration of local functions */ 
    43 void prediction(int *ux); 
     44void prediction(int16 *ux); 
    4445void correction(void); 
    4546void update_psi(void); 
    4647 
    4748/* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ 
    48  int Q[16]; /* matrix [4,4] */ 
    49  int R[4]; /* matrix [2,2] */ 
    50  
    51  int x_est[4]; 
    52  int x_pred[4]; 
    53  int P_pred[16]; /* matrix [4,4] */ 
    54  int P_est[16]; /* matrix [4,4] */ 
    55  int Y_mes[2]; 
    56  int ukalm[2]; 
    57  int Kalm[8]; /* matrix [5,2] */ 
    58  
    59  int PSI[16]; /* matrix [4,4] */ 
    60  
    61  int temp15a[16]; 
    62  
    63  int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
    64  
    65  long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ 
     49 int16 Q[16]; /* matrix [4,4] */ 
     50 int16 R[4]; /* matrix [2,2] */ 
     51 
     52 int16 x_est[4]; 
     53 int16 x_pred[4]; 
     54 int16 P_pred[16]; /* matrix [4,4] */ 
     55 int16 P_est[16]; /* matrix [4,4] */ 
     56 int16 Y_mes[2]; 
     57 int16 ukalm[2]; 
     58 int16 Kalm[8]; /* matrix [5,2] */ 
     59 
     60 int16 PSI[16]; /* matrix [4,4] */ 
     61 
     62 int16 temp15a[16]; 
     63 
     64 int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
     65 
     66 int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ 
    6667 enorm<fsqmat> E; 
    6768 mat Ry; 
    … …  
    7071        //! Default constructor 
    7172        EKFfixed ():BM(),E(),Ry(2,2){ 
    72                 int i; 
     73                int16 i; 
    7374                for(i=0;i<16;i++){Q[i]=0;} 
    7475                for(i=0;i<4;i++){R[i]=0;} 
    … …  
    102103UIREGISTER(EKFfixed); 
    103104 
     105#endif 
     106 
    104107/*! 
    105 \brief Extended Kalman Filter with UD matrices in fixed point arithmetic 
     108\brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic 
    106109 
    107110An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    … …  
    115118                                 
    116119                /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ 
    117                 int Q[16]; /* matrix [4,4] */ 
    118                 int R[4]; /* matrix [2,2] */ 
    119                  
    120                 int x_est[4]; /* estimate and prediction */ 
    121                  
    122                 int PSI[16]; /* matrix [4,4] */ 
    123                 int PSIU[16]; /* matrix PIS*U, [4,4] */ 
    124                  
    125                 int Uf[16]; // upper triangular of covariance (inplace) 
    126                 int Df[4];  // diagonal covariance 
    127                 int Dfold[4]; // temp of D 
    128                 int G[16];  // temp for bierman 
    129                  
    130                 int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
     120                int16 Q[16]; /* matrix [4,4] */ 
     121                int16 R[4]; /* matrix [2,2] */ 
     122                 
     123                int16 x_est[4]; /* estimate and prediction */ 
     124                 
     125                int16 PSI[16]; /* matrix [4,4] */ 
     126                int16 PSIU[16]; /* matrix PIS*U, [4,4] */ 
     127                 
     128                int16 Uf[16]; // upper triangular of covariance (inplace) 
     129                int16 Df[4];  // diagonal covariance 
     130                int16 Dfold[4]; // temp of D 
     131                int16 G[16];  // temp for bierman 
     132                 
     133                int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
    131134                 
    132135                enorm<fsqmat> E; 
    … …  
    136139                //! Default constructor 
    137140                EKFfixedUD ():BM(),E(),Ry(2,2){ 
    138                         int i; 
     141                        int16 i; 
    139142                        for(i=0;i<16;i++){Q[i]=0;} 
    140143                        for(i=0;i<4;i++){R[i]=0;} 
    … …  
    173176 
    174177/*! 
    175  * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic 
     178 * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic 
    176179 *  
    177180 * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    … …  
    185188         
    186189        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ 
    187         int Q[16]; /* matrix [4,4] */ 
    188         int R[4]; /* matrix [2,2] */ 
    189          
    190         int x_est[4]; /* estimate and prediction */ 
    191          
    192         int PSI[16]; /* matrix [4,4] */ 
    193         int PSICh[16]; /* matrix PIS*U, [4,4] */ 
    194          
    195         int Chf[16]; // upper triangular of covariance (inplace) 
    196          
    197         int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
     190        int16 Q[16]; /* matrix [4,4] */ 
     191        int16 R[4]; /* matrix [2,2] */ 
     192         
     193        int16 x_est[4]; /* estimate and prediction */ 
     194         
     195        int16 PSI[16]; /* matrix [4,4] */ 
     196        int16 PSICh[16]; /* matrix PIS*U, [4,4] */ 
     197         
     198        int16 Chf[16]; // upper triangular of covariance (inplace) 
     199         
     200        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
    198201         
    199202        enorm<chmat> E; 
    … …  
    203206        //! Default constructor 
    204207        EKFfixedCh ():BM(),E(),Ry(2,2){ 
    205                 int i; 
     208                int16 i; 
    206209                for(i=0;i<16;i++){Q[i]=0;} 
    207210                for(i=0;i<4;i++){R[i]=0;} 
    … …  
    235238 
    236239 
    237 //! EKF for comparison of EKF_UD with its fixed-point implementation 
     240//! EKF for comparison of EKF_UD with its fixed-point16 implementation 
    238241class EKF_UDfix : public BM { 
    239242        protected: 

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

    r1239 r1240  
    33        Matrix operations 
    44 
    5         V. Smidl 
    6  
    7 Rev. 30.8.2010 
    8  
    9 30.8.2010      Prvni verze 
     5        V. Smidl, Z. Peroutka 
     6 
     7Rev. 28.10.2010   (ZP) 
     8 
     926.10.2010      Prvni verze (VS) 
     10 
     1126.10.2010      Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA. 
     1227.10.2010      Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci. 
     1328.10.2010      Drobne upravy v kodu. 
    1014 
    1115*************************************/ 
    12 #include "fixed.h" 
    13 #include "stdio.h" 
    14 #include <math.h> 
    1516 
    1617#include "matrix_vs.h" 
    1718 
    1819/* Matrix multiply Full matrix by upper diagonal matrix; */ 
    19 void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) { 
    20     unsigned int i, j, k; 
    21     long tmp_sum=0L; //in 15+qAU 
    22     int *m2pom; 
    23     int *m1pom=m1; 
    24     int *respom=result; 
     20void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) { 
     21    unsigned int16 i, j, k; 
     22    int32 tmp_sum=0L; //in 15+qAU 
     23    int16 *m2pom; 
     24    int16 *m1pom=m1; 
     25    int16 *respom=result; 
    2526 
    2627    for (i=0; i<rows; i++) //rows of result 
    2728    { 
    2829        for (j=0; j<columns; j++) //columns of result 
    29         { 
     30        { 
    3031            m2pom=up+j;//?? 
    3132 
    3233            for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1; 
    3334            { 
    34                 tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU); 
     35                tmp_sum+=((int32)(*(m1pom++))**m2pom)>>(15-qAU); 
    3536                m2pom+=columns; 
    3637            } 
    3738            // add the missing A(i,j) 
    38             tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift 
     39            tmp_sum +=(int32)(*m1pom)<<qAU; // no need to shift 
    3940            m1pom-=(j); // shift back to first element 
    4041 
    41             // saturation effect 
    42 /*            tmp_sum=tmp_sum>>15; 
    43             if (tmp_sum>32767) { 
    44                                 printf("Au - saturated\n"); 
    45             } 
    46             if (tmp_sum<-32768) { 
    47                                 printf("Au - saturated\n"); 
    48             }*/ 
    49 //                      printf("Au - saturated\n"); 
    50  
    5142            *respom++=tmp_sum>>15; 
    5243 
    … …  
    5748}; 
    5849 
    59 /* Matrix multiply Full matrix by upper diagonal matrix; */ 
    60 void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) { 
    61     unsigned int i, j, k; 
    62     long tmp_sum=0L; 
    63     int *m2pom; 
    64     int *m1pom=m1; 
    65     int *respom=result; 
    66  
    67     for (i=0; i<rows; i++) //rows of result 
    68     { 
    69         for (j=0; j<columns; j++) //columns of result 
    70         { 
    71             m2pom=up+j;//?? 
    72  
    73             for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1; 
    74             { 
    75                 tmp_sum+=(long)(*(m1pom++))**m2pom; 
    76                 m2pom+=columns; 
    77             } 
    78             m1pom-=(j+1); // shift back to first element 
    79  
    80             // saturation effect 
    81             tmp_sum=tmp_sum>>15; 
    82             if (tmp_sum>32767) { 
    83                 if (i!=3) tmp_sum=32767; 
    84             } 
    85             if (tmp_sum<-32768) { 
    86                 tmp_sum=-32768; 
    87             } 
    88             //                  printf("Au - saturated\n"); 
    89  
    90             *respom++=tmp_sum; 
    91  
    92             tmp_sum=0; 
    93         } 
    94         m1pom+=(columns); 
    95     } 
    96 }; 
    97  
    98 bool DBG=true; 
    99  
    100 void show(const char name[10], int *I, int n) { 
    101     if (!DBG) return; 
    102  
    103     printf("%s: ",name); 
    104     for (int i=0;i<n;i++) { 
    105         printf("%d ",*(I+i)); 
    106     } 
    107     printf("\n"); 
    108 } 
    109  
    110 // Thorton procedure - Kalman predictive variance in UD 
    111 void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) { 
    112     unsigned int i,j,k; 
    113     // copy D to Dold 
    114     int *Dold_pom=Dold; 
    115     for (i=0;i<rows;i++) { 
    116         *Dold_pom++=*D++; 
    117     } 
    118     D-=rows; // back to first D 
    119  
    120     // initialize G = eye() 
    121     int *G_pom = G; 
    122     *G_pom++=1<<14; 
    123     for (i=0;i<rows-1;i++) { 
    124         // clean elem before diag 
    125         for (j=0; j<rows; j++) { 
    126             *G_pom++=0.0; 
    127         } 
    128         *G_pom++=1<<14; 
    129     } 
    130     // eye created 
    131  
    132     long sigma; // in q30!!!!!! 
    133     for (i=rows-1; true;i--) { // check i==0 at the END! 
    134         sigma = 0; 
    135  
    136         for (j=0;j<rows; j++) { 
    137             //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]); 
    138             long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j]; 
    139 //                      printf("%d - %d\n",s1,s2); 
    140             sigma += s2; 
    141         } 
    142         sigma += Q[i*rows+i]<<15; // Q is in q15 
    143         for (j=i+1;j<rows; j++) { 
    144             sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j]; 
    145 //                      sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows]; 
    146         } 
    147  
    148         if (sigma>16384<<15) sigma = 16384<<15; 
    149         *(D+i)=sigma>>15; 
    150         if (D[i]==0) D[i]=1; 
    151 //show("D",D,5); 
    152  
    153         for (j=0;j<i;j++) { 
    154 //                      printf("\n%d,%d\n",i,j); 
    155             sigma =0; 
    156             for (k=0;k<rows;k++) { 
    157                                 int tmp= (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15)); 
    158                                 if (tmp>32767) printf("!"); 
    159                                 sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k]; 
    160             } 
    161             for (k=0;k<rows;k++) { 
    162                 sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k]; 
    163             } 
    164             long z=sigma/D[i]; // shift by 15 
    165             if (z>32767) z=32767; 
    166             if (z<-32768) z=-32768; 
    167  
    168             U[j*rows+i] = (int)z; 
    169  
    170  
    171             for (k=0;k<rows;k++) { 
    172                 PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; //qAU*q15/q15=qAU 
    173             } 
    174  
    175             for (k=0;k<rows;k++) { 
    176                 G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15; 
    177             } 
    178  
    179         } 
    180         //show("U",U,25); 
    181         //show("G",G,25); 
    182         if (i==0) return; 
    183     } 
    184 } 
    185  
    186 void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) { 
    187     int alpha; 
    188     int beta,lambda; 
    189     int b[5]; // ok even for 4-dim state 
    190     int *a; // in [0,1] -> q15 
    191     unsigned int iy,j,i; 
    192  
    193     int *b_j,*b_i; 
    194     int *a_j; 
    195     int *D_j; 
    196     int *U_ij; 
    197     int *x_i; 
    198     int dzs; 
     50 
     51void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) 
     52{ 
     53    int16 alpha; 
     54    int16 beta,lambda; 
     55    int16 b[5]; // ok even for 4-dim state 
     56    int16 *a; // in [0,1] -> q15 
     57    unsigned int16 iy,j,i; 
     58 
     59    int16 *b_j,*b_i; 
     60    int16 *a_j; 
     61    int16 *D_j; 
     62    int16 *U_ij; 
     63    int16 *x_i; 
     64 
     65    int32 z_pom; 
     66    int16 z_pom_int16; 
    19967 
    20068    a = U; // iyth row of U 
    … …  
    20270        // a is a row 
    20371        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++) 
    204             *b_j=((long)(*D_j)*(*a_j))>>15; 
    205  
    206         alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b 
     72            *b_j=((int32)(*D_j)*(*a_j))>>15; 
     73 
     74        alpha = R[iy]; //\alpha = R+vDv = R+a*b 
    20775        // R in q15, a in q15, b=q15 
    20876//              gamma = (1<<15)/alpha; //(in q15) 
    … …  
    21078        //max gamma = 0.0061 => gamma_ref = q7 
    21179        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) { 
    212             beta   = alpha; 
    213             lambda = -((long)(*a_j)<<15)/beta; 
    214             alpha  += ((long)(*a_j)*(*b_j))>>15; 
    215             D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long 
     80/*            beta=alpha; 
     81            lambda = -((int32)(*a_j)<<15)/beta; 
     82            alpha  += ((int32)(*a_j)*(*b_j))>>15; 
     83            D[j] = ((int32)beta**D_j)/alpha;*/ 
     84/*xx*/ 
     85            lambda=alpha; 
     86            alpha  += ((int32)(*a_j)*(*b_j))>>15; 
     87            D[j] = ((int32)lambda**D_j)/alpha; 
     88            z_pom_int16 = -((int32)(*a_j)<<15)/lambda; 
     89/*xx*/ 
     90 
    21691            if (*D_j==0) *D_j=1; 
    21792 
    21893            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) { 
    21994                beta   = *U_ij; 
    220                 *U_ij +=  ((long)lambda*(*b_i))>>15; 
    221                 *b_i  +=  ((long)beta*(*b_j))>>15; 
    222             } 
    223         } 
    224         dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations 
     95//                *U_ij +=  ((int32)lambda*(*b_i))>>15;         // puvodni reseni 
     96                *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni 
     97//                *U_ij -=  ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15;      // tohle funguje - problem je s tim pretypovanim na (int16) 
     98//                              *U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda; 
     99//                              z_pom = (((int32)(*a_j)*(*b_i))/lambda); 
     100/*                          z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda; 
     101                                if (z_pom > 32767) z_pom = 32767; 
     102                                if (z_pom < - 32768) z_pom = -32768; 
     103                                *U_ij = z_pom;                      /**/ 
     104//                *U_ij +=  ((int32)z_pom_int16*(*b_i))>>15;            // puvodni reseni - jen jina konstanta 
     105                *b_i  +=  ((int32)beta*(*b_j))>>15; 
     106            } 
     107        } 
    225108        // no shift due to gamma 
    226109        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) { 
    227             *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain 
    228         } 
    229  
    230         //cout << "Ub: " << U << endl; 
    231         //cout << "Db: " << D << endl <<endl; 
    232  
    233     } 
    234  
    235 } 
     110            *x_i  += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain 
     111        } 
     112    } 
     113} 
     114 
    236115 
    237116// Thorton procedure - Kalman predictive variance in UD 
    238 void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) { 
    239     unsigned int i,j,k; 
     117void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) { 
     118    unsigned int16 i,j,k; 
    240119    // copy D to Dold 
    241     int *Dold_i,*Dold_k; 
    242     int *D_i; 
    243     int *PSIU_ij,*PSIU_ik,*PSIU_jk; 
    244     int *Q_jj,*Q_ii,*Q_kk; 
    245     int *U_ji; 
    246     int *G_ik,*G_jk; 
    247     int irows,jrows; 
    248     long sigma; // in qAU+15!! 
    249     long z; 
     120    int16 *Dold_i,*Dold_k; 
     121    int16 *D_i; 
     122    int16 *PSIU_ij,*PSIU_ik,*PSIU_jk; 
     123    int16 *Q_jj,*Q_ii,*Q_kk; 
     124    int16 *U_ji; 
     125    int16 *G_ik,*G_jk; 
     126    int16 irows,jrows; 
     127    int32 sigma; // in qAU+15!! 
     128    int32 z; 
    250129 
    251130    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) { 
    … …  
    271150        for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold; 
    272151                k<rows; k++, PSIU_ik++,Dold_k++) { 
    273                         sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k); 
    274         } 
    275         sigma += *(Q+i+irows)<<qAU; 
     152                        sigma += (((int32)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k); 
     153        } 
     154        sigma += (int32)(*(Q+i+irows))<<qAU; 
    276155        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) { 
    277             sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows); 
    278  
    279         } 
    280  
    281                 if (sigma>((long)1<<(qAU+15))) { 
     156            sigma += (((int32)(*G_ik)**G_ik)>>16)**(Q+j+j*rows); 
     157        } 
     158 
     159                if (sigma>((int32)1<<(qAU+15))) { 
    282160                        *D_i = 32767; 
    283161//                      *(Dold+i)-=*(Q+i+irows); 
    … …  
    294172                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) { 
    295173 
    296                                 sigma += ((((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k); 
     174                                sigma += ((((int32)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k); 
    297175            } 
    298176 
    299177            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; 
    300178                    k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) { 
    301                 sigma += ((((long)*G_ik)**G_jk)>>16)**Q_kk; 
     179                sigma += ((((int32)*G_ik)**G_jk)>>16)**Q_kk; 
    302180            } 
    303181 
    … …  
    307185 
    308186            U_ji=U+jrows+i; 
    309             *U_ji = (int)z; 
     187            *U_ji = (int16)z; 
    310188 
    311189 
    312190            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows; 
    313191                    k<rows;k++,PSIU_ik++,PSIU_jk++) { 
    314                 *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15; 
     192                *PSIU_jk -= ((int32)*U_ji**PSIU_ik)>>15; 
    315193            } 
    316194 
    317195            for (k=0,G_jk=G+jrows,G_ik=G+irows; 
    318196                    k<rows;k++, G_jk++, G_ik++) { 
    319                 *G_jk -=  ((long)*U_ji**G_ik)>>15; 
     197                *G_jk -=  ((int32)*U_ji**G_ik)>>15; 
    320198            } 
    321199 
    … …  
    323201                if (i==0) return; 
    324202    } 
    325 } 
    326  
    327 void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) { 
    328     long alpha; 
    329     long gamma,beta,lambda; 
    330     int b[5]; // ok even for 4-dim state 
    331     int *a; // in [0,1] -> q15 
    332     unsigned int iy,j,i; 
    333  
    334     for (iy=0; iy<dimy; iy++) { 
    335         // a is a row 
    336         a = U+iy*dimx; // iyth row of U 
    337         for (j=0;j<dimx;j++) { 
    338             (j<iy)? b[j]=0: (j==iy)? b[j]=D[j] : b[j]=((long)D[j]*a[j])>>15; 
    339         } 
    340  
    341         alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b 
    342         // R in q15, a in q15, b=q15 
    343 //              gamma = (1<<15)/alpha; //(in q15) 
    344         //min alpha = R[iy] = 164 
    345         //max gamma = 0.0061 => gamma_ref = q7 
    346         for (j=0;j<dimx;j++) { 
    347             beta   = alpha; 
    348             lambda = -(long(a[j])<<15)/beta; 
    349             alpha  += ((long(a[j])*b[j])>>15); 
    350             D[j] = (((long(beta)<<15)/alpha)*D[j])>>15; //gamma is long 
    351             if (D[j]==0) D[j]=1; 
    352  
    353             //                  cout << "a: " << alpha << "g: " << gamma << endl; 
    354             for (i=0;i<j;i++) { 
    355                 beta   = U[i*dimx+j]; 
    356                 U[i*dimx+j] +=  (lambda*b[i])>>15; 
    357                 b[i]   +=  (beta*b[j])>>15; 
    358             } 
    359         } 
    360         int dzs = (long(difz[iy])<<15)/alpha;  // apply scaling to innovations 
    361         // no shift due to gamma 
    362         for (i=0; i<dimx; i++) { 
    363             xp[i]  += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain 
    364         } 
    365  
    366         //cout << "Ub: " << U << endl; 
    367         //cout << "Db: " << D << endl <<endl; 
    368  
    369     } 
    370  
    371203} 
    372204 

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

    r1230 r1240  
    1212 
    1313#define qAU 14 
     14#define qD 14 
     15 
     16#define int16 short int 
     17#define int32 int 
    1418 
    1519/* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */ 
    16 extern void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns); 
     20extern void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns); 
    1721 
    1822/* Matrix multiply Full matrix by upper diagonal matrix; */ 
    19 extern void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns); 
     23extern void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns); 
    2024 
    2125/* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/ 
    22 extern void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
     26extern void thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
    2327 
    2428/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/ 
    25 extern void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ); 
     29extern void bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 
    2630 
    2731/* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/ 
    28 extern void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
     32extern void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx); 
    2933 
    3034/* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/ 
    31 extern void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ); 
     35extern void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx ); 
    3236 
    3337/* perform Householder update of Ch matrix using PSI*Ch , Q, */ 
    34 extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx); 
     38extern void householder(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx); 
    3539 
    3640/* perform Givens update of Ch matrix using PSI*Ch , Q, */ 
    37 extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx); 
     41extern void givens(int16 *Ch /*= int16 *PSICh*/, int16 *Q, unsigned int16 dimx); 
    3842 
    3943/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ 
    40 extern void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ); 
     44extern void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx );