root/pmsm/pmsm.h @ 302

Revision 283, 7.2 kB (checked in by smidl, 16 years ago)

get rid of BMcond + adaptation in doprava/

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <stat/libFN.h>
5
6/*! \defgroup PMSM
7@{
8*/
9
10using namespace bdm;
11
12//TODO hardcoded RVs!!!
13RV rx ( "{ia ib om th }");
14RV ru ( "{ua ub }");
15RV ry ( "{oia oib }");
16
17// class uipmsm : public uibase{
18//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
19// };
20
21//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
22class IMpmsm : public diffbifn {
23protected:
24        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
25
26public:
27        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2;};
28        //! Set mechanical and electrical variables
29        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;}
30
31        vec eval ( const vec &x0, const vec &u0 ) {
32                // last state
33                const double &iam = x0 ( 0 );
34                const double &ibm = x0 ( 1 );
35                const double &omm = x0 ( 2 );
36                const double &thm = x0 ( 3 );
37                const double &uam = u0 ( 0 );
38                const double &ubm = u0 ( 1 );
39
40                vec xk( 4 );
41                //ia
42                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
43                //ib
44                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
45                //om
46                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
47                //th
48                xk ( 3 ) = thm + omm*dt; // <0..2pi>
49                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
50                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
51                return xk;
52        }
53
54        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
55                const double &iam = x0 ( 0 );
56                const double &ibm = x0 ( 1 );
57                const double &omm = x0 ( 2 );
58                const double &thm = x0 ( 3 );
59                // d ia
60                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
61                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
62                // d ib
63                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
64                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
65                // d om
66                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
67                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
68                A ( 2,2 ) = 1.0;
69                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
70                // d th
71                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
72        }
73
74        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
75
76};
77
78//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
79class IMpmsm2o : public diffbifn {
80        protected:
81                double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
82                //! store first derivatives for the use in second derivatives
83                double dia, dib, dom, dth;
84                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
85                double d2t, cth, sth;
86                double iam, ibm, omm, thm, uam, ubm;
87        public:
88                IMpmsm2o() :diffbifn () {dimy=4;dimx=4;dimu=2;};
89        //! Set mechanical and electrical variables
90                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; d2t=dt*dt/2;}
91
92                vec eval ( const vec &x0, const vec &u0 ) {
93                // last state
94                        iam = x0 ( 0 );
95                        ibm = x0 ( 1 );
96                        omm = x0 ( 2 );
97                        thm = x0 ( 3 );
98                        uam = u0 ( 0 );
99                        ubm = u0 ( 1 );
100
101                        cth = cos(thm);
102                        sth = sin(thm);
103                       
104                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
105                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
106                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
107                        dth = omm;
108                                               
109                        vec xk=zeros ( 4 );
110                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
111                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
112                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
113                        xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi>
114                       
115                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
116                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
117                        return xk;
118                }
119
120                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
121                vec eval2o(const vec &du){
122                        double dua = du ( 0 )/dt;
123                        double dub = du ( 1 )/dt;
124                       
125                        vec xth2o(4);
126                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
127                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
128                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
129                        xth2o(3) = dom;
130                        return xth2o;
131                }
132                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
133                         iam = x0 ( 0 );
134                         ibm = x0 ( 1 );
135                         omm = x0 ( 2 );
136                         thm = x0 ( 3 );
137                // d ia
138                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
139                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
140                // d ib
141                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
142                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
143                // d om
144                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
145                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
146                        A ( 2,2 ) = 1.0;
147                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
148                // d th
149                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
150                }
151
152                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
153
154};
155
156//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
157class IMpmsmStat : public IMpmsm {
158        public:
159        IMpmsmStat() :IMpmsm() {};
160        //! Set mechanical and electrical variables
161        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;}
162
163        vec eval ( const vec &x0, const vec &u0 ) {
164                // last state
165                double iam = x0 ( 0 );
166                double ibm = x0 ( 1 );
167                double omm = x0 ( 2 );
168                double thm = x0 ( 3 );
169                double uam = u0 ( 0 );
170                double ubm = u0 ( 1 );
171
172                vec xk=zeros ( 4 );
173                //ia
174                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
175                //ib
176                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
177                //om
178                xk ( 2 ) = omm;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
179                //th
180                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
181                return xk;
182        }
183
184        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
185//              double iam = x0 ( 0 );
186//              double ibm = x0 ( 1 );
187                double omm = x0 ( 2 );
188                double thm = x0 ( 3 );
189                // d ia
190                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
191                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
192                // d ib
193                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
194                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
195                // d om
196                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
197                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
198                A ( 2,2 ) = 1.0;
199                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
200                // d th
201                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
202        }
203
204        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
205
206};
207
208//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
209class OMpmsm: public diffbifn {
210public:
211        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=2;};
212
213        vec eval ( const vec &x0, const vec &u0 ) {
214                vec y ( 2 );
215                y ( 0 ) = x0 ( 0 );
216                y ( 1 ) = x0 ( 1 );
217                return y;
218        }
219
220        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
221                A.clear();
222                A ( 0,0 ) = 1.0;
223                A ( 1,1 ) = 1.0;
224        }
225};
226
227/*!@}*/
228#endif //PMSM_H
Note: See TracBrowser for help on using the browser.