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

Timestamp:

09/10/10 11:11:24 (14 years ago)

Author:

smidl

Message:

bug in sigma += Q(i) found

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -46 +46 @@
 };
 
-void UDprt(int *U, int *D) {
-    return;
-    for (int i=0;i<5;i++) {
-        for (int j=0;j<5;j++) {
-            printf("%d,",U[i*5+j]);
-        }
-        printf("\n");
-    }
-    for (int i=0;i<5;i++) {
-        printf("%d,",D[i]);
-    }
+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");
 }
@@ -82 +80 @@
     // eye created
 
-    long sigma; // in q15
-    for (i=rows-1; i>=0;i--) { // check i==0 at the END!
+    long sigma; // in q30!!!!!!
+    for (i=rows-1; true;i--) { // check i==0 at the END!
         sigma = 0;
                 
         for (j=0;j<rows; j++) {
-            sigma += (((long)PSIU[i*rows+j]*PSIU[i*rows+j])>>15)*(Dold[i]);
+                        //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]);
+//                      printf("%d - %d\n",s1,s2);
+                        sigma += s2;
                 }
-                sigma += Q[i*rows+i];
+                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];
                 }
                 
         *(D+i)=sigma>>15;
         if (D[i]==0) D[i]=1;
-
-        /*              printf("d=sig\n");
-                        UDprt(U,D);
-                        UDprt(G,Dold);*/
+//show("D",D,5);
 
         for (j=0;j<i;j++) {
@@ -116 +115 @@
             U[j*rows+i] = (int)z;
 
-            /*                  printf("U=sig/D\n");
-                                UDprt(U,D);
-                                UDprt(G,Dold);*/
 
             for (k=0;k<rows;k++) {
@@ -128 +124 @@
             }
                         
-            /*                  printf("end\n");
-                                UDprt(U,D);
-                                UDprt(G,Dold);
-                                printf("\n");   */
-        }
-        if (i==0) return;
+        }
+        //show("U",U,25);
+                //show("G",G,25);
+                if (i==0) return;
     }
 }