Changeset 1182 for applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.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/ekf_obj.cpp (modified) (9 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;*/