root/applications/pmsm/pmsm.h @ 1338

Revision 1327, 23.2 kB (checked in by smidl, 15 years ago)

scaled models for direct comparison

  • 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#include "ekf_example/reference_Q15.h"
7
8/*! \defgroup PMSM
9@{
10*/
11
12using namespace bdm;
13
14//TODO hardcoded RVs!!!
15// RV rx ( "{ia ib om th }");
16// RV ru ( "{o_ua o_ub }");
17// RV ry ( "{oia oib }");
18
19// class uipmsm : public uibase{
20//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
21// };
22
23//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
24class IMpmsm : public diffbifn {
25protected:
26        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
27
28        bool compensate;
29        bool cutoff;
30public:
31        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
32        //! Set mechanical and electrical variables
33        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;}
34
35        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
36/*              ua=u0[0];
37                ub=u0[1];
38                return;*/
39                double sq3=sqrt ( 3.0 );
40                double i1=x0(0);
41                double i2=0.5* ( -i1+sq3*x0[1] );
42                double i3=0.5* ( -i1-sq3*x0[1] );
43                double u1=u0(0);
44                double u2=0.5* ( -u1+sq3*u0(1) );
45                double u3=0.5* ( -u1-sq3*u0(1) );
46
47                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
48                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
49                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
50                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
51                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
52        }
53
54        vec eval ( const vec &x0, const vec &u0 ) {
55                // last state
56                const double &iam = x0 ( 0 );
57                const double &ibm = x0 ( 1 );
58                const double &omm = x0 ( 2 );
59                const double &thm = x0 ( 3 );
60                double uam;
61                double ubm;
62
63                if (compensate){
64                        modelpwm(x0,u0,uam,ubm);
65                } else {
66                        uam = u0(0);
67                        ubm = u0(1);
68                }
69               
70               
71                vec xk( 4 );
72                //ia
73                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
74                //ib
75                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
76                //om
77                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
78                //th
79                xk ( 3 ) = thm + omm*dt; // <0..2pi>
80                if (cutoff) {
81                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
82                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
83                }
84                return xk;
85        }
86
87        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
88                const double &iam = x0 ( 0 );
89                const double &ibm = x0 ( 1 );
90                const double &omm = x0 ( 2 );
91                const double &thm = x0 ( 3 );
92                // d ia
93                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
94                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
95                // d ib
96                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
97                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
98                // d om
99                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
100                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
101                A ( 2,2 ) = 1.0;
102                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
103                // d th
104                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
105        }
106
107        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
108
109        void from_setting( const Setting &root )
110        {       
111
112                const SettingResolver& params_b=root["params"];
113                const Setting& params=params_b.result;
114               
115                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
116                         params["kp"], params["p"], params["J"], 0.0);
117                int comp=0;
118                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
119                int cuto=0;
120                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
121        };
122
123        // TODO dodelat void to_setting( Setting &root ) const;
124};
125
126UIREGISTER ( IMpmsm );
127
128//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
129class IMpmsmOT : public diffbifn {
130protected:
131        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
132       
133        bool compensate;
134        bool cutoff;
135public:
136        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=true;cutoff=true;};
137        //! Set mechanical and electrical variables
138        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;}
139       
140        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
141                /*              ua=u0[0];
142                 *              ub=u0[1];
143                 *              return;*/
144                double sq3=sqrt ( 3.0 );
145                double i1=x0(0);
146                double i2=0.5* ( -i1+sq3*x0[1] );
147                double i3=0.5* ( -i1-sq3*x0[1] );
148                double u1=u0(0);
149                double u2=0.5* ( -u1+sq3*u0(1) );
150                double u3=0.5* ( -u1-sq3*u0(1) );
151               
152                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
153                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
154                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
155                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
156                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
157        }
158       
159        vec eval ( const vec &x0, const vec &u0 ) {
160                // last state
161                const double &omm = x0 ( 0 );
162                const double &thm = x0 ( 1 );
163                double uam;
164                double ubm;
165               
166                if (compensate){
167                        modelpwm(x0,u0,uam,ubm);
168                } else {
169                        uam = u0(0);
170                        ubm = u0(1);
171                }
172               
173                const double &iam = u0 ( 2 );
174                const double &ibm = u0 ( 3 );
175               
176                vec xk( 2 );
177                //ia
178                //om
179                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
180                //th
181                xk ( 1 ) = thm + omm*dt; // <0..2pi>
182                if (cutoff) {
183                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
184                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
185                }
186                return xk;
187        }
188       
189        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
190                const double &omm = x0 ( 0 );
191                const double &thm = x0 ( 1 );
192
193                const double &iam = u0 ( 2 );
194                const double &ibm = u0 ( 3 );
195               
196                // d ia
197                // d om
198                A ( 0,0 ) = 1.0;
199                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
200                // d th
201                A ( 1,0 ) = dt; 
202                A ( 1,1 ) = 1.0;
203        }
204       
205        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
206       
207        void from_setting( const Setting &root )
208        {       
209               
210                const SettingResolver& params_b=root["params"];
211                const Setting& params=params_b.result;
212               
213                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
214                params["kp"], params["p"], params["J"], 0.0);
215                int comp=0;
216                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
217                int cuto=0;
218                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
219        };
220       
221        // TODO dodelat void to_setting( Setting &root ) const;
222};
223
224UIREGISTER ( IMpmsmOT );
225
226//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ -- Scaled to [-1,1]
227class IMpmsmOTsc : public diffbifn {
228protected:
229        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
230       
231        bool cutoff;
232public:
233        IMpmsmOTsc() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;cutoff=true;};
234        //! Set mechanical and electrical variables
235        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;}
236       
237       
238        vec eval ( const vec &x0, const vec &u0 ) {
239                // last state
240                const double &omm = x0 ( 0 );
241                const double &thm = x0 ( 1 );
242                double uam;
243                double ubm;
244               
245                uam = u0(0);
246                ubm = u0(1);
247               
248                const double &iam = u0 ( 2 );
249                const double &ibm = u0 ( 3 );
250               
251                vec xk( 2 );
252                //ia
253                //om
254                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt *Iref/Wref* ( ibm * cos ( thm*Thetaref/(1<<15) )-iam * sin ( thm*Thetaref/(1<<15) ) );
255                //th
256                xk ( 1 ) = thm + omm*Wref/Thetaref*dt; // <0..2pi>
257                if (cutoff) {
258                        if ( xk ( 1 ) >(1<<15) ) xk ( 1 )=-(1<<15);
259                        if ( xk ( 1 ) <-(1<<15) ) xk ( 1 ) =(1<<15);
260                }
261                return xk;
262        }
263       
264        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
265                const double &omm = x0 ( 0 );
266                const double &thm = x0 ( 1 );
267               
268                const double &iam = u0 ( 2 );
269                const double &ibm = u0 ( 3 );
270               
271                // d ia
272                // d om
273                A ( 0,0 ) = 1.0;
274                A ( 0,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
275                // d th
276                A ( 1,0 ) = dt*Wref/Thetaref; 
277                A ( 1,1 ) = 1.0;
278        }
279       
280        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
281       
282        void from_setting( const Setting &root )
283        {       
284               
285                const SettingResolver& params_b=root["params"];
286                const Setting& params=params_b.result;
287               
288                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
289                params["kp"], params["p"], params["J"], 0.0);
290                int cuto=0;
291                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
292        };
293       
294        // TODO dodelat void to_setting( Setting &root ) const;
295};
296
297UIREGISTER ( IMpmsmOTsc );
298
299
300//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
301class IMpmsmDQ : public diffbifn {
302protected:
303        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
304       
305        bool compensate;
306        bool cutoff;
307public:
308        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
309        //! Set mechanical and electrical variables
310        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;}
311       
312        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
313                /*              ua=u0[0];
314                 *              ub=u0[1];
315                 *              return;*/
316                double sq3=sqrt ( 3.0 );
317                double i1=x0(0);
318                double i2=0.5* ( -i1+sq3*x0[1] );
319                double i3=0.5* ( -i1-sq3*x0[1] );
320                double u1=u0(0);
321                double u2=0.5* ( -u1+sq3*u0(1) );
322                double u3=0.5* ( -u1-sq3*u0(1) );
323               
324                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
325                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
326                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
327                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
328                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
329        }
330       
331        vec eval ( const vec &x0, const vec &u0 ) {
332                // last state
333                const double &idm = x0 ( 0 );
334                const double &iqm = x0 ( 1 );
335                const double &omm = x0 ( 2 );
336                const double &thm = x0 ( 3 );
337                double uam;
338                double ubm;
339               
340                if (compensate){
341                        modelpwm(x0,u0,uam,ubm);
342                } else {
343                        uam = u0(0);
344                        ubm = u0(1);
345                }
346               
347                vec xk( 4 );
348                //id
349                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
350                //iq
351                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);
352                //om
353                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
354                //th
355                xk ( 3 ) = thm + omm*dt; // <0..2pi>
356                if (cutoff) {
357                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
358                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
359                }
360                return xk;
361        }
362       
363        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
364                const double &idm = x0 ( 0 );
365                const double &iqm = x0 ( 1 );
366                const double &omm = x0 ( 2 );
367                const double &thm = x0 ( 3 );
368               
369                double uam;
370                double ubm;
371                if (compensate){
372                        modelpwm(x0,u0,uam,ubm);
373                } else {
374                        uam = u0(0);
375                        ubm = u0(1);
376                }
377                // d id
378                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
379                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
380                // d iq
381                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
382                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
383                // d om
384                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
385                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
386                A ( 2,2 ) = 1.0;
387                A ( 2,3 ) = 0;
388                // d th
389                A ( 3,0 ) = 0.0; 
390                A ( 3,1 ) = 0.0; 
391                A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
392        }
393       
394        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
395       
396        void from_setting( const Setting &root )
397        {       
398               
399                const SettingResolver& params_b=root["params"];
400                const Setting& params=params_b.result;
401               
402                set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
403                params["kp"], params["p"], params["J"], 0.0);
404                int comp=0;
405                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
406                int cuto=0;
407                if (UI::get(cuto,root,"cutoff",UI::optional)){
408                        cutoff=(cuto==1);
409                }
410        };
411       
412        // TODO dodelat void to_setting( Setting &root ) const;
413};
414
415UIREGISTER ( IMpmsmDQ );
416
417
418//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
419class IMpmsm2o : public IMpmsm {
420        protected:
421//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
422                //! store first derivatives for the use in second derivatives
423                double dia, dib, dom, dth;
424                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
425                double d2t, cth, sth;
426                double iam, ibm, omm, thm, uam, ubm;
427        public:
428                IMpmsm2o() :IMpmsm () {};
429        //! Set mechanical and electrical variables
430                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;}
431
432                vec eval ( const vec &x0, const vec &u0 ) {
433                // last state
434                        iam = x0 ( 0 );
435                        ibm = x0 ( 1 );
436                        omm = x0 ( 2 );
437                        thm = x0 ( 3 );
438                        uam = u0 ( 0 );
439                        ubm = u0 ( 1 );
440
441                        cth = cos(thm);
442                        sth = sin(thm);
443                       
444                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
445                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
446                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
447                        dth = omm;
448                                               
449                        vec xk=zeros ( 4 );
450                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
451                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
452                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
453                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
454                       
455                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
456                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
457                        return xk;
458                }
459
460                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
461                vec eval2o(const vec &du){
462                        double dua = du ( 0 )/dt;
463                        double dub = du ( 1 )/dt;
464                       
465                        vec xth2o(4);
466                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
467                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
468                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
469                        xth2o(3) = dom;
470                        // multiply by dt^2/2
471                        xth2o*=d2t/2;
472                        return xth2o;
473                }
474                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
475                         iam = x0 ( 0 );
476                         ibm = x0 ( 1 );
477                         omm = x0 ( 2 );
478                         thm = x0 ( 3 );
479                // d ia
480                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
481                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
482                // d ib
483                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
484                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
485                // d om
486                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
487                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
488                        A ( 2,2 ) = 1.0;
489                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
490                // d th
491                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
492                        // FOR d2t*dom!!!!!!!!!
493/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
494                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
495                        A ( 3,2 ) = dt;
496                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
497                }
498
499                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
500
501};
502
503
504UIREGISTER ( IMpmsm2o );
505
506//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
507class IMpmsmStat : public IMpmsm {
508        public:
509        IMpmsmStat() :IMpmsm() {};
510        //! Set mechanical and electrical variables
511        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;}
512
513        vec eval ( const vec &x0, const vec &u0 ) {
514                // last state
515                double iam = x0 ( 0 );
516                double ibm = x0 ( 1 );
517                double omm = x0 ( 2 );
518                double thm = x0 ( 3 );
519                double uam = u0 ( 0 );
520                double ubm = u0 ( 1 );
521
522                vec xk=zeros ( 4 );
523                //ia
524                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
525                //ib
526                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
527                //om
528                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
529                //th
530                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
531                return xk;
532        }
533
534        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
535//              double iam = x0 ( 0 );
536//              double ibm = x0 ( 1 );
537                double omm = x0 ( 2 );
538                double thm = x0 ( 3 );
539                // d ia
540                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
541                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
542                // d ib
543                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
544                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
545                // d om
546                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
547                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
548                A ( 2,2 ) = 1.0;
549                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
550                // d th
551                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
552        }
553
554        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
555
556};
557
558UIREGISTER ( IMpmsmStat );
559
560
561//! State for PMSM with unknown Mz
562class IMpmsmMz: public IMpmsm{
563        public:
564                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
565        //! extend eval by Mz
566                vec eval ( const vec &x0, const vec &u0 ) {
567                        vec x(4);
568                        Mz = x0(4); //last of the state is Mz
569               
570                //teh first 4 states are same as before (given that Mz is set)
571                        x=IMpmsm::eval(x0,u0); // including model of drops!
572                        return concat(x,Mz);
573                }
574                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
575                //call initial
576                        if (full) A.clear();
577                        IMpmsm::dfdx_cond(x0,u0,A,full);
578                        A(2,4)=- p/J*dt;
579                        A(4,4)=1.0;
580                }       
581};
582
583UIREGISTER ( IMpmsmMz );
584
585//! State for PMSM with unknown Mz
586class IMpmsmStatMz: public IMpmsmStat{
587        public:
588                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
589        //! extend eval by Mz
590                vec eval ( const vec &x0, const vec &u0 ) {
591                        vec x(4);
592                        Mz = x0(4); //last of the state is Mz
593               
594                //teh first 4 states are same as before (given that Mz is set)
595                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
596                        return concat(x,Mz);
597                }
598                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
599                //call initial
600                        if (full) A.clear();
601                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
602                        A(2,4)=- p/J*dt;
603                        A(4,4)=1.0;
604                }       
605};
606
607UIREGISTER ( IMpmsmStatMz );
608
609
610//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
611class OMpmsm: public diffbifn {
612public:
613        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
614
615        vec eval ( const vec &x0, const vec &u0 ) {
616                vec y ( 2 );
617                y ( 0 ) = x0 ( 0 );
618                y ( 1 ) = x0 ( 1 );
619                return y;
620        }
621
622        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
623                A.clear();
624                A ( 0,0 ) = 1.0;
625                A ( 1,1 ) = 1.0;
626        }
627};
628
629UIREGISTER ( OMpmsm );
630
631//! Observation model for PMSM drive with roundoff-errors
632class OMpmsmRO: public diffbifn {
633        public:
634                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
635               
636                vec eval ( const vec &x0, const vec &u0 ) {
637                        vec y ( 2 );
638/*                      y ( 0 ) = x0 ( 0 );
639                        y ( 1 ) = x0 ( 1 );*/
640                       
641                        double istep=0.085;
642                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
643                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
644                       
645                        return y;
646                }
647               
648                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
649                        A.clear();
650                        A ( 0,0 ) = 1.0;
651                        A ( 1,1 ) = 1.0;
652                }
653};
654
655UIREGISTER ( OMpmsmRO );
656
657class OMpmsmROMz: public OMpmsmRO{
658public:
659        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
660};
661UIREGISTER ( OMpmsmROMz );
662
663//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
664class OMpmsm4: public diffbifn {
665        public:
666                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
667
668                vec eval ( const vec &x0, const vec &u0 ) {
669                        vec y ( 4 );
670                        y  = x0 ;
671                        return y;
672                }
673
674                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
675                        A.clear();
676                        A(0,0)=1.0;
677                        A(1,1)=1.0;
678                }
679};
680
681UIREGISTER ( OMpmsm4 );
682
683//! Observation model for PMSM drive id d-q coordinates
684class OMpmsmDQ: public diffbifn {
685public:
686        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
687       
688        vec eval ( const vec &x0, const vec &u0 ) {
689                vec y ( 2 );
690                double ct = cos(x0(3));
691                double st = sin(x0(3));
692                y(0)  = ct*x0(0)-st*x0(1);
693                y(1)  = st*x0(0)+ct*x0(1);
694               
695                return y;
696        }
697       
698        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
699                double ct = cos(x0(3));
700                double st = sin(x0(3));
701                A(0,0) = ct;
702                A(1,0) = -st;
703                A(0,1) = st;
704                A(1,1) = ct;
705               
706                A(0,2) = 0.0; 
707                A(1,2) = 0.0;
708                A(0,3) = -st*x0(0)-ct*x0(1);
709                A(1,3) = ct*x0(0)-st*x0(1);
710        }
711};
712
713UIREGISTER ( OMpmsmDQ );
714
715//! Observation model for PMSM drive in reduced form coordinates
716class OMpmsmOT: public diffbifn {
717public:
718        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
719       
720        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;}
721       
722        OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
723       
724        vec eval ( const vec &x0, const vec &u0 ) {
725                vec y ( 2 );
726                const double &omm = x0(0);
727                const double &thm = x0(1);
728               
729                const double &uam = u0 ( 0 );
730                const double &ubm = u0 ( 1 );
731                const double &iam = u0 ( 2 );
732                const double &ibm = u0 ( 3 );
733               
734                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
735                //ib
736                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
737               
738                return y;
739        }
740       
741        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
742                const double &omm = x0(0);
743                const double &thm = x0(1);
744                               
745                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
746                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
747                // d ib
748                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
749                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
750               
751               
752        }
753
754void from_setting( const Setting &root )
755{       
756       
757        const SettingResolver& params_b=root["params"];
758        const Setting& params=params_b.result;
759       
760        set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
761        params["kp"], params["p"], params["J"], 0.0);
762};
763
764};
765
766UIREGISTER ( OMpmsmOT );
767
768//! Observation model for PMSM drive in reduced form coordinates -- scaled to [-1,1]
769class OMpmsmOTsc: public diffbifn {
770public:
771        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
772       
773        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;}
774       
775        OMpmsmOTsc() :diffbifn () {dimy=2;dimx=2;dimu=4;};
776       
777        vec eval ( const vec &x0, const vec &u0 ) {
778                vec y ( 2 );
779                const double &omm = x0(0);
780                const double &thm = x0(1);
781               
782                const double &uam = u0 ( 0 );
783                const double &ubm = u0 ( 1 );
784                const double &iam = u0 ( 2 );
785                const double &ibm = u0 ( 3 );
786               
787                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm *Wref/Iref* sin ( thm*Thetaref/(1<<15) ) + (1<<15)*uam*dt/Ls /Iref;
788                //ib
789                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm *Wref/Iref* cos ( thm*Thetaref/(1<<15) ) + (1<<15)*ubm*dt/Ls /Iref;
790               
791                return y;
792        }
793       
794        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
795                const double &omm = x0(0);
796                const double &thm = x0(1);
797               
798                A ( 0,0 ) = Ypm/Ls*dt* Wref/Iref*sin ( thm*Thetaref/(1<<15) ); 
799                A ( 0,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( cos ( thm*Thetaref/(1<<15) ) );
800                // d ib
801                A ( 1,0 ) = -Ypm/Ls*dt* Wref/Iref*cos ( thm*Thetaref/(1<<15) ); 
802                A ( 1,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( sin ( thm*Thetaref/(1<<15) ) );
803               
804               
805        }
806       
807        void from_setting( const Setting &root )
808        {       
809               
810                const SettingResolver& params_b=root["params"];
811                const Setting& params=params_b.result;
812               
813                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
814                params["kp"], params["p"], params["J"], 0.0);
815        };
816       
817};
818
819UIREGISTER ( OMpmsmOTsc );
820
821
822/*!@}*/
823#endif //PMSM_H
Note: See TracBrowser for help on using the browser.