Changeset 1321 for applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

Timestamp:

04/04/11 14:15:18 (13 years ago)

Author:

smidl

Message:

Carlson fast with C + test

Files:

• applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -19 +19 @@
 #include "matrix.h"
 
+#include <stdio.h>
 /* Matrix multiply Full matrix by upper diagonal matrix; */
 void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
@@ -204 +205 @@
                                 //                *U_ij +=  ((int32)lambda*(*b_i))>>15;         // puvodni reseni
                                 *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
-                                //                *U_ij -=  ((int32)((int16)((int32)(*a_j)<<15)/lambda)**b_i)>>15;      // tohle funguje - problem je s tim pretypovanim na (int16)
-                                //                              *U_ij -= (int16)((int32)(*a_j)*(*b_i))/lambda;
-                                //                              z_pom = (((int32)(*a_j)*(*b_i))/lambda);
-                                /*                          z_pom = (int32)(*U_ij)-(int16)((int32)(*a_j)*(*b_i))/lambda;
-                                 *                              if (z_pom > 32767) z_pom = 32767;
-                                 *                              if (z_pom < - 32768) z_pom = -32768;
-                                 *U_ij = z_pom;                      /**/
-                                 //                *U_ij +=  ((int32)z_pom_int16*(*b_i))>>15;           // puvodni reseni - jen jina konstanta
                                  *b_i  +=  ((int32)beta*(*b_j))>>15;
+                                 printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
                         }
                 }
@@ -218 +212 @@
                 for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
                         *x_i  += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain
-                        
                 }
         }
@@ -700 +693 @@
 }
 
+void carlson_fastC(int16 *difz, int16 *xp, int16 *Ch, int16 *C, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) {
+        int16 alpha,beta,gamma;
+        int16 delta, eta,epsilon,zeta,sigma,tau;
+        int16 i,j,iy;
+        int16 w[5];
+        int32 tmp_long;
+        
+        int16 *Ch_ij, *w_i, *x_i, *C_yi;
+        
+        
+        for (iy=0; iy<dimy; iy++)
+        {
+                alpha=R[iy];
+                delta = difz[iy];
+                
+                for (j=0;j<dimx;j++)
+                {
+                        C_yi = C+iy*dimx; 
+                        sigma = 0;
+                        Ch_ij=Ch+j;
+                        for (i=0;i<=j;i++){
+                                sigma += ((int32)*Ch_ij**C_yi)>>15; //sigma in qCh  
+                                Ch_ij+=dimx;
+                                C_yi++;
+                        }
+                        
+                        //sigma=Ch[iy*dimx+j];
+                        beta=alpha;
+                        //            alpha+=((long)sigma*sigma)>>15;
+                        alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;                              // vyssi presnost
+                        //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
+                        gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
+                        
+                        w[j]=0;
+                        
+                        Ch_ij=Ch+j;
+                        w_i=w;
+                        
+                        for (i=0;i<=j;i++)
+                        {
+                                //                tau=Ch[i*dimx+j];
+                                tau=*Ch_ij;
+                                //                              tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
+                                tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
+                                
+                                if (tmp_long>32767)
+                                        tmp_long=32767;
+                                if (tmp_long<-32768)
+                                        tmp_long=-32768;
+                                *Ch_ij=tmp_long;
+                                
+                                //                w_i+=((long)tau*sigma)>>15;
+                                *w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15;
+                                
+                                w_i++;
+                                Ch_ij+=dimx;
+                        }
+                }
+                
+                x_i=xp;
+                w_i=w;
+                for (i=0;i<dimx;i++) {
+                        //            xp[i]+=((long)w[i]*delta)/alpha;
+                        //            *x_i+=((long)*w_i*delta)/alpha;
+                        *x_i=((int32)*x_i*alpha+(int32)*w_i*delta)/alpha;               // vyssi presnost
+                        x_i++;
+                        w_i++;
+                }
+        }
+}
+