root/applications/pmsm/pmsm.h @ 1239

Revision 1192, 11.2 kB (checked in by smidl, 14 years ago)

fixes ob ProdBM

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <math/functions.h>
5#include "base/user_info.h"
6
7/*! \defgroup PMSM
8@{
9*/
10
11using namespace bdm;
12
13//TODO hardcoded RVs!!!
14RV rx ( "{ia ib om th }");
15RV ru ( "{o_ua o_ub }");
16RV ry ( "{oia oib }");
17
18// class uipmsm : public uibase{
19//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
20// };
21
22//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
23class IMpmsm : public diffbifn {
24protected:
25        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
26
27        bool compensate;
28        bool cutoff;
29public:
30        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
31        //! Set mechanical and electrical variables
32        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;}
33
34        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
35/*              ua=u0[0];
36                ub=u0[1];
37                return;*/
38                double sq3=sqrt ( 3.0 );
39                double i1=x0(0);
40                double i2=0.5* ( -i1+sq3*x0[1] );
41                double i3=0.5* ( -i1-sq3*x0[1] );
42                double u1=u0(0);
43                double u2=0.5* ( -u1+sq3*u0(1) );
44                double u3=0.5* ( -u1-sq3*u0(1) );
45
46                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
47                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
48                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
49                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
50                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
51        }
52
53        vec eval ( const vec &x0, const vec &u0 ) {
54                // last state
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                double uam;
60                double ubm;
61
62                if (compensate){
63                        modelpwm(x0,u0,uam,ubm);
64                } else {
65                        uam = u0(0);
66                        ubm = u0(1);
67                }
68               
69               
70                vec xk( 4 );
71                //ia
72                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
73                //ib
74                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
75                //om
76                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
77                //th
78                xk ( 3 ) = thm + omm*dt; // <0..2pi>
79                if (cutoff) {
80                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
81                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
82                }
83                return xk;
84        }
85
86        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
87                const double &iam = x0 ( 0 );
88                const double &ibm = x0 ( 1 );
89                const double &omm = x0 ( 2 );
90                const double &thm = x0 ( 3 );
91                // d ia
92                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
93                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
94                // d ib
95                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
96                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
97                // d om
98                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
99                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
100                A ( 2,2 ) = 1.0;
101                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
102                // d th
103                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
104        }
105
106        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
107
108        void from_setting( const Setting &root )
109        {       
110
111                const SettingResolver& params_b=root["params"];
112                const Setting& params=params_b.result;
113               
114                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
115                         params["kp"], params["p"], params["J"], 0.0);
116                int comp=0;
117                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
118                int cuto=0;
119                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
120        };
121
122        // TODO dodelat void to_setting( Setting &root ) const;
123};
124
125UIREGISTER ( IMpmsm );
126
127//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
128class IMpmsm2o : public IMpmsm {
129        protected:
130//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
131                //! store first derivatives for the use in second derivatives
132                double dia, dib, dom, dth;
133                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
134                double d2t, cth, sth;
135                double iam, ibm, omm, thm, uam, ubm;
136        public:
137                IMpmsm2o() :IMpmsm () {};
138        //! Set mechanical and electrical variables
139                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;}
140
141                vec eval ( const vec &x0, const vec &u0 ) {
142                // last state
143                        iam = x0 ( 0 );
144                        ibm = x0 ( 1 );
145                        omm = x0 ( 2 );
146                        thm = x0 ( 3 );
147                        uam = u0 ( 0 );
148                        ubm = u0 ( 1 );
149
150                        cth = cos(thm);
151                        sth = sin(thm);
152                       
153                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
154                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
155                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
156                        dth = omm;
157                                               
158                        vec xk=zeros ( 4 );
159                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
160                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
161                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
162                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
163                       
164                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
165                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
166                        return xk;
167                }
168
169                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
170                vec eval2o(const vec &du){
171                        double dua = du ( 0 )/dt;
172                        double dub = du ( 1 )/dt;
173                       
174                        vec xth2o(4);
175                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
176                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
177                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
178                        xth2o(3) = dom;
179                        // multiply by dt^2/2
180                        xth2o*=d2t/2;
181                        return xth2o;
182                }
183                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
184                         iam = x0 ( 0 );
185                         ibm = x0 ( 1 );
186                         omm = x0 ( 2 );
187                         thm = x0 ( 3 );
188                // d ia
189                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
190                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
191                // d ib
192                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
193                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
194                // d om
195                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
196                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
197                        A ( 2,2 ) = 1.0;
198                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
199                // d th
200                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
201                        // FOR d2t*dom!!!!!!!!!
202/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
203                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
204                        A ( 3,2 ) = dt;
205                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
206                }
207
208                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
209
210};
211
212
213UIREGISTER ( IMpmsm2o );
214
215//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
216class IMpmsmStat : public IMpmsm {
217        public:
218        IMpmsmStat() :IMpmsm() {};
219        //! Set mechanical and electrical variables
220        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;}
221
222        vec eval ( const vec &x0, const vec &u0 ) {
223                // last state
224                double iam = x0 ( 0 );
225                double ibm = x0 ( 1 );
226                double omm = x0 ( 2 );
227                double thm = x0 ( 3 );
228                double uam = u0 ( 0 );
229                double ubm = u0 ( 1 );
230
231                vec xk=zeros ( 4 );
232                //ia
233                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
234                //ib
235                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
236                //om
237                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
238                //th
239                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
240                return xk;
241        }
242
243        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
244//              double iam = x0 ( 0 );
245//              double ibm = x0 ( 1 );
246                double omm = x0 ( 2 );
247                double thm = x0 ( 3 );
248                // d ia
249                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
250                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
251                // d ib
252                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
253                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
254                // d om
255                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
256                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
257                A ( 2,2 ) = 1.0;
258                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
259                // d th
260                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
261        }
262
263        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
264
265};
266
267UIREGISTER ( IMpmsmStat );
268
269
270//! State for PMSM with unknown Mz
271class IMpmsmMz: public IMpmsm{
272        public:
273                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
274        //! extend eval by Mz
275                vec eval ( const vec &x0, const vec &u0 ) {
276                        vec x(4);
277                        Mz = x0(4); //last of the state is Mz
278               
279                //teh first 4 states are same as before (given that Mz is set)
280                        x=IMpmsm::eval(x0,u0); // including model of drops!
281                        return concat(x,Mz);
282                }
283                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
284                //call initial
285                        if (full) A.clear();
286                        IMpmsm::dfdx_cond(x0,u0,A,full);
287                        A(2,4)=- p/J*dt;
288                        A(4,4)=1.0;
289                }       
290};
291
292UIREGISTER ( IMpmsmMz );
293
294//! State for PMSM with unknown Mz
295class IMpmsmStatMz: public IMpmsmStat{
296        public:
297                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
298        //! extend eval by Mz
299                vec eval ( const vec &x0, const vec &u0 ) {
300                        vec x(4);
301                        Mz = x0(4); //last of the state is Mz
302               
303                //teh first 4 states are same as before (given that Mz is set)
304                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
305                        return concat(x,Mz);
306                }
307                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
308                //call initial
309                        if (full) A.clear();
310                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
311                        A(2,4)=- p/J*dt;
312                        A(4,4)=1.0;
313                }       
314};
315
316UIREGISTER ( IMpmsmStatMz );
317
318
319//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
320class OMpmsm: public diffbifn {
321public:
322        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
323
324        vec eval ( const vec &x0, const vec &u0 ) {
325                vec y ( 2 );
326                y ( 0 ) = x0 ( 0 );
327                y ( 1 ) = x0 ( 1 );
328                return y;
329        }
330
331        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
332                A.clear();
333                A ( 0,0 ) = 1.0;
334                A ( 1,1 ) = 1.0;
335        }
336};
337
338UIREGISTER ( OMpmsm );
339
340//! Observation model for PMSM drive with roundoff-errors
341class OMpmsmRO: public diffbifn {
342        public:
343                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
344               
345                vec eval ( const vec &x0, const vec &u0 ) {
346                        vec y ( 2 );
347/*                      y ( 0 ) = x0 ( 0 );
348                        y ( 1 ) = x0 ( 1 );*/
349                       
350                        double istep=0.085;
351                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
352                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
353                       
354                        return y;
355                }
356               
357                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
358                        A.clear();
359                        A ( 0,0 ) = 1.0;
360                        A ( 1,1 ) = 1.0;
361                }
362};
363
364UIREGISTER ( OMpmsmRO );
365
366class OMpmsmROMz: public OMpmsmRO{
367public:
368        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
369};
370UIREGISTER ( OMpmsmROMz );
371
372//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
373class OMpmsm4: public diffbifn {
374        public:
375                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
376
377                vec eval ( const vec &x0, const vec &u0 ) {
378                        vec y ( 4 );
379                        y  = x0 ;
380                        return y;
381                }
382
383                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
384                        if (full) A=eye(4);
385                }
386};
387
388UIREGISTER ( OMpmsm4 );
389
390
391
392
393
394/*!@}*/
395#endif //PMSM_H
Note: See TracBrowser for help on using the browser.