Changeset 1233 for applications/pmsm

Timestamp:

10/26/10 14:28:53 (14 years ago)

Author:

smidl

Message:

fix shifts in thorton fast

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -42 +42 @@
             tmp_sum=tmp_sum>>15;
             if (tmp_sum>32767) {
-                //tmp_sum=32767;
+                                printf("Au - saturated\n");
             }
             if (tmp_sum<-32768) {
-                //tmp_sum=-32768;
+                                printf("Au - saturated\n");
             }
 //                      printf("Au - saturated\n");
@@ -155 +155 @@
             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];
             }
@@ -265 +267 @@
         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)**Dold_k)>>(2*qAU-15))*(*PSIU_ik);
+                k<rows; k++, PSIU_ik++,Dold_k++) {
+                sigma += (((long)(*PSIU_ik)**Dold_k)>>(qAU))*(*PSIU_ik)<<(15-qAU);
         }
         sigma += *(Q+i+irows)<<15;
@@ -286 +288 @@
                     k<rows; k++, PSIU_ik++, PSIU_jk++, Dold_k++) {
 
-                                sigma += ((((long)*PSIU_ik)**Dold_k)>>(2*qAU-15))**PSIU_jk;
-            }
-
-            for (k=i,G_ik=G+irows+i,G_jk=G+jrows+i,Q_kk=Q+k*rows+k;
+                                sigma += (((long(*PSIU_ik)**Dold_k)>>qAU)**PSIU_jk)<<(15-qAU);
+            }
+
+            for (k=i+1,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)>>13)**Q_kk;
@@ -338 +340 @@
         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;
 
@@ -350 +352 @@
             }
         }
-        int dzs = (((long)difz[iy])<<15)/alpha;  // apply scaling to innovations
+        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
+            xp[i]  += (long(dzs*b[i]))>>15; // multiply by unscaled Kalman gain
         }