Changeset 1235

Show
Ignore:
Timestamp:
10/27/10 11:10:34 (14 years ago)
Author:
smidl
Message:

thorton_fast: cutoff od D at 1<<15, sigma in qUA+15, G in q15

Files:
1 modified

Legend:

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

    r1233 r1235  
    4040 
    4141            // saturation effect 
    42             tmp_sum=tmp_sum>>15; 
     42/*            tmp_sum=tmp_sum>>15; 
    4343            if (tmp_sum>32767) { 
    4444                                printf("Au - saturated\n"); 
     
    4646            if (tmp_sum<-32768) { 
    4747                                printf("Au - saturated\n"); 
    48             } 
     48            }*/ 
    4949//                      printf("Au - saturated\n"); 
    5050 
    51             *respom++=tmp_sum; 
     51            *respom++=tmp_sum>>15; 
    5252 
    5353            tmp_sum=0; 
     
    244244    int *G_ik,*G_jk; 
    245245    int irows,jrows; 
    246     long sigma; // in q30!! 
     246    long sigma; // in qAU+15!! 
    247247 
    248248    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) { 
     
    252252    // initialize G = eye() 
    253253    G_ik= G; 
    254     *G_ik++=1<<14; 
     254    *G_ik++=32767; 
    255255    for (i=0;i<rows-1;i++) { 
    256256        // clean elem before diag 
     
    258258            *G_ik++=0; 
    259259        } 
    260         *G_ik++=1<<14; 
     260        *G_ik++=32767; 
    261261    } 
    262262    // eye created 
     
    268268        for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold; 
    269269                k<rows; k++, PSIU_ik++,Dold_k++) { 
    270                 sigma += (((long)(*PSIU_ik)**Dold_k)>>(qAU))*(*PSIU_ik)<<(15-qAU); 
    271         } 
    272         sigma += *(Q+i+irows)<<15; 
     270                        sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k); 
     271        } 
     272        sigma += *(Q+i+irows)<<qAU; 
    273273        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) { 
    274             sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows); 
    275  
    276         } 
    277          
    278         *D_i=sigma>>15; 
     274            sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows); 
     275 
     276        }         
     277 
     278                if (sigma>(1<<(qAU+15))) { 
     279                        *D_i = 32767; 
     280                        *(Dold+i)-=*(Q+i+irows); 
     281                } else { 
     282                        *D_i=sigma>>qAU; 
     283                } 
    279284        if (*D_i==0) *D_i=1; 
    280                 if (*D_i>16384) *D_i = 16384; 
    281                  
    282285 
    283286        for (j=0;j<i;j++) { 
     
    288291                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) { 
    289292 
    290                                 sigma += (((long(*PSIU_ik)**Dold_k)>>qAU)**PSIU_jk)<<(15-qAU); 
    291             } 
    292  
    293             for (k=i+1,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; 
     293                                sigma += (((long(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k); 
     294            } 
     295 
     296            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; 
    294297                    k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) { 
    295                 sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk; 
    296             } 
    297  
    298             long z=sigma/(*D_i); // shift by 15 
     298                sigma += ((((long)*G_ik)**G_jk)>>16)**Q_kk; 
     299            } 
     300 
     301            long z=(sigma/(*D_i))<<(15-qAU); // shift to q15 
    299302            if (z>32767) z=32767; 
    300303            if (z<-32768) z=-32768; 
     
    315318 
    316319        } 
    317         if (i==0) return; 
     320                if (i==0) return; 
    318321    } 
    319322}