Changeset 1295 for applications

Timestamp:

03/14/11 09:32:42 (15 years ago)

Author:

smidl

Message:

new modle of pmsm

Location:

applications/pmsm

Files:

• cfg/zcu.cfg (modified) (1 diff)
• pmsm.h (modified) (4 diffs)
• pmsm_estim.cpp (modified) (2 diffs)

applications/pmsm/cfg/zcu.cfg

@@ -2 +2 @@
         Rs=0.28; // Resistance
         Ls=0.003465; // inductance
+        Ld=0.003465; // inductance
+        Lq=0.003; // inductance
         Fmag= 0.1989; // Magnetic??
         kp = 1.5;

applications/pmsm/pmsm.h

@@ -125 +125 @@
 UIREGISTER ( IMpmsm );
 
+//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
+class IMpmsmOT : public diffbifn {
+protected:
+        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+        
+        bool compensate;
+        bool cutoff;
+public:
+        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=true;cutoff=true;};
+        //! Set mechanical and electrical variables
+        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
+        
+        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
+                /*              ua=u0[0];
+                 *              ub=u0[1];
+                 *              return;*/
+                double sq3=sqrt ( 3.0 );
+                double i1=x0(0);
+                double i2=0.5* ( -i1+sq3*x0[1] );
+                double i3=0.5* ( -i1-sq3*x0[1] );
+                double u1=u0(0);
+                double u2=0.5* ( -u1+sq3*u0(1) );
+                double u3=0.5* ( -u1-sq3*u0(1) );
+                
+                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
+                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
+                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
+                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
+                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
+        }
+        
+        vec eval ( const vec &x0, const vec &u0 ) {
+                // last state
+                const double &omm = x0 ( 0 );
+                const double &thm = x0 ( 1 );
+                double uam;
+                double ubm;
+                
+                if (compensate){
+                        modelpwm(x0,u0,uam,ubm);
+                } else {
+                        uam = u0(0);
+                        ubm = u0(1);
+                }
+                
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                vec xk( 2 );
+                //ia
+                //om
+                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
+                //th
+                xk ( 1 ) = thm + omm*dt; // <0..2pi>
+                if (cutoff) {
+                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
+                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
+                }
+                return xk;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                const double &omm = x0 ( 0 );
+                const double &thm = x0 ( 1 );
+
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                // d ia
+                // d om
+                A ( 0,0 ) = 1.0;
+                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
+                // d th
+                A ( 1,0 ) = dt; 
+                A ( 1,1 ) = 1.0;
+        }
+        
+        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
+        
+        void from_setting( const Setting &root )
+        {       
+                
+                const SettingResolver& params_b=root["params"];
+                const Setting& params=params_b.result;
+                
+                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
+                params["kp"], params["p"], params["J"], 0.0);
+                int comp=0;
+                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
+                int cuto=0;
+                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
+        };
+        
+        // TODO dodelat void to_setting( Setting &root ) const;
+};
+
+UIREGISTER ( IMpmsmOT );
+
 
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
@@ -233 +331 @@
                 if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
                 int cuto=0;
-                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
+                if (UI::get(cuto,root,"cutoff",UI::optional)){
+                        cutoff=(cuto==1);
+                }
         };
         
@@ -240 +340 @@
 
 UIREGISTER ( IMpmsmDQ );
-
-
-
-
-
 
 
@@ -544 +639 @@
 UIREGISTER ( OMpmsmDQ );
 
+//! Observation model for PMSM drive in reduced form coordinates
+class OMpmsmOT: public diffbifn {
+public:
+        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+        
+        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
+        
+        OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
+        
+        vec eval ( const vec &x0, const vec &u0 ) {
+                vec y ( 2 );
+                const double &omm = x0(0);
+                const double &thm = x0(1);
+                
+                const double &uam = u0 ( 0 );
+                const double &ubm = u0 ( 1 );
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
+                //ib
+                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
+                
+                return y;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                const double &omm = x0(0);
+                const double &thm = x0(1);
+                                
+                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
+                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
+                // d ib
+                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
+                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
+                
+                
+        }
+
+void from_setting( const Setting &root )
+{       
+        
+        const SettingResolver& params_b=root["params"];
+        const Setting& params=params_b.result;
+        
+        set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
+        params["kp"], params["p"], params["J"], 0.0);
+};
+
+};
+
+UIREGISTER ( OMpmsmOT );
+
 
 /*!@}*/

