Changeset 1292

Timestamp:

03/14/11 09:32:19 (14 years ago)

Author:

smidl

Message:

model in DQ + test

Location:

applications/pmsm

Files:

• pmsm.h (modified) (3 diffs)
• testbidiff.cpp (modified) (1 diff)

applications/pmsm/pmsm.h

@@ -124 +124 @@
 
 UIREGISTER ( IMpmsm );
+
+
+//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
+class IMpmsmDQ : public diffbifn {
+protected:
+        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
+        
+        bool compensate;
+        bool cutoff;
+public:
+        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
+        //! Set mechanical and electrical variables
+        virtual void set_parameters ( double Rs0, double Ld0, double Lq0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ld=Ld0; Lq=Lq0; 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 &idm = x0 ( 0 );
+                const double &iqm = x0 ( 1 );
+                const double &omm = x0 ( 2 );
+                const double &thm = x0 ( 3 );
+                double uam;
+                double ubm;
+                
+                if (compensate){
+                        modelpwm(x0,u0,uam,ubm);
+                } else {
+                        uam = u0(0);
+                        ubm = u0(1);
+                }
+                
+                vec xk( 4 );
+                //id
+                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
+                //iq
+                xk ( 1 ) = ( 1.0- Rs/Lq*dt ) * iqm - Ypm/Lq*dt*omm - Ld/Lq*dt*idm*omm + dt/Lq*(-sin(thm)*uam+cos(thm)*ubm);
+                //om
+                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
+                //th
+                xk ( 3 ) = thm + omm*dt; // <0..2pi>
+                if (cutoff) {
+                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
+                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
+                }
+                return xk;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                const double &idm = x0 ( 0 );
+                const double &iqm = x0 ( 1 );
+                const double &omm = x0 ( 2 );
+                const double &thm = x0 ( 3 );
+                
+                double uam;
+                double ubm;
+                if (compensate){
+                        modelpwm(x0,u0,uam,ubm);
+                } else {
+                        uam = u0(0);
+                        ubm = u0(1);
+                }
+                // d id
+                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
+                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
+                // d iq
+                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
+                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
+                // d om
+                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
+                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
+                A ( 2,2 ) = 1.0;
+                A ( 2,3 ) = 0;
+                // d th
+                A ( 3,0 ) = 0.0; 
+                A ( 3,1 ) = 0.0; 
+                A ( 3,2 ) = dt; A ( 3,3 ) = 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["Ld"],params["Lq"], 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 ( IMpmsmDQ );
+
+
+
+
+
+
 
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
@@ -382 +504 @@
 
                 void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
-                        if (full) A=eye(4);
+                        A.clear();
+                        A(0,0)=1.0;
+                        A(1,1)=1.0;
                 }
 };
@@ -388 +512 @@
 UIREGISTER ( OMpmsm4 );
 
+//! Observation model for PMSM drive id d-q coordinates
+class OMpmsmDQ: public diffbifn {
+public:
+        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
+        
+        vec eval ( const vec &x0, const vec &u0 ) {
+                vec y ( 2 );
+                double ct = cos(x0(3));
+                double st = sin(x0(3));
+                y(0)  = ct*x0(0)-st*x0(1);
+                y(1)  = st*x0(0)+ct*x0(1);
+                
+                return y;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                double ct = cos(x0(3));
+                double st = sin(x0(3));
+                A(0,0) = ct;
+                A(1,0) = -st;
+                A(0,1) = st;
+                A(1,1) = ct;
+                
+                A(0,2) = 0.0; 
+                A(1,2) = 0.0;
+                A(0,3) = -st*x0(0)-ct*x0(1);
+                A(1,3) = ct*x0(0)-st*x0(1);
+        }
+};
+
+UIREGISTER ( OMpmsmDQ );
 
 

applications/pmsm/testbidiff.cpp

@@ -12 +12 @@
 int main()
 {
-        vec x0 = "100 120 100 2";
+        vec x0 = "1 2 10 0";
         vec u0 = "1 1";
         double h=1e-6;
         
-        IMpmsm I;
-        I.set_parameters ( 0.28, 0.003465, h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
+        IMpmsmDQ I;
+        I.set_parameters ( 0.28, 0.003465, 0.003, h, 0.1989,   1.5 ,4.0, 0.04, 0.0 );
         
         vec x(x0);      x(0) += h;
+        cout << I.eval(x,u0) << ", " << I.eval(x0,u0) <<endl<<endl;
+        
         cout << (I.eval(x,u0)-I.eval(x0,u0))/h <<endl;
         x=x0;   x(1) += h;