root/applications/pmsm/pmsm.h @ 1292

Revision 1292, 15.3 kB (checked in by smidl, 13 years ago)

model in DQ + test

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <math/functions.h>
5#include "base/user_info.h"
6
7/*! \defgroup PMSM
8@{
9*/
10
11using namespace bdm;
12
13//TODO hardcoded RVs!!!
14// RV rx ( "{ia ib om th }");
15// RV ru ( "{o_ua o_ub }");
16// RV ry ( "{oia oib }");
17
18// class uipmsm : public uibase{
19//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
20// };
21
22//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
23class IMpmsm : public diffbifn {
24protected:
25        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
26
27        bool compensate;
28        bool cutoff;
29public:
30        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
31        //! Set mechanical and electrical variables
32        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
33
34        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
35/*              ua=u0[0];
36                ub=u0[1];
37                return;*/
38                double sq3=sqrt ( 3.0 );
39                double i1=x0(0);
40                double i2=0.5* ( -i1+sq3*x0[1] );
41                double i3=0.5* ( -i1-sq3*x0[1] );
42                double u1=u0(0);
43                double u2=0.5* ( -u1+sq3*u0(1) );
44                double u3=0.5* ( -u1-sq3*u0(1) );
45
46                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
47                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
48                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
49                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
50                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
51        }
52
53        vec eval ( const vec &x0, const vec &u0 ) {
54                // last state
55                const double &iam = x0 ( 0 );
56                const double &ibm = x0 ( 1 );
57                const double &omm = x0 ( 2 );
58                const double &thm = x0 ( 3 );
59                double uam;
60                double ubm;
61
62                if (compensate){
63                        modelpwm(x0,u0,uam,ubm);
64                } else {
65                        uam = u0(0);
66                        ubm = u0(1);
67                }
68               
69               
70                vec xk( 4 );
71                //ia
72                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
73                //ib
74                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
75                //om
76                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
77                //th
78                xk ( 3 ) = thm + omm*dt; // <0..2pi>
79                if (cutoff) {
80                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
81                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
82                }
83                return xk;
84        }
85
86        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
87                const double &iam = x0 ( 0 );
88                const double &ibm = x0 ( 1 );
89                const double &omm = x0 ( 2 );
90                const double &thm = x0 ( 3 );
91                // d ia
92                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
93                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
94                // d ib
95                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
96                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
97                // d om
98                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
99                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
100                A ( 2,2 ) = 1.0;
101                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
102                // d th
103                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
104        }
105
106        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
107
108        void from_setting( const Setting &root )
109        {       
110
111                const SettingResolver& params_b=root["params"];
112                const Setting& params=params_b.result;
113               
114                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
115                         params["kp"], params["p"], params["J"], 0.0);
116                int comp=0;
117                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
118                int cuto=0;
119                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
120        };
121
122        // TODO dodelat void to_setting( Setting &root ) const;
123};
124
125UIREGISTER ( IMpmsm );
126
127
128//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
129class IMpmsmDQ : public diffbifn {
130protected:
131        double Rs, Ld, Lq, dt, Ypm, kp, p,  J, Mz;
132       
133        bool compensate;
134        bool cutoff;
135public:
136        IMpmsmDQ() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
137        //! Set mechanical and electrical variables
138        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;}
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 &idm = x0 ( 0 );
162                const double &iqm = x0 ( 1 );
163                const double &omm = x0 ( 2 );
164                const double &thm = x0 ( 3 );
165                double uam;
166                double ubm;
167               
168                if (compensate){
169                        modelpwm(x0,u0,uam,ubm);
170                } else {
171                        uam = u0(0);
172                        ubm = u0(1);
173                }
174               
175                vec xk( 4 );
176                //id
177                xk ( 0 ) = ( 1.0- Rs/Ld*dt ) * idm + Lq/Ld*iqm*dt*omm + dt/Ld*(cos(thm)*uam+sin(thm)*ubm);
178                //iq
179                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);
180                //om
181                xk ( 2 ) = omm + kp*p*p /J*dt* ( (Ld-Lq)*idm+ Ypm)*iqm - p/J*dt*Mz;
182                //th
183                xk ( 3 ) = thm + omm*dt; // <0..2pi>
184                if (cutoff) {
185                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
186                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
187                }
188                return xk;
189        }
190       
191        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
192                const double &idm = x0 ( 0 );
193                const double &iqm = x0 ( 1 );
194                const double &omm = x0 ( 2 );
195                const double &thm = x0 ( 3 );
196               
197                double uam;
198                double ubm;
199                if (compensate){
200                        modelpwm(x0,u0,uam,ubm);
201                } else {
202                        uam = u0(0);
203                        ubm = u0(1);
204                }
205                // d id
206                A ( 0,0 ) = ( 1.0- Rs/Ld*dt ); A ( 0,1 ) = Lq/Ld*dt*omm;
207                A ( 0,2 ) = Lq/Ld*iqm*dt; A ( 0,3 ) = dt/Ld*(-sin(thm)*uam+cos(thm)*ubm);
208                // d iq
209                A ( 1,0 ) = -Ld/Lq*omm*dt ; A ( 1,1 ) = ( 1.0- Rs/Lq*dt );
210                A ( 1,2 ) = -Ypm/Lq*dt- Ld/Lq*dt*idm; A ( 1,3 ) = dt/Lq*(-cos(thm)*uam-sin(thm)*ubm);
211                // d om
212                A ( 2,0 ) = kp*p*p /J*dt* ( Ld-Lq)*iqm;
213                A ( 2,1 ) = kp*p*p /J*dt* ((Ld-Lq)*idm+Ypm);
214                A ( 2,2 ) = 1.0;
215                A ( 2,3 ) = 0;
216                // d th
217                A ( 3,0 ) = 0.0; 
218                A ( 3,1 ) = 0.0; 
219                A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
220        }
221       
222        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
223       
224        void from_setting( const Setting &root )
225        {       
226               
227                const SettingResolver& params_b=root["params"];
228                const Setting& params=params_b.result;
229               
230                set_parameters ( params["Rs"], params["Ld"],params["Lq"], 125e-6, params["Fmag"], \
231                params["kp"], params["p"], params["J"], 0.0);
232                int comp=0;
233                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
234                int cuto=0;
235                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
236        };
237       
238        // TODO dodelat void to_setting( Setting &root ) const;
239};
240
241UIREGISTER ( IMpmsmDQ );
242
243
244
245
246
247
248
249//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
250class IMpmsm2o : public IMpmsm {
251        protected:
252//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
253                //! store first derivatives for the use in second derivatives
254                double dia, dib, dom, dth;
255                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
256                double d2t, cth, sth;
257                double iam, ibm, omm, thm, uam, ubm;
258        public:
259                IMpmsm2o() :IMpmsm () {};
260        //! Set mechanical and electrical variables
261                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;}
262
263                vec eval ( const vec &x0, const vec &u0 ) {
264                // last state
265                        iam = x0 ( 0 );
266                        ibm = x0 ( 1 );
267                        omm = x0 ( 2 );
268                        thm = x0 ( 3 );
269                        uam = u0 ( 0 );
270                        ubm = u0 ( 1 );
271
272                        cth = cos(thm);
273                        sth = sin(thm);
274                       
275                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
276                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
277                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
278                        dth = omm;
279                                               
280                        vec xk=zeros ( 4 );
281                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
282                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
283                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
284                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
285                       
286                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
287                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
288                        return xk;
289                }
290
291                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
292                vec eval2o(const vec &du){
293                        double dua = du ( 0 )/dt;
294                        double dub = du ( 1 )/dt;
295                       
296                        vec xth2o(4);
297                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
298                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
299                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
300                        xth2o(3) = dom;
301                        // multiply by dt^2/2
302                        xth2o*=d2t/2;
303                        return xth2o;
304                }
305                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
306                         iam = x0 ( 0 );
307                         ibm = x0 ( 1 );
308                         omm = x0 ( 2 );
309                         thm = x0 ( 3 );
310                // d ia
311                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
312                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
313                // d ib
314                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
315                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
316                // d om
317                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
318                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
319                        A ( 2,2 ) = 1.0;
320                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
321                // d th
322                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
323                        // FOR d2t*dom!!!!!!!!!
324/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
325                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
326                        A ( 3,2 ) = dt;
327                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
328                }
329
330                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
331
332};
333
334
335UIREGISTER ( IMpmsm2o );
336
337//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
338class IMpmsmStat : public IMpmsm {
339        public:
340        IMpmsmStat() :IMpmsm() {};
341        //! Set mechanical and electrical variables
342        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;}
343
344        vec eval ( const vec &x0, const vec &u0 ) {
345                // last state
346                double iam = x0 ( 0 );
347                double ibm = x0 ( 1 );
348                double omm = x0 ( 2 );
349                double thm = x0 ( 3 );
350                double uam = u0 ( 0 );
351                double ubm = u0 ( 1 );
352
353                vec xk=zeros ( 4 );
354                //ia
355                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
356                //ib
357                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
358                //om
359                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
360                //th
361                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
362                return xk;
363        }
364
365        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
366//              double iam = x0 ( 0 );
367//              double ibm = x0 ( 1 );
368                double omm = x0 ( 2 );
369                double thm = x0 ( 3 );
370                // d ia
371                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
372                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
373                // d ib
374                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
375                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
376                // d om
377                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
378                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
379                A ( 2,2 ) = 1.0;
380                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
381                // d th
382                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
383        }
384
385        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
386
387};
388
389UIREGISTER ( IMpmsmStat );
390
391
392//! State for PMSM with unknown Mz
393class IMpmsmMz: public IMpmsm{
394        public:
395                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
396        //! extend eval by Mz
397                vec eval ( const vec &x0, const vec &u0 ) {
398                        vec x(4);
399                        Mz = x0(4); //last of the state is Mz
400               
401                //teh first 4 states are same as before (given that Mz is set)
402                        x=IMpmsm::eval(x0,u0); // including model of drops!
403                        return concat(x,Mz);
404                }
405                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
406                //call initial
407                        if (full) A.clear();
408                        IMpmsm::dfdx_cond(x0,u0,A,full);
409                        A(2,4)=- p/J*dt;
410                        A(4,4)=1.0;
411                }       
412};
413
414UIREGISTER ( IMpmsmMz );
415
416//! State for PMSM with unknown Mz
417class IMpmsmStatMz: public IMpmsmStat{
418        public:
419                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
420        //! extend eval by Mz
421                vec eval ( const vec &x0, const vec &u0 ) {
422                        vec x(4);
423                        Mz = x0(4); //last of the state is Mz
424               
425                //teh first 4 states are same as before (given that Mz is set)
426                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
427                        return concat(x,Mz);
428                }
429                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
430                //call initial
431                        if (full) A.clear();
432                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
433                        A(2,4)=- p/J*dt;
434                        A(4,4)=1.0;
435                }       
436};
437
438UIREGISTER ( IMpmsmStatMz );
439
440
441//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
442class OMpmsm: public diffbifn {
443public:
444        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=0;};
445
446        vec eval ( const vec &x0, const vec &u0 ) {
447                vec y ( 2 );
448                y ( 0 ) = x0 ( 0 );
449                y ( 1 ) = x0 ( 1 );
450                return y;
451        }
452
453        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
454                A.clear();
455                A ( 0,0 ) = 1.0;
456                A ( 1,1 ) = 1.0;
457        }
458};
459
460UIREGISTER ( OMpmsm );
461
462//! Observation model for PMSM drive with roundoff-errors
463class OMpmsmRO: public diffbifn {
464        public:
465                OMpmsmRO() :diffbifn () {dimy=2;dimx=4;dimu=0;};
466               
467                vec eval ( const vec &x0, const vec &u0 ) {
468                        vec y ( 2 );
469/*                      y ( 0 ) = x0 ( 0 );
470                        y ( 1 ) = x0 ( 1 );*/
471                       
472                        double istep=0.085;
473                        y[ 0 ] = istep*itpp::round( x0[ 0 ]/ istep) ;
474                        y[ 1 ] = istep*itpp::round( x0[ 1 ]/ istep);
475                       
476                        return y;
477                }
478               
479                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
480                        A.clear();
481                        A ( 0,0 ) = 1.0;
482                        A ( 1,1 ) = 1.0;
483                }
484};
485
486UIREGISTER ( OMpmsmRO );
487
488class OMpmsmROMz: public OMpmsmRO{
489public:
490        OMpmsmROMz() :OMpmsmRO() {dimy=2;dimx=5;dimu=0;};
491};
492UIREGISTER ( OMpmsmROMz );
493
494//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
495class OMpmsm4: public diffbifn {
496        public:
497                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
498
499                vec eval ( const vec &x0, const vec &u0 ) {
500                        vec y ( 4 );
501                        y  = x0 ;
502                        return y;
503                }
504
505                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
506                        A.clear();
507                        A(0,0)=1.0;
508                        A(1,1)=1.0;
509                }
510};
511
512UIREGISTER ( OMpmsm4 );
513
514//! Observation model for PMSM drive id d-q coordinates
515class OMpmsmDQ: public diffbifn {
516public:
517        OMpmsmDQ() :diffbifn () {dimy=2;dimx=4;dimu=2;};
518       
519        vec eval ( const vec &x0, const vec &u0 ) {
520                vec y ( 2 );
521                double ct = cos(x0(3));
522                double st = sin(x0(3));
523                y(0)  = ct*x0(0)-st*x0(1);
524                y(1)  = st*x0(0)+ct*x0(1);
525               
526                return y;
527        }
528       
529        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
530                double ct = cos(x0(3));
531                double st = sin(x0(3));
532                A(0,0) = ct;
533                A(1,0) = -st;
534                A(0,1) = st;
535                A(1,1) = ct;
536               
537                A(0,2) = 0.0; 
538                A(1,2) = 0.0;
539                A(0,3) = -st*x0(0)-ct*x0(1);
540                A(1,3) = ct*x0(0)-st*x0(1);
541        }
542};
543
544UIREGISTER ( OMpmsmDQ );
545
546
547/*!@}*/
548#endif //PMSM_H
Note: See TracBrowser for help on using the browser.