Changeset 1230

Show
Ignore:
Timestamp:
10/25/10 09:41:23 (14 years ago)
Author:
smidl
Message:

givens + new Q for UD

Location:
applications/pmsm/simulator_zdenek
Files:
4 modified

Legend:

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

    r1229 r1230  
    445445        mmultAU(PSI,Uf,PSIU,4,4); 
    446446        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
    447         thorton(Uf,Df,PSIU,Q,G,Dfold,4); 
     447        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4); 
    448448         
    449449         { 
     
    469469        int dR[2];dR[0]=R[0];dR[1]=R[3]; 
    470470        //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];   
    471         bierman(difz,x_est,Uf,Df,dR,2,4); 
     471        bierman_fast(difz,x_est,Uf,Df,dR,2,4); 
    472472        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
    473473         
     
    487487{ 
    488488        // Tuning of matrix Q 
    489         Q[0]=prevod(.0005,15);       // 0.05 
     489        Q[0]=prevod(.001,15);       // 0.05 
    490490        Q[5]=Q[0]; 
    491         Q[10]=prevod(0.0001,15);      // 1e-3 
     491        Q[10]=prevod(0.001,15);      // 1e-3 
    492492        Q[15]=prevod(0.0001,15);      // 1e-3 
    493493 
     
    585585        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx); 
    586586        householder(PSICh,Q,4); 
     587        // COPY  
     588        for (int ii=0; ii<16; ii++){Chf[ii]=PSICh[ii];} 
    587589         
    588590        { 
     
    600602        int dR[2];dR[0]=R[0];dR[1]=R[3]; 
    601603        //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];   
     604 
    602605        carlson(difz,x_est,Chf,dR,2,4); 
    603606        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3]; 
     
    618621{ 
    619622        // Tuning of matrix Q 
    620         Q[0]=prevod(.01,15);       // 0.05 
     623        Q[0]=prevod(.001,15);       // 0.05 
    621624        Q[5]=Q[0]; 
    622         Q[10]=prevod(0.0001,15);      // 1e-3 
     625        Q[10]=prevod(0.001,15);      // 1e-3 
    623626        Q[15]=prevod(0.0001,15);      // 1e-3 
    624627         
    625         Chf[0]=0x7FFF;// !       // 0.05 
     628        Chf[0]=0x3FFF;// !       // 0.05 
    626629        Chf[1]=Chf[2]=Chf[3]=Chf[4]=0; 
    627         Chf[5]=0x7FFF;//! 
     630        Chf[5]=0x3FFF;//! 
    628631        Chf[6]=Chf[6]=Chf[8]=Chf[9]=0; 
    629         Chf[10]=0x7FFF;//!      // 1e-3 
     632        Chf[10]=0x3FFF;//!      // 1e-3 
    630633        Chf[11]=Chf[12]=Chf[13]=Chf[4]=0; 
    631         Chf[15]=0x7FFF;      // 1e-3 
     634        Chf[15]=0x3FFF;      // 1e-3 
    632635                 
    633636        // Tuning of matrix R 
  • applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

    r1228 r1230  
    136136        for (j=0;j<rows; j++) { 
    137137            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]); 
    138             long s2=((((long)PSIU[i*rows+j]*PSIU[i*rows+j]))>>(2*qAU-15))*Dold[j]; 
     138            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j]; 
    139139//                      printf("%d - %d\n",s1,s2); 
    140140            sigma += s2; 
    141141        } 
    142         sigma += Q[i*rows+i]<<15; 
     142        sigma += Q[i*rows+i]<<15; // Q is in q15 
    143143        for (j=i+1;j<rows; j++) { 
    144144            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j]; 
     
    146146        } 
    147147 
    148 //        if (sigma>16384<<15) sigma = 16384<<15; 
     148        if (sigma>16384<<15) sigma = 16384<<15; 
    149149        *(D+i)=sigma>>15; 
    150150        if (D[i]==0) D[i]=1; 
     
    155155            sigma =0; 
    156156            for (k=0;k<rows;k++) { 
    157                                 sigma += (((long(PSIU[i*rows+k])*PSIU[j*rows+k]))>>(2*qAU-15))*Dold[k]; 
     157                                sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k]; 
    158158            } 
    159159            for (k=0;k<rows;k++) { 
     
    266266        for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold; 
    267267                k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i= 
    268             sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k); 
     268                sigma += (((long)(*PSIU_ik)**Dold_k)>>(2*qAU-15))*(*PSIU_ik); 
    269269        } 
    270270        sigma += *(Q+i+irows)<<15; 
     
    273273 
    274274        } 
    275  
     275         
     276        if (sigma>16384<<15) sigma = 16384<<15; 
    276277        *D_i=sigma>>15; 
    277278        if (*D_i==0) *D_i=1; 
     
    285286                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) { 
    286287 
    287                 sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k; 
     288                                sigma += ((((long)*PSIU_ik)**Dold_k)>>(2*qAU-15))**PSIU_jk; 
    288289            } 
    289290 
     
    364365/* square root of 0<a<1 using taylor at 0.5 in q15*/ 
    365366int int_sqrt(int x) { 
     367        double xd(double(x)/32768.); 
     368        return round(sqrt(xd)*32768); 
     369         
    366370    //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2 
    367371    //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5); 
     
    404408        /*               double sigf=double(sigma)/(1<<15); 
    405409                         double alpf = sqrt(sigf);*/ 
     410                if (sigma>16384) sigma=16384; 
    406411        alpha=int_sqrt(sigma); 
    407412        // alpha = alpf*(1<<15); 
     
    421426        } 
    422427        alpha=sigma>>1; 
     428                if (alpha==0) alpha =1; 
    423429        for (i=0;i<=k;i++) { 
    424430            sigma=0; 
     
    474480    } 
    475481} 
     482 
     483/* perform Householder update of Ch matrix using PSI*Ch , Q, */ 
     484extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx){ 
     485        int i,j,k; 
     486        int rho,s,c,tau; 
     487         
     488        int A[25];//beware 
     489        // copy Q to A 
     490        for (i=0;i<dimx*dimx;i++) { 
     491                A[i]=Q[i]; 
     492        } 
     493         
     494         
     495        for (i=dimx-1; i>=0; i--){ 
     496                for (j=0; j<dimx; j++) { 
     497                        rho=int_sqrt(((long)Ch[i*dimx+i]*Ch[i*dimx+i]+long(A[i*dimx+j])*A[i*dimx+j])>>15); 
     498                        if (rho==0) break; 
     499                        s=(long(A[i*dimx+j])<<15)/rho; 
     500                        c=(long(Ch[i*dimx+i])<<15)/rho; 
     501                        for (k=0;k<=i; k++){ 
     502                                tau=(long(c)*A[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15; 
     503                                Ch[k*dimx +i]=(long(s)*A[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15; 
     504                                A[k*dimx +j]=tau; 
     505                        } 
     506                } 
     507        } 
     508        for (j=0; j<i; j++){ 
     509                rho=int_sqrt((long(Ch[i*dimx+i])*Ch[i*dimx+i]+long(Ch[i*dimx+j])*Ch[i*dimx+j])>>15); 
     510                if (rho==0) break;  
     511                s=(long(Ch[i*dimx+j])<<15)/rho; 
     512                c=(long(Ch[i*dimx+i])<<15)/rho; 
     513                for (k=0; k<=i; k++){ 
     514                        tau=(long(c)*Ch[k*dimx+j]-long(s)*Ch[k*dimx+i])>>15; 
     515                        Ch[k*dimx+i]=(long(s)*Ch[k*dimx+j]+long(c)*Ch[k*dimx+i])>>15; 
     516                        Ch[k*dimx+j]=tau; 
     517                } 
     518        } 
     519} 
  • applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

    r1228 r1230  
    3434extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx); 
    3535 
     36/* perform Givens update of Ch matrix using PSI*Ch , Q, */ 
     37extern void givens(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx); 
     38 
    3639/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/ 
    3740extern void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ); 
  • applications/pmsm/simulator_zdenek/test_Ch.cpp

    r1227 r1230  
    8989         
    9090        householder(PSICh,Qf,5); 
     91//      givens(PSICh,Qf,5); 
    9192 
    9293        /////// disp 
     
    103104        mat Ch2 = 0.9*diag(1./sqrt(d))*Ch; 
    104105        mat_to_int(round_i(Ch2*multip),Chf);             
    105          
     106 
     107        D = pow(diag(Ch2),2); 
     108        U = sqrt(diag(1./D)); 
     109        U = Ch2*U; 
     110                 
    106111        vec     ydif = 2*randu(2)-1; 
    107112        vec     xp = 2*randu(5)-1; 
     
    159164         
    160165        carlson(difz,xf, Chf, Rf, 2, 5); 
     166         
    161167        cout << endl<<"after Carlson" <<endl; 
    162168        cout << "x: "<< round_i(xp*multip) <<endl; 
     
    164170 
    165171        { 
    166         imat Chcmp(Chf,5,5); 
    167         cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl; 
     172                imat Chcmp(Chf,5,5); 
     173                cout << "Delat Ch: " << round_i(Ch*multip-Chcmp) << endl; 
     174                cout << "Delat Ch: " << Chcmp << endl; 
    168175        } 
    169176