Changeset 1228 for applications/pmsm

Timestamp:

10/22/10 22:06:23 (14 years ago)

Author:

smidl

Message:

UD hopefully correct

Location:

applications/pmsm/simulator_zdenek

Files:

• ekf_example/matrix_vs.cpp (modified) (7 diffs)
• ekf_example/matrix_vs.h (modified) (1 diff)
• test_UD.cpp (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -19 +19 @@
 void mmultAU(int *m1, int *up, int *result, unsigned int rows, unsigned int columns) {
     unsigned int i, j, k;
-    long tmp_sum=0L;
+    long tmp_sum=0L; //in 15+qAU
     int *m2pom;
     int *m1pom=m1;
@@ -32 +32 @@
             for (k=0; k<j; k++) //inner loop up to "j" - U(j,j)==1;
             {
-                tmp_sum+=(long)(*(m1pom++))**m2pom;
+                tmp_sum+=((long)(*(m1pom++))**m2pom)>>(15-qAU);
                 m2pom+=columns;
             }
             // add the missing A(i,j)
-            tmp_sum +=(long)(*m1pom)<<15; // no need to shift
+            tmp_sum +=(long)(*m1pom)<<qAU; // no need to shift
             m1pom-=(j); // shift back to first element
 
             // saturation effect
-            tmp_sum=tmp_sum>>(30-qAU);
+            tmp_sum=tmp_sum>>15;
             if (tmp_sum>32767) {
                 //tmp_sum=32767;
@@ -136 +136 @@
         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])>>qAU)*PSIU[i*rows+j])<<(15-qAU);
+            long s2=((((long)PSIU[i*rows+j]*PSIU[i*rows+j]))>>(2*qAU-15))*Dold[j];
 //                      printf("%d - %d\n",s1,s2);
             sigma += s2;
@@ -146 +146 @@
         }
 
-        //if (sigma>16384<<15) sigma = 16384<<15;
+//        if (sigma>16384<<15) sigma = 16384<<15;
         *(D+i)=sigma>>15;
         if (D[i]==0) D[i]=1;
@@ -155 +155 @@
             sigma =0;
             for (k=0;k<rows;k++) {
-                sigma += (((long(PSIU[i*rows+k])*Dold[k])>>qAU)*PSIU[j*rows+k])<<(15-qAU);
+                                sigma += (((long(PSIU[i*rows+k])*PSIU[j*rows+k]))>>(2*qAU-15))*Dold[k];
             }
             for (k=0;k<rows;k++) {
@@ -266 +266 @@
         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)>>(qAU*qAU-15))*(*Dold_k);
+            sigma += (((long)(*PSIU_ik)**PSIU_ik)>>15)*(*Dold_k);
         }
         sigma += *(Q+i+irows)<<15;
@@ -285 +285 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>(qAU*qAU-15))**Dold_k;
+                sigma += ((((long)*PSIU_ik)**PSIU_jk)>>15)**Dold_k;
             }
 

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -11 +11 @@
 *************************************/
 
-#define qAU 15
+#define qAU 14
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */

applications/pmsm/simulator_zdenek/test_UD.cpp

@@ -50 +50 @@
         
         imat PUcmp(PSIU,5,5);
-        cout << "Delta PSI: " << round_i(PhiU*multip-2*PUcmp) <<endl;
+        cout << "Delta PSI: " << round_i(PhiU*multip-(1<<(15-qAU))*PUcmp) <<endl;
         
-        mat_to_int(round_i(PhiU*multip/2),PSIU); //<< make is same
+        mat_to_int(round_i(PhiU*multip/(1<<(15-qAU))),PSIU); //<< make is same
 
 /////////// Test Thorton: