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

Timestamp:

10/29/10 19:10:03 (14 years ago)

Author:

smidl

Message:

prechod na int16

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -3 +3 @@
         Matrix operations
 
-        V. Smidl
-
-Rev. 30.8.2010
-
-30.8.2010      Prvni verze
+        V. Smidl, Z. Peroutka
+
+Rev. 28.10.2010   (ZP)
+
+26.10.2010      Prvni verze (VS)
+
+26.10.2010      Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA.
+27.10.2010      Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci.
+28.10.2010      Drobne upravy v kodu.
 
 *************************************/
-#include "fixed.h"
-#include "stdio.h"
-#include <math.h>
 
 #include "matrix_vs.h"
 
 /* Matrix multiply Full matrix by upper diagonal matrix; */
-void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
-    unsigned int i, j, k;
-    long tmp_sum=0L; //in 15+qAU
-    int *m2pom;
-    int *m1pom=m1;
-    int *respom=result;
+void mmultAU(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) {
+    unsigned int16 i, j, k;
+    int32 tmp_sum=0L; //in 15+qAU
+    int16 *m2pom;
+    int16 *m1pom=m1;
+    int16 *respom=result;
 
     for (i=0; i<rows; i++) //rows of result
     {
         for (j=0; j<columns; j++) //columns of result
-        {
+        {
             m2pom=up+j;//??
 
             for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
             {
-                tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU);
+                tmp_sum+=((int32)(*(m1pom++))**m2pom)>>(15-qAU);
                 m2pom+=columns;
             }
             // add the missing A(i,j)
-            tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift
+            tmp_sum +=(int32)(*m1pom)<<qAU; // no need to shift
             m1pom-=(j); // shift back to first element
 
-            // saturation effect
-/*            tmp_sum=tmp_sum>>15;
-            if (tmp_sum>32767) {
-                                printf("Au - saturated\n");
-            }
-            if (tmp_sum<-32768) {
-                                printf("Au - saturated\n");
-            }*/
-//                      printf("Au - saturated\n");
-
             *respom++=tmp_sum>>15;
 
@@ -57 +48 @@
 };
 
-/* Matrix multiply Full matrix by upper diagonal matrix; */
-void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
-    unsigned int i, j, k;
-    long tmp_sum=0L;
-    int *m2pom;
-    int *m1pom=m1;
-    int *respom=result;
-
-    for (i=0; i<rows; i++) //rows of result
-    {
-        for (j=0; j<columns; j++) //columns of result
-        {
-            m2pom=up+j;//??
-
-            for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1;
-            {
-                tmp_sum+=(long)(*(m1pom++))**m2pom;
-                m2pom+=columns;
-            }
-            m1pom-=(j+1); // shift back to first element
-
-            // saturation effect
-            tmp_sum=tmp_sum>>15;
-            if (tmp_sum>32767) {
-                if (i!=3) tmp_sum=32767;
-            }
-            if (tmp_sum<-32768) {
-                tmp_sum=-32768;
-            }
-            //                  printf("Au - saturated\n");
-
-            *respom++=tmp_sum;
-
-            tmp_sum=0;
-        }
-        m1pom+=(columns);
-    }
-};
-
-bool DBG=true;
-
-void show(const char name[10], int *I, int n) {
-    if (!DBG) return;
-
-    printf("%s: ",name);
-    for (int i=0;i<n;i++) {
-        printf("%d ",*(I+i));
-    }
-    printf("\n");
-}
-
-// Thorton procedure - Kalman predictive variance in UD
-void thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
-    unsigned int i,j,k;
-    // copy D to Dold
-    int *Dold_pom=Dold;
-    for (i=0;i<rows;i++) {
-        *Dold_pom++=*D++;
-    }
-    D-=rows; // back to first D
-
-    // initialize G = eye()
-    int *G_pom = G;
-    *G_pom++=1<<14;
-    for (i=0;i<rows-1;i++) {
-        // clean elem before diag
-        for (j=0; j<rows; j++) {
-            *G_pom++=0.0;
-        }
-        *G_pom++=1<<14;
-    }
-    // eye created
-
-    long sigma; // in q30!!!!!!
-    for (i=rows-1; true;i--) { // check i==0 at the END!
-        sigma = 0;
-
-        for (j=0;j<rows; j++) {
-            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
-            long s2=((((long)PSIU[i*rows+j]*Dold[j]))>>(2*qAU-15))*PSIU[i*rows+j];
-//                      printf("%d - %d\n",s1,s2);
-            sigma += s2;
-        }
-        sigma += Q[i*rows+i]<<15; // Q is in q15
-        for (j=i+1;j<rows; j++) {
-            sigma += (((long)G[i*rows+j]*G[i*rows+j])>>13)*Q[j*rows+j];
-//                      sigma += (((long)G[i+j*rows]*G[i+j*rows])>>13)*Q[j+j*rows];
-        }
-
-        if (sigma>16384<<15) sigma = 16384<<15;
-        *(D+i)=sigma>>15;
-        if (D[i]==0) D[i]=1;
-//show("D",D,5);
-
-        for (j=0;j<i;j++) {
-//                      printf("\n%d,%d\n",i,j);
-            sigma =0;
-            for (k=0;k<rows;k++) {
-                                int tmp= (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15));
-                                if (tmp>32767) printf("!");
-                                sigma += (((long(PSIU[i*rows+k])*Dold[k]))>>(2*qAU-15))*PSIU[j*rows+k];
-            }
-            for (k=0;k<rows;k++) {
-                sigma += ((((long)G[i*rows+k])*G[j*rows+k])>>13)*Q[k*rows+k];
-            }
-            long z=sigma/D[i]; // shift by 15
-            if (z>32767) z=32767;
-            if (z<-32768) z=-32768;
-
-            U[j*rows+i] = (int)z;
-
-
-            for (k=0;k<rows;k++) {
-                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15; //qAU*q15/q15=qAU
-            }
-
-            for (k=0;k<rows;k++) {
-                G[j*rows+k] -=  ((long)U[j*rows+i]*G[i*rows+k])>>15;
-            }
-
-        }
-        //show("U",U,25);
-        //show("G",G,25);
-        if (i==0) return;
-    }
-}
-
-void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
-    int alpha;
-    int beta,lambda;
-    int b[5]; // ok even for 4-dim state
-    int *a; // in [0,1] -> q15
-    unsigned int iy,j,i;
-
-    int *b_j,*b_i;
-    int *a_j;
-    int *D_j;
-    int *U_ij;
-    int *x_i;
-    int dzs;
+
+void bierman_fast(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx )
+{
+    int16 alpha;
+    int16 beta,lambda;
+    int16 b[5]; // ok even for 4-dim state
+    int16 *a; // in [0,1] -> q15
+    unsigned int16 iy,j,i;
+
+    int16 *b_j,*b_i;
+    int16 *a_j;
+    int16 *D_j;
+    int16 *U_ij;
+    int16 *x_i;
+
+    int32 z_pom;
+    int16 z_pom_int16;
 
     a = U; // iyth row of U
@@ -202 +70 @@
         // a is a row
         for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,b_j++,D_j++,a_j++)
