Changeset 1183 for applications/pmsm/simulator_zdenek

Timestamp:

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

Author:

smidl

Message:

Test passed for both bierman and thorton

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (7 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -231 +231 @@
         U = est._R()._L().T();
         D =  est._R()._D();
-        if (false){
+        
+        bool DBG=true;
+        if (DBG){
                 mat L;
                 ivec tmp;
@@ -259 +261 @@
         xref(4) = 34.0;
         
-        if (false){
+        if (DBG){
                 xref = ones(5);
         }
@@ -348 +350 @@
         thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
         
-/*      cout << "bierman double I" <<endl;
+        if (DBG){
+                _mu = vec("0.500 0.200 0.300 0.400 0.1");
+        }
+        
+        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); 
@@ -363 +369 @@
         vec yp = phxu->eval(_mu,u);
         vec xp=_mu; // used in bierman
+        
+        if (DBG){
+                yp = vec("0.40 0.300");
+        }
         
         for (int iy=0; iy<dimy; iy++){
@@ -392 +402 @@
         }
 
-// 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];
@@ -401 +411 @@
         int xf[5];
         vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf);
-        int Rf[2]={1,1};
-        //vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
+        int Rf[2];//={1,1};
+        vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
         //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;
+        cout << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+
+        cout << "dz: "; for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
 
         int xf_old[5];
@@ -417 +427 @@
         
         UDtof(U,D,Utf,Dtf,xref);
-/*      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 << "Uf: "; for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+        cout << "Df: "; for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+        cout << "xf: "; for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+        cout << endl;