Changeset 135

Timestamp:

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

Author:

smidl

Message:

oprava simulatoru

Files:

• bdm/estim/arx.h (modified) (1 diff)
• matlab/mex/linefit2.cpp (modified) (2 diffs)
• pmsm/sim_var.cpp (modified) (5 diffs)
• pmsm/sim_var_arx.cpp (modified) (1 diff)
• pmsm/simulator_zdenek/simulator.cpp (modified) (6 diffs)

bdm/estim/arx.h

@@ -51 +51 @@
         //! Full constructor
         ARX (RV &rv, mat &V0, double &nu0, double frg0=1.0) : BM(rv),est(rv,V0,nu0), V(est._V()), nu(est._nu()), frg(frg0){last_lognc=est.lognc();tll=0.0;};
-        void set_parameters(mat &V0, double &nu0){est._V()=V0;est._nu()=nu0;last_lognc=est.lognc();tll=0.0;}
+        void set_parameters(mat &V0, double &nu0){est._V()=V0;est._nu()=nu0;last_lognc=est.lognc();tll=last_lognc;}
         void get_parameters(mat &V0, double &nu0){V0=est._V().to_mat(); nu0=est._nu();}
         //! Here \f$dt = [y_t psi_t] \f$.

matlab/mex/linefit2.cpp

@@ -12 +12 @@
         // Convert input variables to IT++ format
         mat Data = mxArray2mat ( input[0] );
-        if (Data.rows()!=2)
-                { if (Data.cols()==2) Data=Data.T(); else mexErrMsgTxt("Data are not 2D!");}
+        if ( Data.rows() !=2 )
+                { if ( Data.cols() ==2 ) Data=Data.T(); else mexErrMsgTxt ( "Data are not 2D!" );}
         int ilow = mxArray2int ( input[1] )-1;
         int ihi = mxArray2int ( input[2] )-1; //coreection for different indeces
@@ -20 +20 @@
         // ------------------ Start of routine ---------------------------
         int ndat=Data.cols();
-        int npsi=Data.rows()+1; //add a constant
+        int npsi=Data.rows() +1; //add a constant
         RV thr ( "1","{ theta_r }",vec_1 ( npsi ),vec_1 ( 0 ) );
 
-        mat V0=1e-8*eye ( npsi ); V0 ( 0,0 ) =10.0;
-        vec rgr(npsi);
-        double nu0=2;
+        mat V0=1e-8*eye ( npsi ); V0 ( 0,0 ) =1e-4;
+        vec rgr ( npsi );
+        double nu0=0.1;
         // fitting a linear part => third coef is "1"
-        rgr(2) = 1.0;
+        rgr ( 2 ) = 1.0;
 
         // RESULTS
-        mat Tlls=zeros(ihi,ihi);
+        mat Tlls=zeros ( ihi+1,ihi+1);
         // AR model
         ARX Ar ( thr,V0,nu0 );
 
         //Initialize
-        for (int i=ilow; i<ihi; i++) {
-                rgr.set_subvector(0,Data.get_col ( i ));
-                Ar.bayes(rgr);
+        for ( int i=ilow+2; i<ihi-2; i++ ) {
+                rgr.set_subvector ( 0,Data.get_col ( i ) );
+                Ar.bayes ( rgr );
         }
-        Ar.get_parameters(V0,nu0);
-        V0 /=V0(1,1);
-        nu0=3;
-        Ar.set_parameters(V0,nu0);
-                
         // FLATTEN
-        for (int i=ilow; i<ihi; i++) {
-                Ar.set_parameters(V0,nu0);
-                for (int t=i+1; t<ihi; t++) {
-                        rgr.set_subvector(0,Data.get_col ( t ));
-                        Ar.bayes(rgr);
-                        Tlls(i,t)=Ar._tll();
+/*      Ar.get_parameters ( V0,nu0 );
+        V0 *=1.0/nu0/10000.; // same flattening factor as for nu0
+        nu0=1./10000.;*/
+        Ar.set_parameters ( V0,nu0 );
+
+        for ( int i=ilow; i<=ihi; i++ ) {
+                Ar.set_parameters ( V0,nu0 );
+                for ( int t=i; t<=ihi; t++ ) {
+                        rgr.set_subvector ( 0,Data.get_col ( t ) );
+                        Ar.bayes ( rgr );
+                        Tlls ( i,t ) =Ar._tll();
                 }
         }
 
-        max_index(Tlls,ilow,ihi);
+        max_index ( Tlls,ilow,ihi );
 
         // ------------------ End of routine -----------------------------
 
         output[0] = mxCreateDoubleMatrix ( 2,1, mxREAL );
-        ivec2mxArray (vec_2(ilow+1,ihi+1) , output[0] );
+        ivec2mxArray ( vec_2 ( ilow+1,ihi+1 ) , output[0] );
 
         if ( n_output>1 ) {
-        // Create output vectors
-        output[1] = mxCreateDoubleMatrix ( Tlls.rows(),Tlls.cols(), mxREAL );
-        // Convert the IT++ format to Matlab format for output
-        mat2mxArray ( Tlls, output[1] );
+                // Create output vectors
+                output[1] = mxCreateDoubleMatrix ( Tlls.rows(),Tlls.cols(), mxREAL );
+                // Convert the IT++ format to Matlab format for output
+                mat2mxArray ( Tlls, output[1] );
         }
-        
+
+        if ( n_output>2 ) {
+                Ar.set_parameters ( V0,nu0 );
+                // Redo Ar for given points
+                for ( int i=ilow; i<=ihi; i++ ) {
+                        rgr.set_subvector ( 0,Data.get_col ( i ) );
+                        Ar.bayes ( rgr );
+                }
+
+                // Create output vectors
+                output[2] = mxCreateDoubleMatrix ( 3,1, mxREAL );
+                // Convert the IT++ format to Matlab format for output
+//              mat2mxArray ( Ar._epdf().mean(), output[2] );
+                mat2mxArray ( Ar._epdf().mean(), output[2] );
+        }
+
 }

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");       
         
 

pmsm/sim_var_arx.cpp

@@ -43 +43 @@
 
         //Autoregressive model
-        ARX Ar_a ( rgr,V0,nu0 ,0.99 );
-        ARX Ar_b ( rgr,V0,nu0 ,0.99 );
+        ARX Ar_a ( rgr,V0,nu0 ,0.95 );
+        ARX Ar_b ( rgr,V0,nu0 ,0.95 );
         epdf& pA= Ar_a._epdf();
         epdf& pB= Ar_b._epdf();

pmsm/simulator_zdenek/simulator.cpp

@@ -73 +73 @@
 // real-time
 double t=0.; //VS removed static due to clash with export in .h
+
+// stator voltage components in alfa beta (inluding impact of the real dc-link voltage)
+static double ualfa=0., ubeta=0.;
 
 void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0)
@@ -114 +117 @@
   citac2=abs(0-(int)(DT/h)); //VS: oprava, je to spravne?
   citac_PR=h_reg_counter_mez;
+
+  // first interrupt occur after first period match => add 1 to both counter registers
+  citac++;smer=1;
+  citac2--;
 
   modulace=1;           // THIPWM
@@ -304 +311 @@
 void pmsmsim_step(double Ww)            // you must link array KalmanObs[] to EKF modul
 {
-  double Umk, ua, ub;
+  double Umk, ub, uc;
 
 //  while (t<=t_end)
@@ -310 +317 @@
     pwm(modulace);
 //    *us=KalmanObs[0]; *(us+1)=KalmanObs[1];
+//     *us=ualfa; *(us+1)=ubeta;
     pmsm_model(5);
 
@@ -315 +323 @@
     {
       // voltages and measured currents for EKF
-      Umk=*u*Uc/Ucn;
-      ua=Umk*cos(*(u+1));
-      ub=Umk*cos(*(u+1)-2./3.*M_PI);
-      KalmanObs[0]=ua;                     // usx
-      KalmanObs[1]=(ua+2.*ub)/sqrt(3.);    // usy
+//       Umk=*u*Uc/Ucn;
+//       ualfa=Umk*cos(*(u+1));
+//       ub=Umk*cos(*(u+1)-2./3.*M_PI);
+      KalmanObs[0]=ualfa;                     // usx
+      //KalmanObs[1]=(ualfa+2.*ub)/sqrt(3.);    // usy
+      KalmanObs[1]=ubeta;    // usy
       
       // real sampling - considered transport delay equal to the sampling period
@@ -341 +350 @@
       Isx=x[0];Isy=x[1];speed=x[2];theta=x[3];
 
+      // include ideal commanded stator voltage 
+      Umk=*u*Uc/Ucn;
+      ualfa=Umk*cos(*(u+1));               // usx = usa
+      ub=Umk*cos(*(u+1)-2./3.*M_PI);
+      ubeta=(ualfa+2.*ub)/sqrt(3.);    // usy
+//       uc=-ualfa-ub;
+//       ubeta=(ub-uc)/sqrt(3.);
+
       h_reg_counter=0;
     }