root/applications/pmsm/pmsm.h @ 1466

Revision 1403, 28.7 kB (checked in by vahalam, 13 years ago)
  • 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
23inline double va_drop_interp(double i){
24        //double va_char[16]={0,10,50,100,200,300,500,1000, 0,1,1.8,2.4,3.2,3.8,4.8,6.8};
25        //*
26        double si = sign(i);
27        i = abs(i);
28        double du;
29               
30        if(i < 0.1) du = 5.0*i;
31     else if(i < 0.5) du = 1.0*(i-0.1) + 0.5;
32     else if(i < 1.0) du = 0.6*(i-0.5) + 0.9;
33     else if(i < 2.0) du = 0.4*(i-1.0) + 1.2;
34     else if(i < 3.0) du = 0.3*(i-2.0) + 1.6;
35     else if(i < 5.0) du = 0.25*(i-3.0) + 1.9;                         
36     else du = 0.2*(i - 5.0) + 2.4;
37       
38        return si*du*1.0;
39       
40        /*/
41        double msi = sign(i);
42       
43        i = abs(i) - 0.3;
44       
45        double si = sign(i);
46        i = abs(i);
47        double du;
48       
49        if(i < 0.0654) du = 5.0*i;
50        else if(i < 0.3225) du = 1.0*(i-0.0654) + 0.3225;
51        else if(i < 0.654) du = 0.6*(i-0.3225) + 0.5805;
52        else if(i < 1.29) du = 0.4*(i-0.654) + 0.774;
53        else if(i < 1.935) du = 0.3*(i-1.29) + 1.032;
54        else if(i < 3.225) du = 0.25*(i-1.935) + 1.2255;                               
55        else du = 0.2*(i - 3.225) + 1.548;
56               
57        return msi*(si*du+0.558);
58        /*/
59}
60
61//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
62class IMpmsm : public diffbifn {
63protected:
64        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
65
66        bool compensate;
67        bool cutoff;
68public:
69        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=false;cutoff=true;};
70        //! Set mechanical and electrical variables
71        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;}
72
73        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
74/*              ua=u0[0];
75                ub=u0[1];
76                return;*/
77                double sq3=sqrt ( 3.0 );
78                double i1=x0(0);
79                double i2=0.5* ( -i1+sq3*x0[1] );
80                double i3=0.5* ( -i1-sq3*x0[1] );
81                double u1=u0(0);
82                double u2=0.5* ( -u1+sq3*u0(1) );
83                double u3=0.5* ( -u1-sq3*u0(1) );
84                //
85                double du1=va_drop_interp(i1);
86                double du2=va_drop_interp(i2);
87                double du3=va_drop_interp(i3);
88                /*/
89                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
90                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
91                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
92                */
93                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
94                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
95        }
96
97        vec eval ( const vec &x0, const vec &u0 ) {
98                // last state
99                const double &iam = x0 ( 0 );
100                const double &ibm = x0 ( 1 );
101                const double &omm = x0 ( 2 );
102                const double &thm = x0 ( 3 );
103                double uam;
104                double ubm;
105
106                if (compensate){
107                        modelpwm(x0,u0,uam,ubm);
108                } else {
109                        uam = u0(0);
110                        ubm = u0(1);
111                }
112               
113               
114                vec xk( 4 );
115                //ia
116                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
117                //ib
118                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
119                //om
120                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
121                //th
122                xk ( 3 ) = thm + omm*dt; // <0..2pi>
123                if (cutoff) {
124                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
125                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
126                }
127                return xk;
128        }
129
130        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
131                const double &iam = x0 ( 0 );
132                const double &ibm = x0 ( 1 );
133                const double &omm = x0 ( 2 );
134                const double &thm = x0 ( 3 );
135                // d ia
136                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
137                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
138                // d ib
139                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
140                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
141                // d om
142                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
143                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
144                A ( 2,2 ) = 1.0;
145                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
146                // d th
147                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
148        }
149
150        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
151
152        void from_setting( const Setting &root )
153        {       
154
155                const SettingResolver& params_b=root["params"];
156                const Setting& params=params_b.result;
157               
158                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
159                         params["kp"], params["p"], params["J"], 0.0);
160                int comp=0;
161                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
162                int cuto=0;
163                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
164        };
165
166        // TODO dodelat void to_setting( Setting &root ) const;
167};
168
169UIREGISTER ( IMpmsm );
170
171//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
172class IMpmsmOT : public diffbifn {
173protected:
174        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
175       
176        bool compensate;
177        bool cutoff;
178public:
179        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=false;cutoff=true;};
180        //! Set mechanical and electrical variables
181        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;}
182       
183        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
184                /*              ua=u0[0];
185                 *              ub=u0[1];
186                 *              return;*/
187                double sq3=sqrt ( 3.0 );
188                double i1=x0(0);
189                double i2=0.5* ( -i1+sq3*x0[1] );
190                double i3=0.5* ( -i1-sq3*x0[1] );
191                double u1=u0(0);
192                double u2=0.5* ( -u1+sq3*u0(1) );
193                double u3=0.5* ( -u1-sq3*u0(1) );
194               
195                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
196                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
197                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
198                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
199                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
200        }
201       
202        vec eval ( const vec &x0, const vec &u0 ) {
203                // last state
204                const double &omm = x0 ( 0 );
205                const double &thm = x0 ( 1 );
206                double uam;
207                double ubm;
208               
209                if (compensate){
210                        modelpwm(x0,u0,uam,ubm);
211                } else {
212                        uam = u0(0);
213                        ubm = u0(1);
214                }
215               
216                const double &iam = u0 ( 2 );
217                const double &ibm = u0 ( 3 );
218               
219                vec xk( 2 );
220                //ia
221                //om
222                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
223                //th
224                xk ( 1 ) = thm + omm*dt; // <0..2pi>
225                if (cutoff) {
226                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
227                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
228                }
229                return xk;
230        }
231       
232        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
233//              const double &omm = x0 ( 0 );
234                const double &thm = x0 ( 1 );
235
236                const double &iam = u0 ( 2 );
237                const double &ibm = u0 ( 3 );
238               
239                // d ia
240                // d om
241                A ( 0,0 ) = 1.0;
242                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
243                // d th
244                A ( 1,0 ) = dt; 
245                A ( 1,1 ) = 1.0;
246        }
247       
248        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
249       
250        void from_setting( const Setting &root )
251        {       
252               
253                const SettingResolver& params_b=root["params"];
254                const Setting& params=params_b.result;
255               
256                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
257                params["kp"], params["p"], params["J"], 0.0);
258                int comp=0;
259                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
260                int cuto=0;
261                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
262        };
263       
264        // TODO dodelat void to_setting( Setting &root ) const;
265};
266
267UIREGISTER ( IMpmsmOT );
268
269//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
270class IMpmsmOTM : public diffbifn {
271protected:
272        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
273       
274        bool compensate;
275        bool cutoff;
276public:
277        IMpmsmOTM() :diffbifn ( ) {dimy=2; dimx = 3; dimu=4; dimc=6;compensate=false;cutoff=true;};
278        //! Set mechanical and electrical variables
279        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;}
280       
281        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
282                /*              ua=u0[0];
283                 *              ub=u0[1];
284                 *              return;*/
285                double sq3=sqrt ( 3.0 );
286                double i1=x0(0);
287                double i2=0.5* ( -i1+sq3*x0[1] );
288                double i3=0.5* ( -i1-sq3*x0[1] );
289                double u1=u0(0);
290                double u2=0.5* ( -u1+sq3*u0(1) );
291                double u3=0.5* ( -u1-sq3*u0(1) );
292               
293                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
294                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
295                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
296                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
297                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
298        }
299       
300        vec eval ( const vec &x0, const vec &u0 ) {
301                // last state
302                const double &omm = x0 ( 0 );
303                const double &thm = x0 ( 1 );
304                const double &Mm  = x0 ( 2 );
305                double uam;
306                double ubm;
307               
308                if (compensate){
309                        modelpwm(x0,u0,uam,ubm);
310                } else {
311                        uam = u0(0);
312                        ubm = u0(1);
313                }
314               
315                const double &iam = u0 ( 2 );
316                const double &ibm = u0 ( 3 );
317               
318                vec xk( 3 );
319                //ia
320                //om
321                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mm;
322                //th
323                xk ( 1 ) = thm + omm*dt; // <0..2pi>
324                xk (2) = Mm;
325               
326                if (cutoff) {
327                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
328                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
329                }
330                return xk;
331        }
332       
333        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
334//              const double &omm = x0 ( 0 );
335                const double &thm = x0 ( 1 );
336//              const double &Mm  = x0 ( 2 );
337               
338                const double &iam = u0 ( 2 );
339                const double &ibm = u0 ( 3 );
340               
341                // d ia
342                // d om
343                A ( 0,0 ) = 1.0;
344                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
345                A ( 0,2 ) = -p/J*dt;
346                // d th
347                A ( 1,0 ) = dt; 
348                A ( 1,1 ) = 1.0;
349                A ( 1,2 ) = 0.0;
350                // d M
351                A ( 2,0 ) = 0.0;
352                A ( 2,1 ) = 0.0;
353                A ( 2,2 ) = 1.0;
354        }
355       
356        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
357       
358        void from_setting( const Setting &root )
359        {       
360               
361                const SettingResolver& params_b=root["params"];
362                const Setting& params=params_b.result;
363               
364                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
365                params["kp"], params["p"], params["J"], 0.0);
366                int comp=0;
367                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
368                int cuto=0;
369                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
370        };
371       
372        // TODO dodelat void to_setting( Setting &root ) const;
373};
374
375UIREGISTER ( IMpmsmOTM );
376
377//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ -- Scaled to [-1,1]
378class IMpmsmOTsc : public diffbifn {
379protected:
380        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
381       
382        bool cutoff;
383public:
384        IMpmsmOTsc() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;cutoff=true;};
385        //! Set mechanical and electrical variables
386        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;}
387       
388       
389        vec eval ( const vec &x0, const vec &u0 ) {
390                // last state
391                const double &omm = x0 ( 0 );
392                const double &thm = x0 ( 1 );
393                double uam;
394                double ubm;
395               
396                uam = u0(0);
397                ubm = u0(1);
398               
399                const double &iam = u0 ( 2 );
400                const double &ibm = u0 ( 3 );
401               
402                vec xk( 2 );
403                //ia
404                //om
405                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt *Iref/Wref* ( ibm * cos ( thm*Thetaref/(1<<15) )-iam * sin ( thm*Thetaref/(1<<15) ) );
406                //th
407                xk ( 1 ) = thm + omm*Wref/Thetaref*dt; // <0..2pi>
408                if (cutoff) {
409                        if ( xk ( 1 ) >(1<<15) ) xk ( 1 )=-(1<<15);
410                        if ( xk ( 1 ) <-(1<<15) ) xk ( 1 ) =(1<<15);
411                }
412                return xk;
413        }
414       
415        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
416                const double &omm = x0 ( 0 );
417                const double &thm = x0 ( 1 );
418               
419                const double &iam = u0 ( 2 );
420                const double &ibm = u0 ( 3 );
421               
422                // d ia
423                // d om
424                A ( 0,0 ) = 1.0;
425                A ( 0,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
426                // d th
427                A ( 1,0 ) = dt*Wref/Thetaref; 
428                A ( 1,1 ) = 1.0;
429        }
430       
431        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
432       
433        void from_setting( const Setting &root )
434        {       
435               
436                const SettingResolver& params_b=root["params"];
437                const Setting& params=params_b.result;
438               
439                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
440                params["kp"], params["p"], params["J"], 0.0);
441                int cuto=0;
442                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
443        };
444       
445        // TODO dodelat void to_setting( Setting &root ) const;
446};
447
448UIREGISTER ( IMpmsmOTsc );
449
450
451//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
452class IMpmsmDQ : public diffbifn {
453protected:
454        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
455       
456        bool compensate;
457        bool cutoff;
458public:
459        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
460        //! Set mechanical and electrical variables
461        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;}
462       
463        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
464                /*              ua=u0[0];
465                 *              ub=u0[1];
466                 *              return;*/
467                double sq3=sqrt ( 3.0 );
468                double i1=x0(0);
469                double i2=0.5* ( -i1+sq3*x0[1] );
470                double i3=0.5* ( -i1-sq3*x0[1] );
471                double u1=u0(0);
472                double u2=0.5* ( -u1+sq3*u0(1) );
473                double u3=0.5* ( -u1-sq3*u0(1) );
474               
475                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
476                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
477                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
478                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
479                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
480        }
481       
482        vec eval ( const vec &x0, const vec &u0 ) {
483                // last state
484                const double &idm = x0 ( 0 );
485                const double &iqm = x0 ( 1 );
486                const double &omm = x0 ( 2 );
487                const double &thm = x0 ( 3 );
488                double uam;
489                double ubm;
490               
491                if (compensate){
492                        modelpwm(x0,u0,uam,ubm);
493                } else {
494                        uam = u0(0);
495                        ubm = u0(1);
496                }
497               
498                vec xk( 4 );
499                //id
500                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
501                //iq
502                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);
503                //om
504                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
505                //th
506                xk ( 3 ) = thm + omm*dt; // <0..2pi>
507                if (cutoff) {
508                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
509                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
510                }
511                return xk;
512        }
513       
514        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
515                const double &idm = x0 ( 0 );
516                const double &iqm = x0 ( 1 );
517                const double &omm = x0 ( 2 );
518                const double &thm = x0 ( 3 );
519               
520                double uam;
521                double ubm;
522                if (compensate){
523                        modelpwm(x0,u0,uam,ubm);
524                } else {
525                        uam = u0(0);
526                        ubm = u0(1);
527                }
528                // d id
529                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
530                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
531                // d iq
532                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
533                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
534                // d om
535                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
536                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
537                A ( 2,2 ) = 1.0;
538                A ( 2,3 ) = 0;
539                // d th
540                A ( 3,0 ) = 0.0; 
541                A ( 3,1 ) = 0.0; 
542                A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
543        }
544       
545        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
546       
547        void from_setting( const Setting &root )
548        {       
549               
550                const SettingResolver& params_b=root["params"];
551                const Setting& params=params_b.result;
552               
553                set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
554                params["kp"], params["p"], params["J"], 0.0);
555                int comp=0;
556                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
557                int cuto=0;
558                if (UI::get(cuto,root,"cutoff",UI::optional)){
559                        cutoff=(cuto==1);
560                }
561        };
562       
563        // TODO dodelat void to_setting( Setting &root ) const;
564};
565
566UIREGISTER ( IMpmsmDQ );
567
568
569//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
570class IMpmsm2o : public IMpmsm {
571        protected:
572//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
573                //! store first derivatives for the use in second derivatives
574                double dia, dib, dom, dth;
575                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
576                double d2t, cth, sth;
577                double iam, ibm, omm, thm, uam, ubm;
578        public:
579                IMpmsm2o() :IMpmsm () {};
580        //! Set mechanical and electrical variables
581                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;}
582
583                vec eval ( const vec &x0, const vec &u0 ) {
584                // last state
585                        iam = x0 ( 0 );
586                        ibm = x0 ( 1 );
587                        omm = x0 ( 2 );
588                        thm = x0 ( 3 );
589                        uam = u0 ( 0 );
590                        ubm = u0 ( 1 );
591
592                        cth = cos(thm);
593                        sth = sin(thm);
594                       
595                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
596                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
597                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
598                        dth = omm;
599                                               
600                        vec xk=zeros ( 4 );
601                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
602                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
603                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
604                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
605                       
606                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
607                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
608                        return xk;
609                }
610
611                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
612                vec eval2o(const vec &du){
613                        double dua = du ( 0 )/dt;
614                        double dub = du ( 1 )/dt;
615                       
616                        vec xth2o(4);
617                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
618                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
619                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
620                        xth2o(3) = dom;
621                        // multiply by dt^2/2
622                        xth2o*=d2t/2;
623                        return xth2o;
624                }
625                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
626                         iam = x0 ( 0 );
627                         ibm = x0 ( 1 );
628                         omm = x0 ( 2 );
629                         thm = x0 ( 3 );
630                // d ia
631                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
632                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
633                // d ib
634                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
635                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
636                // d om
637                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
638                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
639                        A ( 2,2 ) = 1.0;
640                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
641                // d th
642                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
643                        // FOR d2t*dom!!!!!!!!!
644/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
645                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
646                        A ( 3,2 ) = dt;
647                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
648                }
649
650                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
651
652};
653
654
655UIREGISTER ( IMpmsm2o );
656
657//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
658class IMpmsmStat : public IMpmsm {
659        public:
660        IMpmsmStat() :IMpmsm() {};
661        //! Set mechanical and electrical variables
662        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;}
663
664        vec eval ( const vec &x0, const vec &u0 ) {
665                // last state
666                double iam = x0 ( 0 );
667                double ibm = x0 ( 1 );
668                double omm = x0 ( 2 );
669                double thm = x0 ( 3 );
670                double uam = u0 ( 0 );
671                double ubm = u0 ( 1 );
672
673                vec xk=zeros ( 4 );
674                //ia
675                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
676                //ib
677                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
678                //om
679                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
680                //th
681                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
682                return xk;
683        }
684
685        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
686//              double iam = x0 ( 0 );
687//              double ibm = x0 ( 1 );
688                double omm = x0 ( 2 );
689                double thm = x0 ( 3 );
690                // d ia
691                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
692                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
693                // d ib
694                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
695                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
696                // d om
697                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
698                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
699                A ( 2,2 ) = 1.0;
700                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
701                // d th
702                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
703        }
704
705        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
706
707};
708
709UIREGISTER ( IMpmsmStat );
710
711
712//! State for PMSM with unknown Mz
713class IMpmsmMz: public IMpmsm{
714        public:
715                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
716        //! extend eval by Mz
717                vec eval ( const vec &x0, const vec &u0 ) {
718                        vec x(4);
719                        Mz = x0(4); //last of the state is Mz
720               
721                //teh first 4 states are same as before (given that Mz is set)
722                        x=IMpmsm::eval(x0,u0); // including model of drops!
723                        return concat(x,Mz);
724                }
725                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
726                //call initial
727                        if (full) A.clear();
728                        IMpmsm::dfdx_cond(x0,u0,A,full);
729                        A(2,4)=- p/J*dt;
730                        A(4,4)=1.0;
731                }       
732};
733
734UIREGISTER ( IMpmsmMz );
735
736//! State for PMSM with unknown Mz
737class IMpmsmStatMz: public IMpmsmStat{
738        public:
739                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
740        //! extend eval by Mz
741                vec eval ( const vec &x0, const vec &u0 ) {
742                        vec x(4);
743                        Mz = x0(4); //last of the state is Mz
744               
745                //teh first 4 states are same as before (given that Mz is set)
746                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
747                        return concat(x,Mz);
748                }
749                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
750                //call initial
751                        if (full) A.clear();
752                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
753                        A(2,4)=- p/J*dt;
754                        A(4,4)=1.0;
755                }       
756};
757
758UIREGISTER ( IMpmsmStatMz );
759
760
761//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
762class OMpmsm: public diffbifn {
763public:
764        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
765
766        vec eval ( const vec &x0, const vec &u0 ) {
767                vec y ( 2 );
768                y ( 0 ) = x0 ( 0 );
769                y ( 1 ) = x0 ( 1 );
770                return y;
771        }
772
773        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
774                A.clear();
775                A ( 0,0 ) = 1.0;
776                A ( 1,1 ) = 1.0;
777        }
778};
779
780UIREGISTER ( OMpmsm );
781
782//! Observation model for PMSM drive with roundoff-errors
783class OMpmsmRO: public diffbifn {
784        public:
785                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
786               
787                vec eval ( const vec &x0, const vec &u0 ) {
788                        vec y ( 2 );
789/*                      y ( 0 ) = x0 ( 0 );
790                        y ( 1 ) = x0 ( 1 );*/
791                       
792                        double istep=0.085;
793                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
794                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
795                       
796                        return y;
797                }
798               
799                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
800                        A.clear();
801                        A ( 0,0 ) = 1.0;
802                        A ( 1,1 ) = 1.0;
803                }
804};
805
806UIREGISTER ( OMpmsmRO );
807
808class OMpmsmROMz: public OMpmsmRO{
809public:
810        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
811};
812UIREGISTER ( OMpmsmROMz );
813
814//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
815class OMpmsm4: public diffbifn {
816        public:
817                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
818
819                vec eval ( const vec &x0, const vec &u0 ) {
820                        vec y ( 4 );
821                        y  = x0 ;
822                        return y;
823                }
824
825                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
826                        A.clear();
827                        A(0,0)=1.0;
828                        A(1,1)=1.0;
829                }
830};
831
832UIREGISTER ( OMpmsm4 );
833
834//! Observation model for PMSM drive id d-q coordinates
835class OMpmsmDQ: public diffbifn {
836public:
837        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
838       
839        vec eval ( const vec &x0, const vec &u0 ) {
840                vec y ( 2 );
841                double ct = cos(x0(3));
842                double st = sin(x0(3));
843                y(0)  = ct*x0(0)-st*x0(1);
844                y(1)  = st*x0(0)+ct*x0(1);
845               
846                return y;
847        }
848       
849        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
850                double ct = cos(x0(3));
851                double st = sin(x0(3));
852                A(0,0) = ct;
853                A(1,0) = -st;
854                A(0,1) = st;
855                A(1,1) = ct;
856               
857                A(0,2) = 0.0; 
858                A(1,2) = 0.0;
859                A(0,3) = -st*x0(0)-ct*x0(1);
860                A(1,3) = ct*x0(0)-st*x0(1);
861        }
862};
863
864UIREGISTER ( OMpmsmDQ );
865
866//! Observation model for PMSM drive in reduced form coordinates
867class OMpmsmOT: public diffbifn {
868public:
869        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
870       
871        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;}
872       
873        OMpmsmOT() :diffbifn () {dimy=2;dimx=2;dimu=4;};
874       
875        vec eval ( const vec &x0, const vec &u0 ) {
876                vec y ( 2 );
877                const double &omm = x0(0);
878                const double &thm = x0(1);
879               
880                const double &uam = u0 ( 0 );
881                const double &ubm = u0 ( 1 );
882                const double &iam = u0 ( 2 );
883                const double &ibm = u0 ( 3 );
884               
885                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
886                //ib
887                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
888               
889                return y;
890        }
891       
892        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
893                const double &omm = x0(0);
894                const double &thm = x0(1);
895                               
896                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
897                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
898                // d ib
899                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
900                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
901               
902               
903        }
904
905void from_setting( const Setting &root )
906{       
907       
908        const SettingResolver& params_b=root["params"];
909        const Setting& params=params_b.result;
910       
911        set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
912        params["kp"], params["p"], params["J"], 0.0);
913};
914
915};
916
917UIREGISTER ( OMpmsmOT );
918
919//! Observation model for PMSM drive in reduced form coordinates
920class OMpmsmOTM: public diffbifn {
921public:
922        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
923       
924        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;}
925       
926        OMpmsmOTM() :diffbifn () {dimy=2;dimx=2;dimu=4;};
927       
928        vec eval ( const vec &x0, const vec &u0 ) {
929                vec y ( 2 );
930                const double &omm = x0(0);
931                const double &thm = x0(1);
932               
933                const double &uam = u0 ( 0 );
934                const double &ubm = u0 ( 1 );
935                const double &iam = u0 ( 2 );
936                const double &ibm = u0 ( 3 );
937               
938                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
939                //ib
940                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
941               
942                return y;
943        }
944       
945        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
946                const double &omm = x0(0);
947                const double &thm = x0(1);
948               
949                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
950                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
951                A( 0,2) = 0.0;
952                // d ib
953                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
954                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
955                A(1,2) = 0.0;
956               
957        }
958       
959        void from_setting( const Setting &root )
960        {       
961               
962                const SettingResolver& params_b=root["params"];
963                const Setting& params=params_b.result;
964               
965                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
966                params["kp"], params["p"], params["J"], 0.0);
967        };
968       
969};
970
971UIREGISTER ( OMpmsmOTM );
972
973
974
975//! Observation model for PMSM drive in reduced form coordinates -- scaled to [-1,1]
976class OMpmsmOTsc: public diffbifn {
977public:
978        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
979       
980        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;}
981       
982        OMpmsmOTsc() :diffbifn () {dimy=2;dimx=2;dimu=4;};
983       
984        vec eval ( const vec &x0, const vec &u0 ) {
985                vec y ( 2 );
986                const double &omm = x0(0);
987                const double &thm = x0(1);
988               
989                const double &uam = u0 ( 0 );
990                const double &ubm = u0 ( 1 );
991                const double &iam = u0 ( 2 );
992                const double &ibm = u0 ( 3 );
993               
994                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;
995                //ib
996                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;
997               
998                return y;
999        }
1000       
1001        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
1002                const double &omm = x0(0);
1003                const double &thm = x0(1);
1004               
1005                A ( 0,0 ) = Ypm/Ls*dt* Wref/Iref*sin ( thm*Thetaref/(1<<15) ); 
1006                A ( 0,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( cos ( thm*Thetaref/(1<<15) ) );
1007                // d ib
1008                A ( 1,0 ) = -Ypm/Ls*dt* Wref/Iref*cos ( thm*Thetaref/(1<<15) ); 
1009                A ( 1,1 ) = Ypm/Ls*dt*omm *Wref/Iref*Thetaref/(1<<15)* ( sin ( thm*Thetaref/(1<<15) ) );
1010               
1011               
1012        }
1013       
1014        void from_setting( const Setting &root )
1015        {       
1016               
1017                const SettingResolver& params_b=root["params"];
1018                const Setting& params=params_b.result;
1019               
1020                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
1021                params["kp"], params["p"], params["J"], 0.0);
1022        };
1023       
1024};
1025
1026UIREGISTER ( OMpmsmOTsc );
1027
1028
1029/*!@}*/
1030#endif //PMSM_H
Note: See TracBrowser for help on using the browser.