root/applications/pmsm/pmsm.h @ 1356

Revision 1341, 27.6 kB (checked in by smidl, 14 years ago)

extended reduced model for unknown Torque

  • 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=false;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=false;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$
227class IMpmsmOTM : public diffbifn {
228protected:
229        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
230       
231        bool compensate;
232        bool cutoff;
233public:
234        IMpmsmOTM() :diffbifn ( ) {dimy=2; dimx = 3; dimu=4; dimc=6;compensate=false;cutoff=true;};
235        //! Set mechanical and electrical variables
236        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;}
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 &omm = x0 ( 0 );
260                const double &thm = x0 ( 1 );
261                const double &Mm  = x0 ( 2 );
262                double uam;
263                double ubm;
264               
265                if (compensate){
266                        modelpwm(x0,u0,uam,ubm);
267                } else {
268                        uam = u0(0);
269                        ubm = u0(1);
270                }
271               
272                const double &iam = u0 ( 2 );
273                const double &ibm = u0 ( 3 );
274               
275                vec xk( 3 );
276                //ia
277                //om
278                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mm;
279                //th
280                xk ( 1 ) = thm + omm*dt; // <0..2pi>
281                xk (2) = Mm;
282               
283                if (cutoff) {
284                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
285                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
286                }
287                return xk;
288        }
289       
290        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
291//              const double &omm = x0 ( 0 );
292                const double &thm = x0 ( 1 );
293//              const double &Mm  = x0 ( 2 );
294               
295                const double &iam = u0 ( 2 );
296                const double &ibm = u0 ( 3 );
297               
298                // d ia
299                // d om
300                A ( 0,0 ) = 1.0;
301                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
302                A ( 0,2 ) = -p/J*dt;
303                // d th
304                A ( 1,0 ) = dt; 
305                A ( 1,1 ) = 1.0;
306                A ( 1,2 ) = 0.0;
307                // d M
308                A ( 2,0 ) = 0.0;
309                A ( 2,1 ) = 0.0;
310                A ( 2,2 ) = 1.0;
311        }
312       
313        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
314       
315        void from_setting( const Setting &root )
316        {       
317               
318                const SettingResolver& params_b=root["params"];
319                const Setting& params=params_b.result;
320               
321                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
322                params["kp"], params["p"], params["J"], 0.0);
323                int comp=0;
324                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
325                int cuto=0;
326                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
327        };
328       
329        // TODO dodelat void to_setting( Setting &root ) const;
330};
331
332UIREGISTER ( IMpmsmOTM );
333
334//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ -- Scaled to [-1,1]
335class IMpmsmOTsc : public diffbifn {
336protected:
337        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
338       
339        bool cutoff;
340public:
341        IMpmsmOTsc() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;cutoff=true;};
342        //! Set mechanical and electrical variables
343        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;}
344       
345       
346        vec eval ( const vec &x0, const vec &u0 ) {
347                // last state
348                const double &omm = x0 ( 0 );
349                const double &thm = x0 ( 1 );
350                double uam;
351                double ubm;
352               
353                uam = u0(0);
354                ubm = u0(1);
355               
356                const double &iam = u0 ( 2 );
357                const double &ibm = u0 ( 3 );
358               
359                vec xk( 2 );
360                //ia
361                //om
362                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt *Iref/Wref* ( ibm * cos ( thm*Thetaref/(1<<15) )-iam * sin ( thm*Thetaref/(1<<15) ) );
363                //th
364                xk ( 1 ) = thm + omm*Wref/Thetaref*dt; // <0..2pi>
365                if (cutoff) {
366                        if ( xk ( 1 ) >(1<<15) ) xk ( 1 )=-(1<<15);
367                        if ( xk ( 1 ) <-(1<<15) ) xk ( 1 ) =(1<<15);
368                }
369                return xk;
370        }
371       
372        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
373                const double &omm = x0 ( 0 );
374                const double &thm = x0 ( 1 );
375               
376                const double &iam = u0 ( 2 );
377                const double &ibm = u0 ( 3 );
378               
379                // d ia
380                // d om
381                A ( 0,0 ) = 1.0;
382                A ( 0,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
383                // d th
384                A ( 1,0 ) = dt*Wref/Thetaref; 
385                A ( 1,1 ) = 1.0;
386        }
387       
388        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
389       
390        void from_setting( const Setting &root )
391        {       
392               
393                const SettingResolver& params_b=root["params"];
394                const Setting& params=params_b.result;
395               
396                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
397                params["kp"], params["p"], params["J"], 0.0);
398                int cuto=0;
399                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
400        };
401       
402        // TODO dodelat void to_setting( Setting &root ) const;
403};
404
405UIREGISTER ( IMpmsmOTsc );
406
407
408//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
409class IMpmsmDQ : public diffbifn {
410protected:
411        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
412       
413        bool compensate;
414        bool cutoff;
415public:
416        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
417        //! Set mechanical and electrical variables
418        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;}
419       
420        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
421                /*              ua=u0[0];
422                 *              ub=u0[1];
423                 *              return;*/
424                double sq3=sqrt ( 3.0 );
425                double i1=x0(0);
426                double i2=0.5* ( -i1+sq3*x0[1] );
427                double i3=0.5* ( -i1-sq3*x0[1] );
428                double u1=u0(0);
429                double u2=0.5* ( -u1+sq3*u0(1) );
430                double u3=0.5* ( -u1-sq3*u0(1) );
431               
432                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
433                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
434                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
435                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
436                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
437        }
438       
439        vec eval ( const vec &x0, const vec &u0 ) {
440                // last state
441                const double &idm = x0 ( 0 );
442                const double &iqm = x0 ( 1 );
443                const double &omm = x0 ( 2 );
444                const double &thm = x0 ( 3 );
445                double uam;
446                double ubm;
447               
448                if (compensate){
449                        modelpwm(x0,u0,uam,ubm);
450                } else {
451                        uam = u0(0);
452                        ubm = u0(1);
453                }
454               
455                vec xk( 4 );
456                //id
457                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
458                //iq
459                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);
460                //om
461                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
462                //th
463                xk ( 3 ) = thm + omm*dt; // <0..2pi>
464                if (cutoff) {
465                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
466                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
467                }
468                return xk;
469        }
470       
471        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
472                const double &idm = x0 ( 0 );
473                const double &iqm = x0 ( 1 );
474                const double &omm = x0 ( 2 );
475                const double &thm = x0 ( 3 );
476               
477                double uam;
478                double ubm;
479                if (compensate){
480                        modelpwm(x0,u0,uam,ubm);
481                } else {
482                        uam = u0(0);
483                        ubm = u0(1);
484                }
485                // d id
486                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
487                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
488                // d iq
489                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
490                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
491                // d om
492                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
493                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
494                A ( 2,2 ) = 1.0;
495                A ( 2,3 ) = 0;
496                // d th
497                A ( 3,0 ) = 0.0; 
498                A ( 3,1 ) = 0.0; 
499                A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
500        }
501       
502        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
503       
504        void from_setting( const Setting &root )
505        {       
506               
507                const SettingResolver& params_b=root["params"];
508                const Setting& params=params_b.result;
509               
510                set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
511                params["kp"], params["p"], params["J"], 0.0);
512                int comp=0;
513                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
514                int cuto=0;
515                if (UI::get(cuto,root,"cutoff",UI::optional)){
516                        cutoff=(cuto==1);
517                }
518        };
519       
520        // TODO dodelat void to_setting( Setting &root ) const;
521};
522
523UIREGISTER ( IMpmsmDQ );
524
525
526//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
527class IMpmsm2o : public IMpmsm {
528        protected:
529//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
530                //! store first derivatives for the use in second derivatives
531                double dia, dib, dom, dth;
532                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
533                double d2t, cth, sth;
534                double iam, ibm, omm, thm, uam, ubm;
535        public:
536                IMpmsm2o() :IMpmsm () {};
537        //! Set mechanical and electrical variables
538                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;}
539
540                vec eval ( const vec &x0, const vec &u0 ) {
541                // last state
542                        iam = x0 ( 0 );
543                        ibm = x0 ( 1 );
544                        omm = x0 ( 2 );
545                        thm = x0 ( 3 );
546                        uam = u0 ( 0 );
547                        ubm = u0 ( 1 );
548
549                        cth = cos(thm);
550                        sth = sin(thm);
551                       
552                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
553                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
554                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
555                        dth = omm;
556                                               
557                        vec xk=zeros ( 4 );
558                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
559                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
560                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
561                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
562                       
563                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
564                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
565                        return xk;
566                }
567
568                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
569                vec eval2o(const vec &du){
570                        double dua = du ( 0 )/dt;
571                        double dub = du ( 1 )/dt;
572                       
573                        vec xth2o(4);
574                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
575                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
576                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
577                        xth2o(3) = dom;
578                        // multiply by dt^2/2
579                        xth2o*=d2t/2;
580                        return xth2o;
581                }
582                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
583                         iam = x0 ( 0 );
584                         ibm = x0 ( 1 );
585                         omm = x0 ( 2 );
586                         thm = x0 ( 3 );
587                // d ia
588                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
589                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
590                // d ib
591                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
592                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
593                // d om
594                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
595                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
596                        A ( 2,2 ) = 1.0;
597                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
598                // d th
599                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
600                        // FOR d2t*dom!!!!!!!!!
601/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
602                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
603                        A ( 3,2 ) = dt;
604                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
605                }
606
607                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
608
609};
610
611
612UIREGISTER ( IMpmsm2o );
613
614//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
615class IMpmsmStat : public IMpmsm {
616        public:
617        IMpmsmStat() :IMpmsm() {};
618        //! Set mechanical and electrical variables
619        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;}
620
621        vec eval ( const vec &x0, const vec &u0 ) {
622                // last state
623                double iam = x0 ( 0 );
624                double ibm = x0 ( 1 );
625                double omm = x0 ( 2 );
626                double thm = x0 ( 3 );
627                double uam = u0 ( 0 );
628                double ubm = u0 ( 1 );
629
630                vec xk=zeros ( 4 );
631                //ia
632                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
633                //ib
634                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
635                //om
636                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
637                //th
638                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
639                return xk;
640        }
641
642        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
643//              double iam = x0 ( 0 );
644//              double ibm = x0 ( 1 );
645                double omm = x0 ( 2 );
646                double thm = x0 ( 3 );
647                // d ia
648                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
649                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
650                // d ib
651                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
652                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
653                // d om
654                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
655                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
656                A ( 2,2 ) = 1.0;
657                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
658                // d th
659                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
660        }
661
662        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
663
664};
665
666UIREGISTER ( IMpmsmStat );
667
668
669//! State for PMSM with unknown Mz
670class IMpmsmMz: public IMpmsm{
671        public:
672                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
673        //! extend eval by Mz
674                vec eval ( const vec &x0, const vec &u0 ) {
675                        vec x(4);
676                        Mz = x0(4); //last of the state is Mz
677               
678                //teh first 4 states are same as before (given that Mz is set)
679                        x=IMpmsm::eval(x0,u0); // including model of drops!
680                        return concat(x,Mz);
681                }
682                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
683                //call initial
684                        if (full) A.clear();
685                        IMpmsm::dfdx_cond(x0,u0,A,full);
686                        A(2,4)=- p/J*dt;
687                        A(4,4)=1.0;
688                }       
689};
690
691UIREGISTER ( IMpmsmMz );
692
693//! State for PMSM with unknown Mz
694class IMpmsmStatMz: public IMpmsmStat{
695        public:
696                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
697        //! extend eval by Mz
698                vec eval ( const vec &x0, const vec &u0 ) {
699                        vec x(4);
700                        Mz = x0(4); //last of the state is Mz
701               
702                //teh first 4 states are same as before (given that Mz is set)
703                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
704                        return concat(x,Mz);
705                }
706                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
707                //call initial
708                        if (full) A.clear();
709                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
710                        A(2,4)=- p/J*dt;
711                        A(4,4)=1.0;
712                }       
713};
714
715UIREGISTER ( IMpmsmStatMz );
716
717
718//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
719class OMpmsm: public diffbifn {
720public:
721        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
722
723        vec eval ( const vec &x0, const vec &u0 ) {
724                vec y ( 2 );
725                y ( 0 ) = x0 ( 0 );
726                y ( 1 ) = x0 ( 1 );
727                return y;
728        }
729
730        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
731                A.clear();
732                A ( 0,0 ) = 1.0;
733                A ( 1,1 ) = 1.0;
734        }
735};
736
737UIREGISTER ( OMpmsm );
738
739//! Observation model for PMSM drive with roundoff-errors
740class OMpmsmRO: public diffbifn {
741        public:
742                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
743               
744                vec eval ( const vec &x0, const vec &u0 ) {
745                        vec y ( 2 );
746/*                      y ( 0 ) = x0 ( 0 );
747                        y ( 1 ) = x0 ( 1 );*/
748                       
749                        double istep=0.085;
750                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
751                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
752                       
753                        return y;
754                }
755               
756                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
757                        A.clear();
758                        A ( 0,0 ) = 1.0;
759                        A ( 1,1 ) = 1.0;
760                }
761};
762
763UIREGISTER ( OMpmsmRO );
764
765class OMpmsmROMz: public OMpmsmRO{
766public:
767        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
768};
769UIREGISTER ( OMpmsmROMz );
770
771//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
772class OMpmsm4: public diffbifn {
773        public:
774                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
775
776                vec eval ( const vec &x0, const vec &u0 ) {
777                        vec y ( 4 );
778                        y  = x0 ;
779                        return y;
780                }
781
782                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
783                        A.clear();
784                        A(0,0)=1.0;
785                        A(1,1)=1.0;
786                }
787};
788
789UIREGISTER ( OMpmsm4 );
790
791//! Observation model for PMSM drive id d-q coordinates
792class OMpmsmDQ: public diffbifn {
793public:
794        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
795       
796        vec eval ( const vec &x0, const vec &u0 ) {
797                vec y ( 2 );
798                double ct = cos(x0(3));
799                double st = sin(x0(3));
800                y(0)  = ct*x0(0)-st*x0(1);
801                y(1)  = st*x0(0)+ct*x0(1);
802               
803                return y;
804        }
805       
806        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
807                double ct = cos(x0(3));
808                double st = sin(x0(3));
809                A(0,0) = ct;
810                A(1,0) = -st;
811                A(0,1) = st;
812                A(1,1) = ct;
813               
814                A(0,2) = 0.0; 
815                A(1,2) = 0.0;
816                A(0,3) = -st*x0(0)-ct*x0(1);
817                A(1,3) = ct*x0(0)-st*x0(1);
818        }
819};
820
821UIREGISTER ( OMpmsmDQ );
822
823//! Observation model for PMSM drive in reduced form coordinates
824class OMpmsmOT: public diffbifn {
825public:
826        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
827       
828        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;}
829       
830        OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
831       
832        vec eval ( const vec &x0, const vec &u0 ) {
833                vec y ( 2 );
834                const double &omm = x0(0);
835                const double &thm = x0(1);
836               
837                const double &uam = u0 ( 0 );
838                const double &ubm = u0 ( 1 );
839                const double &iam = u0 ( 2 );
840                const double &ibm = u0 ( 3 );
841               
842                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
843                //ib
844                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
845               
846                return y;
847        }
848       
849        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
850                const double &omm = x0(0);
851                const double &thm = x0(1);
852                               
853                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
854                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
855                // d ib
856                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
857                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
858               
859               
860        }
861
862void from_setting( const Setting &root )
863{       
864       
865        const SettingResolver& params_b=root["params"];
866        const Setting& params=params_b.result;
867       
868        set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
869        params["kp"], params["p"], params["J"], 0.0);
870};
871
872};
873
874UIREGISTER ( OMpmsmOT );
875
876//! Observation model for PMSM drive in reduced form coordinates
877class OMpmsmOTM: public diffbifn {
878public:
879        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
880       
881        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;}
882       
883        OMpmsmOTM() :diffbifn () {dimy=2;dimx=2;dimu=4;};
884       
885        vec eval ( const vec &x0, const vec &u0 ) {
886                vec y ( 2 );
887                const double &omm = x0(0);
888                const double &thm = x0(1);
889               
890                const double &uam = u0 ( 0 );
891                const double &ubm = u0 ( 1 );
892                const double &iam = u0 ( 2 );
893                const double &ibm = u0 ( 3 );
894               
895                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
896                //ib
897                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
898               
899                return y;
900        }
901       
902        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
903                const double &omm = x0(0);
904                const double &thm = x0(1);
905               
906                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
907                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
908                A( 0,2) = 0.0;
909                // d ib
910                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
911                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
912                A(1,2) = 0.0;
913               
914        }
915       
916        void from_setting( const Setting &root )
917        {       
918               
919                const SettingResolver& params_b=root["params"];
920                const Setting& params=params_b.result;
921               
922                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
923                params["kp"], params["p"], params["J"], 0.0);
924        };
925       
926};
927
928UIREGISTER ( OMpmsmOTM );
929
930
931
932//! Observation model for PMSM drive in reduced form coordinates -- scaled to [-1,1]
933class OMpmsmOTsc: public diffbifn {
934public:
935        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
936       
937        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;}
938       
939        OMpmsmOTsc() :diffbifn () {dimy=2;dimx=2;dimu=4;};
940       
941        vec eval ( const vec &x0, const vec &u0 ) {
942                vec y ( 2 );
943                const double &omm = x0(0);
944                const double &thm = x0(1);
945               
946                const double &uam = u0 ( 0 );
947                const double &ubm = u0 ( 1 );
948                const double &iam = u0 ( 2 );
949                const double &ibm = u0 ( 3 );
950               
951                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;
952                //ib
953                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;
954               
955                return y;
956        }
957       
958        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
959                const double &omm = x0(0);
960                const double &thm = x0(1);
961               
962                A ( 0,0 ) = Ypm/Ls*dt* Wref/Iref*sin ( thm*Thetaref/(1<<15) ); 
963                A ( 0,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( cos ( thm*Thetaref/(1<<15) ) );
964                // d ib
965                A ( 1,0 ) = -Ypm/Ls*dt* Wref/Iref*cos ( thm*Thetaref/(1<<15) ); 
966                A ( 1,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( sin ( thm*Thetaref/(1<<15) ) );
967               
968               
969        }
970       
971        void from_setting( const Setting &root )
972        {       
973               
974                const SettingResolver& params_b=root["params"];
975                const Setting& params=params_b.result;
976               
977                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
978                params["kp"], params["p"], params["J"], 0.0);
979        };
980       
981};
982
983UIREGISTER ( OMpmsmOTsc );
984
985
986/*!@}*/
987#endif //PMSM_H
Note: See TracBrowser for help on using the browser.