Changeset 1225 for applications/pmsm/simulator_zdenek

Timestamp:

10/22/10 21:14:59 (14 years ago)

Author:

smidl

Message:

broken UD + Chol

Location:

applications/pmsm/simulator_zdenek

Files:

• ekf_example/matrix_vs.cpp (modified) (13 diffs)
• ekf_example/matrix_vs.h (modified) (2 diffs)
• test_UD.cpp (modified) (2 diffs)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -12 +12 @@
 #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) {
@@ -17 +21 @@
     long tmp_sum=0L;
     int *m2pom;
+    int *m1pom=m1;
+    int *respom=result;
 
     for (i=0; i<rows; i++) //rows of result
@@ -26 +32 @@
             for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
             {
-                tmp_sum+=(long)(*m1++)**m2pom;
+                tmp_sum+=(long)(*(m1pom++))**m2pom;
                 m2pom+=columns;
             }
             // add the missing A(i,j)
-            tmp_sum +=(long)(*m1)<<15; // no need to shift
-            m1-=(j); // shift back to first element
+            tmp_sum +=(long)(*m1pom)<<15; // no need to shift
+            m1pom-=(j); // shift back to first element
+
+            // saturation effect
+            tmp_sum=tmp_sum>>(30-qAU);
+            if (tmp_sum>32767) {
+                //tmp_sum=32767;
+            }
+            if (tmp_sum<-32768) {
+                //tmp_sum=-32768;
+            }
+//                      printf("Au - saturated\n");
+
+            *respom++=tmp_sum;
+
+            tmp_sum=0;
+        }
+        m1pom+=(columns);
+    }
+};
+
+/* 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) tmp_sum=32767;
-            if (tmp_sum<-32768) tmp_sum=-32768;
-
-            *result++=tmp_sum;
+            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;
         }
-        m1+=(columns);
+        m1pom+=(columns);
     }
 };
@@ -49 +99 @@
 
 void show(const char name[10], int *I, int n) {
-        if (!DBG) return;
-        
-        printf("%s: ",name);
+    if (!DBG) return;
+
+    printf("%s: ",name);
     for (int i=0;i<n;i++) {
-                printf("%d ",*(I+i));
-        }
+        printf("%d ",*(I+i));
+    }
     printf("\n");
 }
@@ -83 +133 @@
     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]*PSIU[i*rows+j])>>15)*(Dold[j]);
+            //long s1=(((long)PSIU[i+j*rows]*PSIU[i+j*rows])>>15)*(Dold[i]);
+            long s2=((((long)PSIU[i*rows+j]*Dold[j])>>qAU)*PSIU[i*rows+j])<<(15-qAU);
 //                      printf("%d - %d\n",s1,s2);
-                        sigma += s2;
-                }
-                sigma += Q[i*rows+i]<<15;
+            sigma += s2;
+        }
+        sigma += Q[i*rows+i]<<15;
         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;
@@ -104 +155 @@
             sigma =0;
             for (k=0;k<rows;k++) {
-                sigma += ((((long)PSIU[i*rows+k])*PSIU[j*rows+k])>>15)*Dold[k];
-                   }
-                        for (k=0;k<rows;k++) {
+                sigma += (((long(PSIU[i*rows+k])*Dold[k])>>qAU)*PSIU[j*rows+k])<<(15-qAU);
+            }
+            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
+            long z=sigma/D[i]; // shift by 15
             if (z>32767) z=32767;
             if (z<-32768) z=-32768;
@@ -117 +168 @@
 
             for (k=0;k<rows;k++) {
-                PSIU[j*rows+k] -= ((long)U[j*rows+i]*PSIU[i*rows+k])>>15;
-            }
-                        
+                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;
+        //show("G",G,25);
+        if (i==0) return;
     }
 }
@@ -214 +265 @@
         sigma = 0;
         for (k=0, PSIU_ik=PSIU+irows,Dold_k=Dold;
-           k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i=
-            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k);
-                }
-                sigma += *(Q+i+irows)<<15;
+                k<rows; k++, PSIU_ik++,Dold_k++) {//Dold_i=
+            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>(qAU*qAU-15))*(*Dold_k);
+        }
+        sigma += *(Q+i+irows)<<15;
         for (j=i+1, G_ik=G+irows+i+1; j<rows; j++,G_ik++) {
             sigma += (((long)(*G_ik)**G_ik)>>13)**(Q+j+j*rows);
-                        
-        }
-                
+
+        }
+
         *D_i=sigma>>15;
         if (*D_i==0) *D_i=1;
@@ -233 +284 @@
             for (k=0, PSIU_ik=PSIU+irows, PSIU_jk=PSIU+jrows, Dold_k=Dold;
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
-                                
-                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
-            }
-                        
+
+                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>(qAU*qAU-15))**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) {
+                    k<rows;k++,G_ik++,G_jk++,Q_kk+=rows+1) {
                 sigma += ((((long)*G_ik)**G_jk)>>13)**Q_kk;
             }
-                        
-                        long z=sigma/(*D_i); // shift by 15
+
+            long z=sigma/(*D_i); // shift by 15
             if (z>32767) z=32767;
             if (z<-32768) z=-32768;
@@ -254 +305 @@
                 *PSIU_jk -= ((long)*U_ji**PSIU_ik)>>15;
             }
-                        
+
             for (k=0,G_jk=G+jrows,G_ik=G+irows;
-            k<rows;k++, G_jk++, G_ik++) {
+                    k<rows;k++, G_jk++, G_ik++) {
                 *G_jk -=  ((long)*U_ji**G_ik)>>15;
             }
-                        
+
         }
         if (i==0) return;
@@ -275 +326 @@
         // a is a row
         a = U+iy*dimx; // iyth row of U
-        for (j=0;j<dimx;j++)
-            b[j]=((long)D[j]*a[j])>>15;
+        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
@@ -285 +337 @@
         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
+            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;
 
@@ -309 +361 @@
 
 }
+
+/* square root of 0<a<1 using taylor at 0.5 in q15*/
+int int_sqrt(int x) {
+    //sqrt(x) == 1/2*2^(1/2)+1/2*2^(1/2)*(x-1/2)-1/4*2^(1/2)*(x-1/2)^2
+    //         = k1 + k1*(x-0.5) - k2*(x-0.5)(x-0.5);
+#define k1 23170 //0.5*sqrt(2)*32768
+#define k2 11585 //0.25*sqrt(2)*32768
+
+    int tmp;
+    if (x>6554) {
+        int xm05=x-16384;
+        tmp = ((long)k1*xm05)>>15;
+        tmp-=(((long(k2)*xm05)>>15)*xm05)>>15;
+        tmp +=k1;
+    } else {
+        tmp = 4*x;
+        tmp-=long(8*x)*x>>15;
+    }
+    return tmp;
+}
+
+void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx) {
+    int k,j,i;
+    int sigma,alpha,beta;
+    int B[25];//beware
+    int w[5];
+    int v[5];
+
+    // copy Q to B
+    for (i=0;i<dimx*dimx;i++) {
+        B[i]=Q[i];
+    }
+
+    for (k=dimx-1; k>=0; k--) {
+        sigma=0;
+        for (j=0;j<dimx;j++) {
+            sigma+=(long(B[k*dimx+j])*B[k*dimx+j])>>15;
+        }
+        for (j=0;j<=k;j++) {
+            sigma+=(long(Ch[k*dimx+j])*Ch[k*dimx+j])>>15;
+        }
+        /*               double sigf=double(sigma)/(1<<15);
+                         double alpf = sqrt(sigf);*/
+        alpha=int_sqrt(sigma);
+        // alpha = alpf*(1<<15);
+        //
+        sigma=0;
+        for (j=0;j<dimx;j++) {
+            w[j]=B[k*dimx+j];
+            sigma+=(long(w[j])*w[j])>>15;
+        }
+        for (j=0; j<=k;j++) {
+            if (j==k) {
+                v[j]=Ch[k*dimx+j]-alpha;
+            } else {
+                v[j]=Ch[k*dimx+j];
+            }
+            sigma+=(long(v[j])*v[j])>>15;
+        }
+        alpha=sigma>>1;
+        for (i=0;i<=k;i++) {
+            sigma=0;
+            for (j=0;j<dimx;j++) {
+                sigma+=(long(B[i*dimx+j])*w[j])>>15;
+            }
+            for (j=0;j<=k;j++) {
+                sigma+=(long(Ch[i*dimx+j])*v[j])>>15;
+            }
+            for (j=0;j<dimx;j++) {
+                B[i*dimx+j]-=(long(sigma)*w[j]/alpha);
+            };
+            for (j=0;j<=k;j++) {
+                Ch[i*dimx+j]-=(long(sigma)*v[j]/alpha);
+            }
+        }
+    }
+
+}
+
+void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx ) {
+    int alpha,beta,gamma;
+    int delta, eta,epsilon,zeta,sigma,tau;
+    int i,j,iy;
+    int w[5];
+
+    for (iy=0; iy<dimy; iy++) {
+        alpha=R[iy];
+        delta = difz[iy];
+
+        for (j=0;j<dimx;j++) {
+            sigma=Ch[iy*dimx+j];
+            beta=alpha;
+            alpha+=(long(sigma)*sigma)>>15;
+//                      double ab=(double)alpha*beta/32768./32768.;
+//                      double s_ab=sqrt(ab);
+            gamma=int_sqrt(((long)alpha*beta)>>15);
+            //gamma = round(s_ab*(1<<15));
+            eta=(long (beta)<<15) / gamma;
+            //zeta=(long(sigma)<<15)/ gamma;
+            w[j]=0;
+            for (i=0;i<=j;i++) {
+                tau=Ch[i*dimx+j];
+                Ch[i*dimx+j]=((long(eta)*Ch[i*dimx+j])>>15) -(long(sigma)*w[i])/gamma;
+                w[i]+=(long(tau)*sigma)>>15;
+            }
+        }
+
+        //epsilon=(long(difz)<<15) / (alpha); // q15*q13/q13 = q15
+        for (i=0;i<dimx;i++) {
+            xp[i]+=(long(w[i])*delta)/alpha;
+        }
+    }
+}

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -10 +10 @@
 
 *************************************/
