Changeset 135 for pmsm/sim_var.cpp

Timestamp:

07/25/08 15:04:05 (16 years ago)

Author:

smidl

Message:

oprava simulatoru

Files:

• pmsm/sim_var.cpp (modified) (5 diffs)

pmsm/sim_var.cpp

@@ -40 +40 @@
         vec dt ( 2 );
         vec ut ( 2 );
-        vec dut ( 4 );
+        vec dut ( 2 );
         vec dit (2);
         vec xtm=zeros ( 4 );
+        vec xte=zeros ( 4 );
         vec xdif=zeros ( 4 );
         vec xt ( 4 );
@@ -95 +96 @@
                 //Number of steps of a simulator for one step of Kalman
                 for ( int ii=0; ii<Nsimstep;ii++ ) {
-                        sim_profile_steps1 ( Ww , true);
+                        sim_profile_steps1 ( Ww , false);
                         pmsmsim_step ( Ww );
                 };
@@ -105 +106 @@
                 dut ( 0 ) = KalmanObs[4];
                 dut ( 1 ) = KalmanObs[5];
-                dut ( 2 ) = KalmanObs[6];
-                dut ( 3 ) = KalmanObs[7];
                 dit ( 0 ) = KalmanObs[8];
                 dit ( 1 ) = KalmanObs[9];
 
-                xt = fxu.eval ( xtm,ut );
+                xte = fxu.eval ( xtm,ut );
                 //Results:
-                // in xt we have simulaton according to the model
+                // in xt we have simulation according to the model
                 // in x we have "reality"
-                xtm ( 0 ) =x[0];xtm ( 1 ) =x[1];xtm ( 2 ) =x[2];xtm ( 3 ) =x[3];
-                xdif = xtm-xt;
+                xt ( 0 ) =x[0];xt ( 1 ) =x[1];xt ( 2 ) =x[2];xt ( 3 ) =x[3];
+                xdif = xt-xte; //xtm is a copy of x[]
                 if (xdif(0)>3.0){
                         cout << "here" <<endl;
@@ -122 +121 @@
                 if ( xdif ( 3 ) <-pi ) xdif ( 3 ) +=2*pi;
                 
-                ddif = hxu.eval(xtm,ut) - dt;
+                ddif = hxu.eval(xt,ut) - dit;
 
                 //Rt = 0.9*Rt + xdif^2
-                Qt*=0.01;
-                Qt += outer_product ( xdif,xdif ); //(x-xt)^2
-                Rt*=0.01;
-                Rt += outer_product ( ddif,ddif ); //(x-xt)^2
+                Qt*=0.1;
+                Qt += 10*outer_product ( xdif,xdif ); //(x-xt)^2
+
+                if (Qt(0,0)>3.0){
+                        cout << "here" <<endl;
+                        }
+                // For future ref.
+                xtm = xt;
+
+                Rt*=0.1;
+//              Rt += 10*outer_product ( ddif,ddif ); //(x-xt)^2
 
                 //ESTIMATE
                 Efix.bayes(concat(dt,ut));
                 //
-                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-16*eye(4)),(Rt+1e-3*eye(2)));
+                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-8*eye(4)),(Rt+1e-6*eye(2)));
                 Eop.bayes(concat(dt,ut));
                 //
@@ -154 +160 @@
 
         L.step(true);
-//      L.itsave("sim_var.it"); 
+        //L.itsave("sim_var.it");