Changeset 63 for tests/pmsm_sim3.cpp

Timestamp:

04/09/08 15:29:53 (16 years ago)

Author:

smidl

Message:

odstraneni net a drobne upravy

Files:

• tests/pmsm_sim3.cpp (modified) (8 diffs)

tests/pmsm_sim3.cpp

@@ -27 +27 @@
 void set_simulator_t(double &Ww) {
 
-        if (t>0.2) x[8]=1.2;    // 1A //0.2ZP
+        if (t>0.0002) x[8]=1.2;    // 1A //0.2ZP
         if (t>0.4) x[8]=10.8;   // 9A
         if (t>0.6) x[8]=25.2;  // 21A
@@ -73 +73 @@
         if (t>8.5) x[8]=25.2;  // 21A
         
-//        x[8]=0.0;
+        x[8]=0.0;
 }
 
@@ -81 +81 @@
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 500;
+        int Npart = 100;
         
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        vec Qdiag ( "0.01 0.01 0.0001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+        vec Qdiag ( "0.05 0.05 0.002 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
         vec Rdiag ( "0.05 0.05" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
@@ -99 +99 @@
         MPF<EKFfixed> M ( rx,rQ,evolQ,evolQ,Npart,KFE );
         // initialize
-        evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
+        evolQ.set_parameters ( 10.0 ,Qdiag, 1.0); //sigma = 1/10 mu
         evolQ.condition ( Qdiag ); //Zdenek default
         epdf& pfinit=evolQ._epdf();
         M.set_est ( pfinit );
-        evolQ.set_parameters ( 100000.0, Qdiag, 0.99999 );
+        evolQ.set_parameters ( 500000.0, Qdiag, 0.9999 );
 
 
@@ -111 +111 @@
 
         mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-        mat Dt=zeros ( Ndat,4+2 ); //observation
+        mat Dt=zeros ( Ndat,4 ); //observation
         mat XtE=zeros ( Ndat, 4 );
         mat XtM=zeros ( Ndat,4+4 ); //Q + x
@@ -118 +118 @@
         pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
         double Ww=0.0;
+        static int k_rampa=1;
+        static long k_rampa_tmp=0;
+
         vec dt ( 2 );
         vec ut ( 2 );
@@ -125 +128 @@
                 for ( int ii=0; ii<Nsimstep;ii++ ) {
                         //simulator
-                        set_simulator_t(Ww);
+                        Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
+                        if ( Ww>2.*M_PI*150. ) {
+                                Ww=2.*M_PI*150.;
+                                if ( k_rampa_tmp<500000 ) k_rampa_tmp++;
+                                else {k_rampa=-1;k_rampa_tmp=0;}
+                        };
+                        if ( Ww<-2.*M_PI*150. ) Ww=-2.*M_PI*150.; /* */
+//                      set_simulator_t(Ww);
                         pmsmsim_step ( Ww );
                 };
@@ -138 +148 @@
                 
                 Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
-                Dt.set_row ( tK, concat ( dt,ut,vec_1(sqrt(pow(ut(0),2)+pow(ut(1),2))), vec_1(sqrt(pow(dt(0),2)+pow(dt(1),2))) ) );
+                Dt.set_row ( tK, concat ( dt,ut));
                 XtE.set_row ( tK,KFEep.mean() );
+                double qNaN = std::numeric_limits<double>::quiet_NaN();
                 XtM.set_row ( tK,Mep.mean() );
         }