applications/pmsm/pmsm_estim.cpp

@@ -3 +3 @@
 \file
 \brief Multi-Estimator (developped for PMSM)
- 
+
  */
 
@@ -24 +24 @@
 
 void mexFunction ( int n_output, mxArray *output[], int n_input, const mxArray *input[] ) {
-        // Check the number of inputs and output arguments
-        if ( n_input<2 ) mexErrMsgTxt ( "Usage:\n"
-                                                "[Res,estimators,Res2]=estimator(system, estimators, experiment, logger)\n"
-                                                "  system     = struct('class','datasource',...);  % Estimated system\n"
-                                                "  estimators = {struct('class','estimator',...),  % Estimators\n"
-                                                "                struct('class','estimator',...),...} \n"
-                                                "  === optional ==="
-                                                "  experiment = struct('ndat',10);                 % number of data in experiment, full length of finite datasources, 10 otherwise \n"
-                                                "  logger     = struct('class','mexlogger');       % How to store results, default=mexlog, i.e. matlab structure\n\n"
-                                                "Output:\n"
-                                                "  Res          Matlab structure with logged results, \n"  
-                                                "  estimators   Array of estimators updated with data \n"
-                                                "  Res2         When logfull log_level is on, this structure is filled with structures of posterior densities\n\n"
-                                                "see documentation of classes datasource, BM, and mexlogger and their offsprings in BDM." );
+    // Check the number of inputs and output arguments
+    if ( n_input<2 ) mexErrMsgTxt ( "Usage:\n"
+                                        "[Res,estimators,Res2]=estimator(system, estimators, experiment, logger)\n"
+                                        "  system     = struct('class','datasource',...);  % Estimated system\n"
+                                        "  estimators = {struct('class','estimator',...),  % Estimators\n"
+                                        "                struct('class','estimator',...),...} \n"
+                                        "  === optional ==="
+                                        "  experiment = struct('ndat',10);                 % number of data in experiment, full length of finite datasources, 10 otherwise \n"
+                                        "  logger     = struct('class','mexlogger');       % How to store results, default=mexlog, i.e. matlab structure\n\n"
+                                        "Output:\n"
+                                        "  Res          Matlab structure with logged results, \n"
+                                        "  estimators   Array of estimators updated with data \n"
+                                        "  Res2         When logfull log_level is on, this structure is filled with structures of posterior densities\n\n"
+                                        "see documentation of classes datasource, BM, and mexlogger and their offsprings in BDM." );
 
-        RV::clear_all();
-        //CONFIG
-        UImxArray F;
-        try {
-                F.addGroup ( input[0],"system" );
-                F.addList ( input[1],"estimators" );
-                if ( n_input>2 ) {
-                        F.addGroup ( input[2],"experiment" );
-                }
-                if ( n_input>3 ) {
-                        F.addGroup ( input[3],"logger" );
-                }
-        } catch ( SettingException e ) {
-                it_error ( "error: "+string ( e.getPath() ) );
-        }
+    RV::clear_all();
+    //CONFIG
+    UImxArray F;
+    try {
+        F.addGroup ( input[0],"system" );
+        F.addList ( input[1],"estimators" );
+        if ( n_input>2 ) {
+            F.addGroup ( input[2],"experiment" );
+        }
+        if ( n_input>3 ) {
+            F.addGroup ( input[3],"logger" );
+        }
+    } catch ( SettingException e ) {
+        it_error ( "error: "+string ( e.getPath() ) );
+    }
 
-        //DBG
-        F.writeFile ( "pmsm_estim.cfg" );
+    //DBG
+    F.writeFile ( "pmsm_estim.cfg" );
 
 #else
 int main ( int argc, char* argv[] ) {
-        const char *fname;
-        if ( argc>1 ) {fname = argv[1]; }
-        else { cout << "Missing configuration file.\n Usage: \n $> estimator config_file.cfg"<<endl; abort(); }
-        UIFile F ( fname );
+    const char *fname;
+    if ( argc>1 ) {
+        fname = argv[1];
+    }
+    else {
+        cout << "Missing configuration file.\n Usage: \n $> estimator config_file.cfg"<<endl;
+        abort();
+    }
+    UIFile F ( fname );
 #endif
-        
-        shared_ptr<logger> L = UI::build <logger>( F, "logger" );
-        shared_ptr<DS>  pDS = UI::build<DS> ( F, "system" );
-        Array<shared_ptr<BM> > Es;                      // array of estimators
-        UI::get( Es, F, "estimators" );
-        int nE = Es.length();   //number of estimators
-        int Ndat;                               //number of data records
-        F.lookupValue ( "experiment.ndat",Ndat );
-                        
-        if ( !L ) {
-        #ifdef MEX
-                        //mex logger has only from_setting constructor - we  have to fill it...
-                        L=new mexlog ( Ndat );
-        #else
-                        L=new stdlog();
-        #endif
+
+    shared_ptr<logger> L = UI::build <logger>( F, "logger" );
+    shared_ptr<DS>  pDS = UI::build<DS> ( F, "system" );
+    Array<shared_ptr<BM> > Es;                  // array of estimators
+    UI::get( Es, F, "estimators" );
+    int nE = Es.length();       //number of estimators
+    int Ndat;                           //number of data records
+    F.lookupValue ( "experiment.ndat",Ndat );
+
+    if ( !L ) {
+#ifdef MEX
+        //mex logger has only from_setting constructor - we  have to fill it...
+        L=new mexlog ( Ndat );
+#else
+        L=new stdlog();
+#endif
+    }
+    pDS->log_register ( *L, "true" );
+    string Ename;
+    for (int i=0; i<nE; i++) {
+        try {
+            UI::get ( Ename, F.getRoot()["estimators"][i], "name",UI::optional );
+        } catch ( ...) {
+            Ename="Est"+num2str ( i );
+        }
+        Es(i)->log_register(*L,Ename); // estimate
+    }
+    L->init();
+
+    vec dt=zeros ( pDS->_drv()._dsize() );   //data variable
+    Array<datalink*> Dls(nE);
+    Array<datalink*> Dlsc(nE);
+    Array<datalink_buffered*> Dls_buf (0);
+    for ( int i=0; i<Es.length(); i++ ) {
+        //check if they are properly named
+        bdm_assert(Es(i)->_yrv()._dsize() == Es(i)->dimensiony(), "Estimator["+num2str(i)+"] does not name yrv properly."
+                   "size(yrv)="+num2str(Es(i)->_yrv()._dsize() ) + ", declared dimension of y="+num2str(Es(i)->dimensiony()));
+        bdm_assert(Es(i)->_rvc()._dsize() == Es(i)->dimensionc(), "Estimator["+num2str(i)+"] does not name rvc properly."
+                   "size(rvc)="+num2str(Es(i)->_rvc()._dsize() ) + ", declared dimension of rvc="+num2str(Es(i)->dimensionc()));
+        //connect actual data
+        Dls ( i ) = new datalink ( Es ( i )->_yrv(),pDS->_drv() ); //datalink between a datasource and estimator
+        //connect data in condition
+        if (Es ( i )->_rvc().mint()<0) {
+            //delayed values are required
+
+            //create delayed dl
+            int ith_buf=Dls_buf.size();
+            Dls_buf.set_size( ith_buf + 1, true);
+            Dls_buf(ith_buf) = new datalink_buffered();
+            //add dl to list of buffered DS
+            Dlsc(i) = Dls_buf(ith_buf);
+            Dlsc(i)->set_connection ( Es ( i )->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator
+
+            bdm_assert_debug(Dlsc(i)->_downsize() == Es ( i )->_rvc()._dsize(), "Data required by Est[" + num2str(i) + "], " +
+                             Es(i)->_rvc().to_string() + ", are not available in DS drv:" + pDS->_drv().to_string(););
+        } else {
+            Dlsc ( i ) = new datalink ( Es ( i )->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator
+        }
+    }
+
+    // Main cycle
+    for ( int tK=1;tK<Ndat;tK++ ) {
+        // Data Source
+        pDS->getdata ( dt );                                    // read data
+        pDS->log_write ();
+
+        // Estimators
+        for (int i=0; i<nE; i++) {
+            Es(i)->bayes ( Dls(i)->pushdown ( dt ), Dlsc(i)->pushdown(dt) );            // update estimates
+
+            Es(i)->log_write ();
+        }
+        // Regulators
+        L->step();
+                pDS->step();                                                    // simulator step
+                for (int i=0; i<Dls_buf.length(); i++){
+                        Dls_buf(i)->store_data(dt);
                 }
-        pDS->log_register ( *L, "true" );
-        string Ename;
-        for (int i=0; i<nE; i++){
-                try {
-                        UI::get ( Ename, F.getRoot()["estimators"][i], "name",UI::optional );
-                } catch ( ...) {
-                        Ename="Est"+num2str ( i );
-                }
-                Es(i)->log_register(*L,Ename); // estimate
-        }
-        L->init();
-
-        vec dt=zeros ( pDS->_drv()._dsize() );   //data variable
-        Array<datalink*> Dls(nE); 
-        Array<datalink*> Dlsc(nE); 
-        for (int i=0; i<nE; i++){
-                Dls(i)=new datalink( Es(i)->_yrv(),pDS->_drv() ); //datalink between a datasource and estimator
-                Dlsc(i)=new datalink( Es(i)->_rvc(),pDS->_drv() ); //datalink between a datasource and estimator
-        }
-        
-        // Main cycle
-        for ( int tK=1;tK<Ndat;tK++ ) {
-                // Data Source
-                pDS->step();                                                    // simulator step
-                pDS->getdata ( dt );                                    // read data
-                pDS->log_write ();
-                
-                // Estimators
-                for (int i=0; i<nE; i++){
-                        Es(i)->bayes ( Dls(i)->pushdown ( dt ), Dlsc(i)->pushdown(dt) );                // update estimates
-
-                        Es(i)->log_write ();
-                }
-                // Regulators
-                L->step();
         }
 
-        L->finalize();
+    L->finalize();
 #ifdef MEX
-        mexlog* mL=dynamic_cast<mexlog*> ( L.get() );
+    mexlog* mL=dynamic_cast<mexlog*> ( L.get() );
 
-        if ( mL ) { // user wants output!!
-                if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
-                output[0] = mL->toCell();
-                if (n_output>1){ // write estimators
-                        UImxArray Ests;
-                        UI::save(Es, Ests,"estimators");
-                        output[1]=UImxArray::create_mxArray(Ests);
-                }
-                if (n_output>2) {
-                        mL->_setting_conf().setAutoConvert(true);
-                        output[2]= UImxArray::create_mxArray(mL->_setting_conf().getRoot());
-                }
-        }
+    if ( mL ) { // user wants output!!
+        if ( n_output<1 ) mexErrMsgTxt ( "Wrong number of output variables!" );
+        output[0] = mL->toCell();
+        if (n_output>1) { // write estimators
+            UImxArray Ests;
+            UI::save(Es, Ests,"estimators");
+            output[1]=UImxArray::create_mxArray(Ests);
+        }
+        if (n_output>2) {
+            mL->_setting_conf().setAutoConvert(true);
+            output[2]= UImxArray::create_mxArray(mL->_setting_conf().getRoot());
+        }
+    }
 #endif
 
-        for (int i=0; i<nE; i++){
-                delete Dls(i);
-                delete Dlsc(i);
-        }
+    for (int i=0; i<nE; i++) {
+        delete Dls(i);
+        delete Dlsc(i);
+    }
 }