Changeset 1182

Timestamp:

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

Author:

smidl

Message:

bug in sigma += Q(i) found

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (9 diffs)
• matrix_vs.cpp (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -228 +228 @@
         const vec &u=cond;
         int dim = dimension();
-        
+        ///// !!!!!!!!!!!!!!!!
         U = est._R()._L().T();
         D =  est._R()._D();
-        
+        if (false){
+                mat L;
+                ivec tmp;
+                mat X=randn(5,5);
+                mat XX=X*X.T();
+                mat T=diag(sqrt(1.0/diag(XX)));
+                XX= T*XX*T;
+                
+                ldmat ldd(XX);
+                U = ldd._L().T();
+                D = ldd._D()*0.9;
+        }
+                
         ////////////
         
@@ -246 +258 @@
         xref(3) = 3.141593;
         xref(4) = 34.0;
+        
+        if (false){
+                xref = ones(5);
+        }
         
         imat Utf;
@@ -255 +271 @@
         mat_to_int(Utf,Uf);             
         vec_to_int(Dtf, Df); 
+
+//      cout << Dtf << endl;
         
         // A*U == PSI*U
@@ -285 +303 @@
                         sigma2 += G(i,j)*G(i,j) * Q(j,j);
                 }*/
-                
-/*              UDtof(U,D,Utf,Dtf,xref);
+                D(i) = sigma; 
+                
+        /*      UDtof(U,D,Utf,Dtf,xref);
                 cout << "d=sig"<<endl;
-                cout << Utf << endl << Dtf << endl;
-                cout << G << endl << Din << endl<<endl;*/
-                
-                D(i) = sigma; 
+                cout << Dtf << endl;
+        */      
                 for (j=0;j<i;j++){ 
 //                      cout << i << "," << j << endl;
@@ -316 +333 @@
                         }
                 
-/*                      cout << "end"<<endl;
-                        UDtof(U,D,Utf,Dtf,xref);
-                        cout << G << endl << Din << endl;
-                        cout << Utf << endl << Dtf << endl<<endl;*/
                 }
+                UDtof(U,D,Utf,Dtf,xref);
+                //      cout << G << endl << Din << endl;
+                imat Urw=Utf.T();
+                cout << ivec(Urw._data(),25) << endl << Dtf << endl;
         }
 
@@ -328 +345 @@
         mat_to_int(Qrnd,Qf);
 
+//      cout << Df <<endl;
         thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
         
-        cout << "bierman double I" <<endl;
+/*      cout << "bierman double I" <<endl;
         UDtof(U,D,Utf,Dtf,xref);
         cout << "Uf: " << Utf << endl;
         cout << "Df: " << Dtf << endl;
-        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
+        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;*/
         mat_to_int(Utf,Uf);             
         vec_to_int(Dtf, Df); 
@@ -374 +392 @@
         }
 
-UDtof(U,D,Utf,Dtf,xref);
-cout << "Uf: " << Utf << endl;
-cout << "Df: " << Dtf << endl;
-cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
+// UDtof(U,D,Utf,Dtf,xref);
+// cout << "Uf: " << Utf << endl;
+// cout << "Df: " << Dtf << endl;
+// cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
 
         int difz[2];
@@ -387 +405 @@
         //beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx 
         
-        cout <<endl<< "bierman fixed" <<endl;
-        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
-        for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
-        for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-
-        for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
+//      cout <<endl<< "bierman fixed" <<endl;
+//      for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+//      for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+//      for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+// 
+//      for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
 
         int xf_old[5];
@@ -399 +417 @@
         
         UDtof(U,D,Utf,Dtf,xref);
-        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+/*      for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
         for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
         for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
-        cout << endl;
+        cout << endl;*/
         
         

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;
     }
 }