-            *b_j=((long)(*D_j)*(*a_j))>>15;
-
-        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
+            *b_j=((int32)(*D_j)*(*a_j))>>15;
+
+        alpha = R[iy]; //\alpha = R+vDv = R+a*b
         // R in q15, a in q15, b=q15
 //              gamma = (1<<15)/alpha; //(in q15)
@@ -210 +78 @@
         //max gamma = 0.0061 => gamma_ref = q7
         for (j=0,a_j=a,b_j=b,D_j=D; j<dimx; j++,a_j++,b_j++,D_j++) {
-            beta   = alpha;
-            lambda = -((long)(*a_j)<<15)/beta;
-            alpha  += ((long)(*a_j)*(*b_j))>>15;
-            D[j] = ((((long)beta<<15)/alpha)*(*D_j))>>15; //gamma is long
+/*            beta=alpha;
+            lambda = -((int32)(*a_j)<<15)/beta;
+            alpha  += ((int32)(*a_j)*(*b_j))>>15;
+            D[j] = ((int32)beta**D_j)/alpha;*/
+/*xx*/
+            lambda=alpha;
+            alpha  += ((int32)(*a_j)*(*b_j))>>15;
+            D[j] = ((int32)lambda**D_j)/alpha;
+            z_pom_int16 = -((int32)(*a_j)<<15)/lambda;
+/*xx*/
+
             if (*D_j==0) *D_j=1;
 
             for (i=0,b_i=b,U_ij=U+j; i<j; i++, b_i++,U_ij+=dimx) {
                 beta   = *U_ij;
-                *U_ij +=  ((long)lambda*(*b_i))>>15;
-                *b_i  +=  ((long)beta*(*b_j))>>15;
-            }
-        }
-        dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
+//                *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;
+            }
+        }
         // no shift due to gamma
         for (i=0,x_i=xp,b_i=b; i<dimx; i++,x_i++,b_i++) {
-            *x_i  += ((long)dzs*(*b_i))>>15; // multiply by unscaled Kalman gain
-        }
-
-        //cout << "Ub: " << U << endl;
-        //cout << "Db: " << D << endl <<endl;
-
-    }
-
-}
+            *x_i  += ((int32)difz[iy]*(*b_i))/alpha; // multiply by unscaled Kalman gain
+        }
+    }
+}
+
 
 // Thorton procedure - Kalman predictive variance in UD
