Show
Ignore:
Timestamp:
09/02/10 23:21:43 (14 years ago)
Author:
smidl
Message:

fast versions of thorton and bierman

Files:
1 modified

Legend:

Unmodified
Added
Removed

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

    r1178 r1179  
    1313#include "stdio.h" 
    1414/* Matrix multiply Full matrix by upper diagonal matrix; */ 
    15 void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns){ 
    16   unsigned int i, j, k; 
    17   long tmp_sum=0L; 
    18   int *m2pom; 
    19  
    20   for (i=0; i<rows; i++) //rows of result 
    21   { 
    22     for (j=0; j<columns; j++) //columns of result 
     15void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) { 
     16    unsigned int i, j, k; 
     17    long tmp_sum=0L; 
     18    int *m2pom; 
     19 
     20    for (i=0; i<rows; i++) //rows of result 
    2321    { 
    24       m2pom=up+j;//?? 
    25  
    26       for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1; 
    27       { 
    28         tmp_sum+=(long)(*m1++)**m2pom; 
    29         m2pom+=columns; 
    30       } 
    31       // add the missing A(i,j) 
    32                 tmp_sum +=(long)(*m1)<<15; // no need to shift 
    33                 m1-=(j); // shift back to first element 
    34  
    35       // saturation effect 
    36       tmp_sum=tmp_sum>>15; 
    37       if (tmp_sum>32767) tmp_sum=32767; 
    38       if (tmp_sum<-32768) tmp_sum=-32768; 
    39  
    40       *result++=tmp_sum; 
    41  
    42       tmp_sum=0; 
    43     } 
    44     m1+=(columns); 
    45   } 
     22        for (j=0; j<columns; j++) //columns of result 
     23        { 
     24            m2pom=up+j;//?? 
     25 
     26            for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1; 
     27            { 
     28                tmp_sum+=(long)(*m1++)**m2pom; 
     29                m2pom+=columns; 
     30            } 
     31            // add the missing A(i,j) 
     32            tmp_sum +=(long)(*m1)<<15; // no need to shift 
     33            m1-=(j); // shift back to first element 
     34 
     35            // saturation effect 
     36            tmp_sum=tmp_sum>>15; 
     37            if (tmp_sum>32767) tmp_sum=32767; 
     38            if (tmp_sum<-32768) tmp_sum=-32768; 
     39 
     40            *result++=tmp_sum; 
     41 
     42            tmp_sum=0; 
     43        } 
     44        m1+=(columns); 
     45    } 
    4646}; 
    4747 
    48 void UDprt(int *U, int *D){ 
    49         return; 
    50         for (int i=0;i<5;i++){ 
    51                 for (int j=0;j<5;j++){ 
    52                         printf("%d,",U[i*5+j]); 
     48void UDprt(int *U, int *D) { 
     49    return; 
     50    for (int i=0;i<5;i++) { 
     51        for (int j=0;j<5;j++) { 
     52            printf("%d,",U[i*5+j]); 
     53        } 
     54        printf("\n"); 
     55    } 
     56    for (int i=0;i<5;i++) { 
     57        printf("%d,",D[i]); 
     58    } 
     59    printf("\n"); 
     60} 
     61 
     62// Thorton procedure - Kalman predictive variance in UD 
     63void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) { 
     64    unsigned int i,j,k; 
     65    // copy D to Dold 
     66    int *Dold_pom=Dold; 
     67    for (i=0;i<rows;i++) { 
     68        *Dold_pom++=*D++; 
     69    } 
     70    D-=rows; // back to first D 
     71 
     72    // initialize G = eye() 
     73    int *G_pom = G; 
     74    *G_pom++=1<<14; 
     75    for (i=0;i<rows-1;i++) { 
     76        // clean elem before diag 
     77        for (j=0; j<rows; j++) { 
     78            *G_pom++=0.0; 
     79        } 
     80        *G_pom++=1<<14; 
     81    } 
     82    // eye created 
     83 
     84    long sigma; // in q15 
     85    for (i=rows-1; i>=0;i--) { // check i==0 at the END! 
     86        sigma = 0; 
     87                 
     88        for (j=0;j<rows; j++) { 
     89            sigma += (((long)PSIU[i*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]); 
     90                        printf("-- %d,%d --", PSIU[i*rows+j], Dold[i]); 
    5391                } 
    54                 printf("\n"); 
    55         } 
    56         for (int i=0;i<5;i++){printf("%d,",D[i]);} 
    57         printf("\n"); 
    58 } 
    59   
     92                sigma += Q[i*rows+i]; 
     93        for (j=i+1;j<rows; j++) { 
     94            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j]; 
     95                        printf("-- %d,%d --", G[i*rows+j], Q[j*rows+j]); 
     96                } 
     97        printf("%d\n",sigma); 
     98                 
     99        *(D+i)=sigma>>15; 
     100        if (D[i]==0) D[i]=1; 
     101 
     102        /*              printf("d=sig\n"); 
     103                        UDprt(U,D); 
     104                        UDprt(G,Dold);*/ 
     105 
     106        for (j=0;j<i;j++) { 
     107//                      printf("\n%d,%d\n",i,j); 
     108            sigma =0; 
     109            for (k=0;k<rows;k++) { 
     110                sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k]; 
     111                   } 
     112            printf("%d\n",sigma); 
     113                        for (k=0;k<rows;k++) { 
     114                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k]; 
     115            } 
     116            printf("%d\n",sigma); 
     117                        long z=sigma/D[i]; // shift by 15 
     118            if (z>32767) z=32767; 
     119            if (z<-32768) z=-32768; 
     120 
     121            U[j*rows+i] = (int)z; 
     122 
     123            /*                  printf("U=sig/D\n"); 
     124                                UDprt(U,D); 
     125                                UDprt(G,Dold);*/ 
     126 
     127            for (k=0;k<rows;k++) { 
     128                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; 
     129            } 
     130            for(int x=0;x<16;x++) printf("%d,", PSIU[x]); 
     131                        printf("\n"); 
     132                         
     133            for (k=0;k<rows;k++) { 
     134                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15; 
     135            } 
     136            for(int x=0;x<16;x++) printf("%d,", G[x]); 
     137                        printf("\n"); 
     138                         
     139            /*                  printf("end\n"); 
     140                                UDprt(U,D); 
     141                                UDprt(G,Dold); 
     142                                printf("\n");   */ 
     143        } 
     144        if (i==0) return; 
     145    } 
     146} 
     147 
     148void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) { 
     149    int alpha; 
     150    int beta,lambda; 
     151    int b[5]; // ok even for 4-dim state 
     152    int *a; // in [0,1] -> q15 
     153    unsigned int iy,j,i; 
     154 
     155    int *b_j,*b_i; 
     156    int *a_j; 
     157    int *D_j; 
     158    int *U_ij; 
     159    int *x_i; 
     160    a = U; // iyth row of U 
     161    for (iy=0; iy<dimy; iy++, a+=dimx) { 
     162        // a is a row 
     163        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++) 
     164            *b_j=((long)(*D_j)*(*a_j))>>15; 
     165 
     166        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b 
     167        // R in q15, a in q15, b=q15 
     168//              gamma = (1<<15)/alpha; //(in q15) 
     169        //min alpha = R[iy] = 164 
     170        //max gamma = 0.0061 => gamma_ref = q7 
     171        for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) { 
     172            beta   = alpha; 
     173            lambda = -((long)(*a_j)<<15)/beta; 
     174            alpha  += ((long)(*a_j)*(*b_j)>>15); 
     175            D[j] = (((long)beta<<15)/alpha)*(*D_j)>>15; //gamma is long 
     176            if (*D_j==0) *D_j=1; 
     177 
     178            for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) { 
     179                beta   = *U_ij; 
     180                *U_ij +=  (lambda*(*b_i))>>15; 
     181                *b_i  +=  ((long)beta*(*b_j))>>15; 
     182            } 
     183        } 
     184        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations 
     185        // no shift due to gamma 
     186        for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) { 
     187            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain 
     188        } 
     189 
     190        //cout << "Ub: " << U << endl; 
     191        //cout << "Db: " << D << endl <<endl; 
     192 
     193    } 
     194 
     195} 
     196 
    60197// Thorton procedure - Kalman predictive variance in UD 
    61 void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows){ 
    62         unsigned int i,j,k; 
    63         // copy D to Dold 
    64         int *Dold_pom=Dold; 
    65         for (i=0;i<rows;i++){*Dold_pom++=*D++;} 
    66         D-=rows; // back to first D 
    67          
    68         // initialize G = eye() 
    69         int *G_pom = G; 
    70         *G_pom++=1<<14; 
    71         for (i=0;i<rows-1;i++){ 
    72                 // clean elem before diag 
    73                 for (j=0; j<rows; j++){ 
    74                         *G_pom++=0.0;  
     198void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) { 
     199    unsigned int i,j,k; 
     200    // copy D to Dold 
     201    int *Dold_i,*Dold_k; 
     202    int *D_i; 
     203    int *PSIU_ij,*PSIU_ik,*PSIU_jk; 
     204    int *Q_jj,*Q_ii,*Q_kk; 
     205    int *U_ji; 
     206    int *G_ik,*G_jk; 
     207    int irows,jrows; 
     208    long sigma; // in q15 
     209 
     210    for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) { 
     211        *Dold_i=*D_i; 
     212    } 
     213 
     214    // initialize G = eye() 
     215    G_ik= G; 
     216    *G_ik++=1<<14; 
     217    for (i=0;i<rows-1;i++) { 
     218        // clean elem before diag 
     219        for (k=0; k<rows; k++) { 
     220            *G_ik++=0; 
     221        } 
     222        *G_ik++=1<<14; 
     223    } 
     224    // eye created 
     225 
     226    for (i=rows-1, Dold_i=Dold+i, D_i=D+i; 
     227            true; i--, Dold_i--,D_i--) { // stop if i==0 at the END! 
     228        irows=i*rows; 
     229        sigma = 0; 
     230        for (k=0, PSIU_ik=PSIU+irows; 
     231           k<rows; k++, PSIU_ik++) {//Dold_i= 
     232            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_i); 
     233                        printf("-- %d,%d --", *PSIU_ik, *Dold_i); 
    75234                } 
    76                 *G_pom++=1<<14; 
    77         } 
    78         // eye created 
    79          
    80         long sigma; // in q15 
    81         for (i=rows-1; i>=0;i--){ // check i==0 at the END! 
    82                 sigma = 0; 
     235                sigma += *(Q+i+irows); 
     236        for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) { 
     237            sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows); 
     238                        printf("++ %d,%d ++", *G_ik,  *(Q+j+j*rows)); 
     239                         
     240        } 
     241        printf("%d\n",sigma); 
    83242                 
    84                 for (j=0;j<rows; j++){ 
    85                         sigma += (((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]); 
    86                 } 
    87                 sigma += Q[i+i*rows]; 
    88                 for (j=i+1;j<rows; j++){ 
    89                         sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows]; 
    90                 } 
    91                  
    92                 *(D+i)=sigma>>15; 
    93                 if (D[i]==0) D[i]=1; 
    94  
    95 /*              printf("d=sig\n"); 
    96                 UDprt(U,D); 
    97                 UDprt(G,Dold);*/ 
    98                  
    99                 for (j=0;j<i;j++){ 
    100 //                      printf("\n%d,%d\n",i,j); 
    101                         sigma =0; 
    102                         for (k=0;k<rows;k++){ 
    103                                 sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k]; 
    104                         } 
    105                         for (k=0;k<rows;k++){  
    106                                 sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];  
    107                         }                
    108                         long z=sigma/D[i]; // shift by 15 
    109                         if (z>32767) z=32767; 
    110                         if (z<-32768) z=-32768;   
    111                          
    112                         U[j*rows+i] = (int)z; 
    113  
    114 /*                      printf("U=sig/D\n"); 
    115                         UDprt(U,D); 
    116                         UDprt(G,Dold);*/ 
    117                          
    118                         for (k=0;k<rows;k++){  
    119                                 PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15;  
    120                         } 
    121                         for (k=0;k<rows;k++){  
    122                                 G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;  
    123                         } 
    124                          
    125 /*                      printf("end\n"); 
    126                         UDprt(U,D); 
    127                         UDprt(G,Dold); 
    128                         printf("\n");   */ 
    129                 } 
    130                 if(i==0) return; 
    131         } 
    132 } 
    133  
    134 void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ){ 
    135         long alpha; 
    136         long gamma,beta,lambda; 
    137         int b[5]; 
    138         int *a; // in [0,1] -> q15 
    139         unsigned int iy,j,i; 
    140          
    141         for (iy=0; iy<dimy; iy++){ 
    142                 // a is a row  
    143                 a = U+iy*dimx; // iyth row of U 
    144                 for (j=0;j<dimx;j++) 
    145                         b[j]=((long)D[j]*a[j])>>15; 
    146                  
    147                 alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b 
    148                 // R in q15, a in q15, b=q15 
    149                 gamma = (1<<15)/alpha; //(in q15) 
    150                 //min alpha = R[iy] = 164 
    151                 //max gamma = 0.0061 => gamma_ref = q7 
    152                 for (j=0;j<dimx;j++){ 
    153                         beta   = alpha;  
    154                         lambda = -((long)a[j]<<15)/beta;  
    155                         alpha  += ((long)(a[j])*b[j]>>15);  
    156                         D[j] = (((long)beta<<15)/alpha)*D[j]>>15; //gamma is long 
    157                         if (D[j]==0) D[j]=1; 
    158                          
    159                         //                      cout << "a: " << alpha << "g: " << gamma << endl; 
    160                         for (i=0;i<j;i++){ 
    161                                 beta   = U[i*dimx+j];  
    162                                 U[i*dimx+j] +=  (lambda*b[i])>>15;  
    163                                 b[i]   +=  (beta*b[j])>>15;  
    164                         } 
    165                 } 
    166                 int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations  
    167                                                                         // no shift due to gamma 
    168                 for (i=0; i<dimx; i++){ 
    169                         xp[i]  += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain  
    170                 } 
    171                  
    172                 //cout << "Ub: " << U << endl; 
    173                 //cout << "Db: " << D << endl <<endl; 
    174                  
    175         } 
    176          
    177 } 
     243        *D_i=sigma>>15; 
     244        if (*D_i==0) *D_i=1; 
     245 
     246 
     247        for (j=0;j<i;j++) { 
     248            jrows = j*rows; 
     249 
     250            sigma =0; 
     251            for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold; 
     252                    k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) { 
     253                                 
     254                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k; 
     255            } 
     256            printf("%d\n",sigma); 
     257                         
     258            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k; 
     259            k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) { 
     260                sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk; 
     261            } 
     262            printf("%d\n",sigma); 
     263                         
     264                        long z=sigma/(*D_i); // shift by 15 
     265            if (z>32767) z=32767; 
     266            if (z<-32768) z=-32768; 
     267 
     268            U_ji=U+jrows+i; 
     269            *U_ji = (int)z; 
     270 
     271 
     272            for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows; 
     273                    k<rows;k++,PSIU_ik++,PSIU_jk++) { 
     274                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15; 
     275            } 
     276            for(int x=0;x<16;x++) printf("%d,", PSIU[x]); 
     277                        printf("\n"); 
     278                         
     279            for (k=0,G_jk=G+jrows,G_ik=G+irows; 
     280            k<rows;k++, G_jk++, G_ik++) { 
     281                *G_jk -=  ((long)*U_ji**G_ik)>>15; 
     282            } 
     283            for(int x=0;x<16;x++) printf("%d,", G[x]); 
     284                        printf("\n"); 
     285                         
     286        } 
     287        if (i==0) return; 
     288    } 
     289} 
     290 
     291void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) { 
     292    long alpha; 
     293    long gamma,beta,lambda; 
     294    int b[5]; // ok even for 4-dim state 
     295    int *a; // in [0,1] -> q15 
     296    unsigned int iy,j,i; 
     297 
     298    for (iy=0; iy<dimy; iy++) { 
     299        // a is a row 
     300        a = U+iy*dimx; // iyth row of U 
     301        for (j=0;j<dimx;j++) 
     302            b[j]=((long)D[j]*a[j])>>15; 
     303 
     304        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b 
     305        // R in q15, a in q15, b=q15 
     306//              gamma = (1<<15)/alpha; //(in q15) 
     307        //min alpha = R[iy] = 164 
     308        //max gamma = 0.0061 => gamma_ref = q7 
     309        for (j=0;j<dimx;j++) { 
     310            beta   = alpha; 
     311            lambda = -((long)a[j]<<15)/beta; 
     312            alpha  += ((long)(a[j])*b[j]>>15); 
     313            D[j] = (((long)beta<<15)/alpha)*D[j]>>15; //gamma is long 
     314            if (D[j]==0) D[j]=1; 
     315 
     316            //                  cout << "a: " << alpha << "g: " << gamma << endl; 
     317            for (i=0;i<j;i++) { 
     318                beta   = U[i*dimx+j]; 
     319                U[i*dimx+j] +=  (lambda*b[i])>>15; 
     320                b[i]   +=  (beta*b[j])>>15; 
     321            } 
     322        } 
     323        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations 
     324        // no shift due to gamma 
     325        for (i=0; i<dimx; i++) { 
     326            xp[i]  += ((long)dzs*b[i])>>15; // multiply by unscaled Kalman gain 
     327        } 
     328 
     329        //cout << "Ub: " << U << endl; 
     330        //cout << "Db: " << D << endl <<endl; 
     331 
     332    } 
     333 
     334}