root/applications/pmsm/pmsm.h @ 1312

Revision 1295, 19.4 kB (checked in by smidl, 14 years ago)

new modle of pmsm

  • 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!!!
14// RV rx ( "{ia ib om th }");
15// RV ru ( "{o_ua o_ub }");
16// RV 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//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
128class IMpmsmOT : public diffbifn {
129protected:
130        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
131       
132        bool compensate;
133        bool cutoff;
134public:
135        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=true;cutoff=true;};
136        //! Set mechanical and electrical variables
137        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;}
138       
139        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
140                /*              ua=u0[0];
141                 *              ub=u0[1];
142                 *              return;*/
143                double sq3=sqrt ( 3.0 );
144                double i1=x0(0);
145                double i2=0.5* ( -i1+sq3*x0[1] );
146                double i3=0.5* ( -i1-sq3*x0[1] );
147                double u1=u0(0);
148                double u2=0.5* ( -u1+sq3*u0(1) );
149                double u3=0.5* ( -u1-sq3*u0(1) );
150               
151                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
152                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
153                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
154                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
155                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
156        }
157       
158        vec eval ( const vec &x0, const vec &u0 ) {
159                // last state
160                const double &omm = x0 ( 0 );
161                const double &thm = x0 ( 1 );
162                double uam;
163                double ubm;
164               
165                if (compensate){
166                        modelpwm(x0,u0,uam,ubm);
167                } else {
168                        uam = u0(0);
169                        ubm = u0(1);
170                }
171               
172                const double &iam = u0 ( 2 );
173                const double &ibm = u0 ( 3 );
174               
175                vec xk( 2 );
176                //ia
177                //om
178                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
179                //th
180                xk ( 1 ) = thm + omm*dt; // <0..2pi>
181                if (cutoff) {
182                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
183                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
184                }
185                return xk;
186        }
187       
188        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
189                const double &omm = x0 ( 0 );
190                const double &thm = x0 ( 1 );
191
192                const double &iam = u0 ( 2 );
193                const double &ibm = u0 ( 3 );
194               
195                // d ia
196                // d om
197                A ( 0,0 ) = 1.0;
198                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
199                // d th
200                A ( 1,0 ) = dt; 
201                A ( 1,1 ) = 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        void from_setting( const Setting &root )
207        {       
208               
209                const SettingResolver& params_b=root["params"];
210                const Setting& params=params_b.result;
211               
212                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
213                params["kp"], params["p"], params["J"], 0.0);
214                int comp=0;
215                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
216                int cuto=0;
217                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
218        };
219       
220        // TODO dodelat void to_setting( Setting &root ) const;
221};
222
223UIREGISTER ( IMpmsmOT );
224
225
226//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
227class IMpmsmDQ : public diffbifn {
228protected:
229        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
230       
231        bool compensate;
232        bool cutoff;
233public:
234        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
235        //! Set mechanical and electrical variables
236        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;}
237       
238        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
239                /*              ua=u0[0];
240                 *              ub=u0[1];
241                 *              return;*/
242                double sq3=sqrt ( 3.0 );
243                double i1=x0(0);
244                double i2=0.5* ( -i1+sq3*x0[1] );
245                double i3=0.5* ( -i1-sq3*x0[1] );
246                double u1=u0(0);
247                double u2=0.5* ( -u1+sq3*u0(1) );
248                double u3=0.5* ( -u1-sq3*u0(1) );
249               
250                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
251                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
252                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
253                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
254                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
255        }
256       
257        vec eval ( const vec &x0, const vec &u0 ) {
258                // last state
259                const double &idm = x0 ( 0 );
260                const double &iqm = x0 ( 1 );
261                const double &omm = x0 ( 2 );
262                const double &thm = x0 ( 3 );
263                double uam;
264                double ubm;
265               
266                if (compensate){
267                        modelpwm(x0,u0,uam,ubm);
268                } else {
269                        uam = u0(0);
270                        ubm = u0(1);
271                }
272               
273                vec xk( 4 );
274                //id
275                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
276                //iq
277                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);
278                //om
279                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
280                //th
281                xk ( 3 ) = thm + omm*dt; // <0..2pi>
282                if (cutoff) {
283                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
284                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
285                }
286                return xk;
287        }
288       
289        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
290                const double &idm = x0 ( 0 );
291                const double &iqm = x0 ( 1 );
292                const double &omm = x0 ( 2 );
293                const double &thm = x0 ( 3 );
294               
295                double uam;
296                double ubm;
297                if (compensate){
298                        modelpwm(x0,u0,uam,ubm);
299                } else {
300                        uam = u0(0);
301                        ubm = u0(1);
302                }
303                // d id
304                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
305                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
306                // d iq
307                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
308                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
309                // d om
310                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
311                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
312                A ( 2,2 ) = 1.0;
313                A ( 2,3 ) = 0;
314                // d th
315                A ( 3,0 ) = 0.0; 
316                A ( 3,1 ) = 0.0; 
317                A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
318        }
319       
320        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
321       
322        void from_setting( const Setting &root )
323        {       
324               
325                const SettingResolver& params_b=root["params"];
326                const Setting& params=params_b.result;
327               
328                set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
329                params["kp"], params["p"], params["J"], 0.0);
330                int comp=0;
331                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
332                int cuto=0;
333                if (UI::get(cuto,root,"cutoff",UI::optional)){
334                        cutoff=(cuto==1);
335                }
336        };
337       
338        // TODO dodelat void to_setting( Setting &root ) const;
339};
340
341UIREGISTER ( IMpmsmDQ );
342
343
344//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
345class IMpmsm2o : public IMpmsm {
346        protected:
347//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
348                //! store first derivatives for the use in second derivatives
349                double dia, dib, dom, dth;
350                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
351                double d2t, cth, sth;
352                double iam, ibm, omm, thm, uam, ubm;
353        public:
354                IMpmsm2o() :IMpmsm () {};
355        //! Set mechanical and electrical variables
356                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;}
357
358                vec eval ( const vec &x0, const vec &u0 ) {
359                // last state
360                        iam = x0 ( 0 );
361                        ibm = x0 ( 1 );
362                        omm = x0 ( 2 );
363                        thm = x0 ( 3 );
364                        uam = u0 ( 0 );
365                        ubm = u0 ( 1 );
366
367                        cth = cos(thm);
368                        sth = sin(thm);
369                       
370                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
371                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
372                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
373                        dth = omm;
374                                               
375                        vec xk=zeros ( 4 );
376                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
377                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
378                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
379                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
380                       
381                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
382                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
383                        return xk;
384                }
385
386                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
387                vec eval2o(const vec &du){
388                        double dua = du ( 0 )/dt;
389                        double dub = du ( 1 )/dt;
390                       
391                        vec xth2o(4);
392                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
393                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
394                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
395                        xth2o(3) = dom;
396                        // multiply by dt^2/2
397                        xth2o*=d2t/2;
398                        return xth2o;
399                }
400                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
401                         iam = x0 ( 0 );
402                         ibm = x0 ( 1 );
403                         omm = x0 ( 2 );
404                         thm = x0 ( 3 );
405                // d ia
406                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
407                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
408                // d ib
409                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
410                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
411                // d om
412                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
413                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
414                        A ( 2,2 ) = 1.0;
415                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
416                // d th
417                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
418                        // FOR d2t*dom!!!!!!!!!
419/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
420                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
421                        A ( 3,2 ) = dt;
422                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
423                }
424
425                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
426
427};
428
429
430UIREGISTER ( IMpmsm2o );
431
432//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
433class IMpmsmStat : public IMpmsm {
434        public:
435        IMpmsmStat() :IMpmsm() {};
436        //! Set mechanical and electrical variables
437        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;}
438
439        vec eval ( const vec &x0, const vec &u0 ) {
440                // last state
441                double iam = x0 ( 0 );
442                double ibm = x0 ( 1 );
443                double omm = x0 ( 2 );
444                double thm = x0 ( 3 );
445                double uam = u0 ( 0 );
446                double ubm = u0 ( 1 );
447
448                vec xk=zeros ( 4 );
449                //ia
450                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
451                //ib
452                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
453                //om
454                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
455                //th
456                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
457                return xk;
458        }
459
460        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
461//              double iam = x0 ( 0 );
462//              double ibm = x0 ( 1 );
463                double omm = x0 ( 2 );
464                double thm = x0 ( 3 );
465                // d ia
466                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
467                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
468                // d ib
469                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
470                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
471                // d om
472                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
473                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
474                A ( 2,2 ) = 1.0;
475                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
476                // d th
477                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
478        }
479
480        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
481
482};
483
484UIREGISTER ( IMpmsmStat );
485
486
487//! State for PMSM with unknown Mz
488class IMpmsmMz: public IMpmsm{
489        public:
490                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
491        //! extend eval by Mz
492                vec eval ( const vec &x0, const vec &u0 ) {
493                        vec x(4);
494                        Mz = x0(4); //last of the state is Mz
495               
496                //teh first 4 states are same as before (given that Mz is set)
497                        x=IMpmsm::eval(x0,u0); // including model of drops!
498                        return concat(x,Mz);
499                }
500                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
501                //call initial
502                        if (full) A.clear();
503                        IMpmsm::dfdx_cond(x0,u0,A,full);
504                        A(2,4)=- p/J*dt;
505                        A(4,4)=1.0;
506                }       
507};
508
509UIREGISTER ( IMpmsmMz );
510
511//! State for PMSM with unknown Mz
512class IMpmsmStatMz: public IMpmsmStat{
513        public:
514                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
515        //! extend eval by Mz
516                vec eval ( const vec &x0, const vec &u0 ) {
517                        vec x(4);
518                        Mz = x0(4); //last of the state is Mz
519               
520                //teh first 4 states are same as before (given that Mz is set)
521                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
522                        return concat(x,Mz);
523                }
524                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
525                //call initial
526                        if (full) A.clear();
527                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
528                        A(2,4)=- p/J*dt;
529                        A(4,4)=1.0;
530                }       
531};
532
533UIREGISTER ( IMpmsmStatMz );
534
535
536//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
537class OMpmsm: public diffbifn {
538public:
539        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
540
541        vec eval ( const vec &x0, const vec &u0 ) {
542                vec y ( 2 );
543                y ( 0 ) = x0 ( 0 );
544                y ( 1 ) = x0 ( 1 );
545                return y;
546        }
547
548        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
549                A.clear();
550                A ( 0,0 ) = 1.0;
551                A ( 1,1 ) = 1.0;
552        }
553};
554
555UIREGISTER ( OMpmsm );
556
557//! Observation model for PMSM drive with roundoff-errors
558class OMpmsmRO: public diffbifn {
559        public:
560                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
561               
562                vec eval ( const vec &x0, const vec &u0 ) {
563                        vec y ( 2 );
564/*                      y ( 0 ) = x0 ( 0 );
565                        y ( 1 ) = x0 ( 1 );*/
566                       
567                        double istep=0.085;
568                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
569                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
570                       
571                        return y;
572                }
573               
574                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
575                        A.clear();
576                        A ( 0,0 ) = 1.0;
577                        A ( 1,1 ) = 1.0;
578                }
579};
580
581UIREGISTER ( OMpmsmRO );
582
583class OMpmsmROMz: public OMpmsmRO{
584public:
585        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
586};
587UIREGISTER ( OMpmsmROMz );
588
589//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
590class OMpmsm4: public diffbifn {
591        public:
592                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
593
594                vec eval ( const vec &x0, const vec &u0 ) {
595                        vec y ( 4 );
596                        y  = x0 ;
597                        return y;
598                }
599
600                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
601                        A.clear();
602                        A(0,0)=1.0;
603                        A(1,1)=1.0;
604                }
605};
606
607UIREGISTER ( OMpmsm4 );
608
609//! Observation model for PMSM drive id d-q coordinates
610class OMpmsmDQ: public diffbifn {
611public:
612        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
613       
614        vec eval ( const vec &x0, const vec &u0 ) {
615                vec y ( 2 );
616                double ct = cos(x0(3));
617                double st = sin(x0(3));
618                y(0)  = ct*x0(0)-st*x0(1);
619                y(1)  = st*x0(0)+ct*x0(1);
620               
621                return y;
622        }
623       
624        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
625                double ct = cos(x0(3));
626                double st = sin(x0(3));
627                A(0,0) = ct;
628                A(1,0) = -st;
629                A(0,1) = st;
630                A(1,1) = ct;
631               
632                A(0,2) = 0.0; 
633                A(1,2) = 0.0;
634                A(0,3) = -st*x0(0)-ct*x0(1);
635                A(1,3) = ct*x0(0)-st*x0(1);
636        }
637};
638
639UIREGISTER ( OMpmsmDQ );
640
641//! Observation model for PMSM drive in reduced form coordinates
642class OMpmsmOT: public diffbifn {
643public:
644        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
645       
646        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;}
647       
648        OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
649       
650        vec eval ( const vec &x0, const vec &u0 ) {
651                vec y ( 2 );
652                const double &omm = x0(0);
653                const double &thm = x0(1);
654               
655                const double &uam = u0 ( 0 );
656                const double &ubm = u0 ( 1 );
657                const double &iam = u0 ( 2 );
658                const double &ibm = u0 ( 3 );
659               
660                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
661                //ib
662                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
663               
664                return y;
665        }
666       
667        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
668                const double &omm = x0(0);
669                const double &thm = x0(1);
670                               
671                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
672                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
673                // d ib
674                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
675                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
676               
677               
678        }
679
680void from_setting( const Setting &root )
681{       
682       
683        const SettingResolver& params_b=root["params"];
684        const Setting& params=params_b.result;
685       
686        set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
687        params["kp"], params["p"], params["J"], 0.0);
688};
689
690};
691
692UIREGISTER ( OMpmsmOT );
693
694
695/*!@}*/
696#endif //PMSM_H
Note: See TracBrowser for help on using the browser.