-void thorton_fast(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int rows) {
-    unsigned int i,j,k;
+void thorton_fast(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 rows) {
+    unsigned int16 i,j,k;
     // copy D to Dold
-    int *Dold_i,*Dold_k;
-    int *D_i;
-    int *PSIU_ij,*PSIU_ik,*PSIU_jk;
-    int *Q_jj,*Q_ii,*Q_kk;
-    int *U_ji;
-    int *G_ik,*G_jk;
-    int irows,jrows;
-    long sigma; // in qAU+15!!
-    long z;
+    int16 *Dold_i,*Dold_k;
+    int16 *D_i;
+    int16 *PSIU_ij,*PSIU_ik,*PSIU_jk;
+    int16 *Q_jj,*Q_ii,*Q_kk;
+    int16 *U_ji;
+    int16 *G_ik,*G_jk;
+    int16 irows,jrows;
+    int32 sigma; // in qAU+15!!
+    int32 z;
 
     for (i=0,Dold_i=Dold,D_i=D;i<rows;i++,Dold_i++,D_i++) {
@@ -271 +150 @@
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
                 k<rows; k++, PSIU_ik++,Dold_k++) {
-                        sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
-        }
-        sigma += *(Q+i+irows)<<qAU;
+                        sigma += (((int32)(*PSIU_ik)**PSIU_ik)>>(qAU))*(*Dold_k);
+        }
+        sigma += (int32)(*(Q+i+irows))<<qAU;
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
-            sigma += (((long)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
-
-        }
-
-                if (sigma>((long)1<<(qAU+15))) {
+            sigma += (((int32)(*G_ik)**G_ik)>>16)**(Q+j+j*rows);
+        }
+
+                if (sigma>((int32)1<<(qAU+15))) {
                         *D_i = 32767;
 //                      *(Dold+i)-=*(Q+i+irows);
@@ -294 +172 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                                sigma += ((((long)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
+                                sigma += ((((int32)(*PSIU_ik)**PSIU_jk)>>qAU)**Dold_k);
             }
 
             for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
                     k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
-                sigma += ((((long)*G_ik)**G_jk)>>16)**Q_kk;
+                sigma += ((((int32)*G_ik)**G_jk)>>16)**Q_kk;
             }
 
@@ -307 +185 @@
 
             U_ji=U+jrows+i;
-            *U_ji = (int)z;
+            *U_ji = (int16)z;
 
 
             for (k=0,PSIU_ik=PSIU+irows,PSIU_jk=PSIU+jrows;
                     k<rows;k++,PSIU_ik++,PSIU_jk++) {
-                *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
+                *PSIU_jk -= ((int32)*U_ji**PSIU_ik)>>15;
             }
 
             for (k=0,G_jk=G+jrows,G_ik=G+irows;
                     k<rows;k++, G_jk++, G_ik++) {
-                *G_jk -=  ((long)*U_ji**G_ik)>>15;
+                *G_jk -=  ((int32)*U_ji**G_ik)>>15;
             }
 
@@ -323 +201 @@
                 if (i==0) return;
     }
-}
-
-void bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx ) {
-    long alpha;
-    long gamma,beta,lambda;
-    int b[5]; // ok even for 4-dim state
-    int *a; // in [0,1] -> q15
-    unsigned int iy,j,i;
-
-    for (iy=0; iy<dimy; iy++) {
-        // a is a row
-        a = U+iy*dimx; // iyth row of U
-        for (j=0;j<dimx;j++) {
-            (j<iy)? b[j]=0: (j==iy)? b[j]=D[j] : b[j]=((long)D[j]*a[j])>>15;
-        }
-
-        alpha = (long)R[iy]; //\alpha = R+vDv = R+a*b
-        // R in q15, a in q15, b=q15
-//              gamma = (1<<15)/alpha; //(in q15)
-        //min alpha = R[iy] = 164
-        //max gamma = 0.0061 => gamma_ref = q7
-        for (j=0;j<dimx;j++) {
-            beta   = alpha;
-            lambda = -(long(a[j])<<15)/beta;
-            alpha  += ((long(a[j])*b[j])>>15);
-            D[j] = (((long(beta)<<15)/alpha)*D[j])>>15; //gamma is long
-            if (D[j]==0) D[j]=1;
-
-            //                  cout << "a: " << alpha << "g: " << gamma << endl;
-            for (i=0;i<j;i++) {
-                beta   = U[i*dimx+j];
-                U[i*dimx+j] +=  (lambda*b[i])>>15;
-                b[i]   +=  (beta*b[j])>>15;
-            }
-        }
-        int dzs = (long(difz[iy])<<15)/alpha;  // apply scaling to innovations
-        // no shift due to gamma
-        for (i=0; i<dimx; i++) {
-            xp[i]  += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain
-        }
-
-        //cout << "Ub: " << U << endl;
-        //cout << "Db: " << D << endl <<endl;
-
-    }
-
 }