+
+#define qAU 15
+
+/* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */
+extern void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns);
+
 /* Matrix multiply Full matrix by upper diagonal matrix; */
-extern void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns);
+extern void mmultACh(int *m1, int *up, int *result, unsigned int rows, unsigned int columns);
 
 /* perform Thorton update of UD matrix using PSI*U, Q, and temporaries G, Dold, for size dimx*/
@@ -24 +30 @@
 /* perform Bierman update of UD matrix using difz, R and xp, for size dimx*/
 extern void bierman_fast(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
+
+/* perform Householder update of Ch matrix using PSI*Ch , Q, */
+extern void householder(int *Ch /*= int *PSICh*/, int *Q, unsigned int dimx);
+
+/* perform Carlson update of Ch matrix using difz, R and xp, for size dimx*/
+extern void carlson(int *difz, int *xp, int *Ch, int *R, unsigned int dimy, unsigned int dimx );

applications/pmsm/simulator_zdenek/test_UD.cpp

@@ -50 +50 @@
         
         imat PUcmp(PSIU,5,5);
-        cout << "Delta PSI: " << round_i(PhiU*multip-PUcmp) <<endl;
+        cout << "Delta PSI: " << round_i(PhiU*multip-2*PUcmp) <<endl;
         
-        mat_to_int(round_i(PhiU*multip),PSIU); //<< make is same
+        mat_to_int(round_i(PhiU*multip/2),PSIU); //<< make is same
 
 /////////// Test Thorton:
@@ -85 +85 @@
         
         
-        thorton_fast(Uf,Df,PSIU,Qf,Gf,Dfold,5);
+        //thorton_fast(Uf,Df,PSIU,Qf,Gf,Dfold,5);
+        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
 
         /////// disp