root/applications/pmsm/pmsm.h @ 357

Revision 357, 10.1 kB (checked in by mido, 15 years ago)

mnoho zmen:
1) presun FindXXX modulu do \system
2) zalozeni dokumentace \doc\local\library_structure.dox
3) presun obsahu \tests\UI primo do \tests
4) namisto \INSTALL zalozen \install.html, je to vhodnejsi pro uzivatele WINDOWS, a snad i obecne
5) snaha o predelani veskerych UI podle nove koncepce, soubory pmsm_ui.h, arx_ui.h, KF_ui.h, libDS_ui.h, libEF_ui.h a loggers_ui.h ponechavam
jen zdokumentacnich duvodu, nic by na nich jiz nemelo zaviset, a po zkontrolovani spravnosti provedenych uprav by mely byt smazany
6) predelani estimatoru tak, aby fungoval s novym UI konceptem
7) vytazeni tridy bdmroot do samostatneho souboru \bdm\bdmroot.h
8) pridana dokumentace pro zacleneni programu ASTYLE do Visual studia, ASTYLE pridan do instalacniho balicku pro Windows

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <stat/libFN.h>
5#include "user_info.h"
6
7/*! \defgroup PMSM
8@{
9*/
10
11using namespace bdm;
12
13//TODO hardcoded RVs!!!
14RV rx ( "{ia ib om th }");
15RV ru ( "{ua ub }");
16RV 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
27public:
28        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2;};
29        //! Set mechanical and electrical variables
30        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;}
31
32        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
33/*              ua=u0[0];
34                ub=u0[1];
35                return;*/
36                double sq3=sqrt ( 3.0 );
37                double i1=x0(0);
38                double i2=0.5* ( -i1+sq3*x0[1] );
39                double i3=0.5* ( -i1-sq3*x0[1] );
40                double u1=u0(0);
41                double u2=0.5* ( -u1+sq3*u0(1) );
42                double u3=0.5* ( -u1-sq3*u0(1) );
43
44                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
45                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
46                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
47                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
48                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
49        }
50
51        vec eval ( const vec &x0, const vec &u0 ) {
52                // last state
53                const double &iam = x0 ( 0 );
54                const double &ibm = x0 ( 1 );
55                const double &omm = x0 ( 2 );
56                const double &thm = x0 ( 3 );
57                double uam;
58                double ubm;
59
60                modelpwm(x0,u0,uam,ubm);
61               
62                vec xk( 4 );
63                //ia
64                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
65                //ib
66                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
67                //om
68                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
69                //th
70                xk ( 3 ) = thm + omm*dt; // <0..2pi>
71                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
72                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
73                return xk;
74        }
75
76        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
77                const double &iam = x0 ( 0 );
78                const double &ibm = x0 ( 1 );
79                const double &omm = x0 ( 2 );
80                const double &thm = x0 ( 3 );
81                // d ia
82                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
83                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
84                // d ib
85                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
86                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
87                // d om
88                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
89                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
90                A ( 2,2 ) = 1.0;
91                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
92                // d th
93                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
94        }
95
96        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
97
98        void from_setting( const Setting &root )
99        {       
100                set_parameters ( root["params"]["Rs"], root["params"]["Ls"], 125e-6, root["params"]["Fmag"], \
101                        root["params"]["kp"],  root["params"]["p"], root["params"]["J"], 0.0 );
102        };
103
104        // TODO dodelat void to_setting( Setting &root ) const;
105};
106
107UIREGISTER ( IMpmsm );
108
109//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
110class IMpmsm2o : public IMpmsm {
111        protected:
112//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
113                //! store first derivatives for the use in second derivatives
114                double dia, dib, dom, dth;
115                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
116                double d2t, cth, sth;
117                double iam, ibm, omm, thm, uam, ubm;
118        public:
119                IMpmsm2o() :IMpmsm () {};
120        //! Set mechanical and electrical variables
121                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;}
122
123                vec eval ( const vec &x0, const vec &u0 ) {
124                // last state
125                        iam = x0 ( 0 );
126                        ibm = x0 ( 1 );
127                        omm = x0 ( 2 );
128                        thm = x0 ( 3 );
129                        uam = u0 ( 0 );
130                        ubm = u0 ( 1 );
131
132                        cth = cos(thm);
133                        sth = sin(thm);
134                       
135                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
136                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
137                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
138                        dth = omm;
139                                               
140                        vec xk=zeros ( 4 );
141                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
142                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
143                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
144                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
145                       
146                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
147                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
148                        return xk;
149                }
150
151                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
152                vec eval2o(const vec &du){
153                        double dua = du ( 0 )/dt;
154                        double dub = du ( 1 )/dt;
155                       
156                        vec xth2o(4);
157                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
158                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
159                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
160                        xth2o(3) = dom;
161                        // multiply by dt^2/2
162                        xth2o*=d2t/2;
163                        return xth2o;
164                }
165                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
166                         iam = x0 ( 0 );
167                         ibm = x0 ( 1 );
168                         omm = x0 ( 2 );
169                         thm = x0 ( 3 );
170                // d ia
171                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
172                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
173                // d ib
174                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
175                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
176                // d om
177                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
178                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
179                        A ( 2,2 ) = 1.0;
180                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
181                // d th
182                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
183                        // FOR d2t*dom!!!!!!!!!
184/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
185                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
186                        A ( 3,2 ) = dt;
187                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
188                }
189
190                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
191
192};
193
194
195UIREGISTER ( IMpmsm2o );
196
197//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
198class IMpmsmStat : public IMpmsm {
199        public:
200        IMpmsmStat() :IMpmsm() {};
201        //! Set mechanical and electrical variables
202        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;}
203
204        vec eval ( const vec &x0, const vec &u0 ) {
205                // last state
206                double iam = x0 ( 0 );
207                double ibm = x0 ( 1 );
208                double omm = x0 ( 2 );
209                double thm = x0 ( 3 );
210                double uam = u0 ( 0 );
211                double ubm = u0 ( 1 );
212
213                vec xk=zeros ( 4 );
214                //ia
215                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
216                //ib
217                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
218                //om
219                xk ( 2 ) = omm - p/J*dt*Mz;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) );
220                //th
221                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
222                return xk;
223        }
224
225        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
226//              double iam = x0 ( 0 );
227//              double ibm = x0 ( 1 );
228                double omm = x0 ( 2 );
229                double thm = x0 ( 3 );
230                // d ia
231                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
232                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
233                // d ib
234                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
235                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
236                // d om
237                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
238                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
239                A ( 2,2 ) = 1.0;
240                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
241                // d th
242                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
243        }
244
245        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
246
247};
248
249UIREGISTER ( IMpmsmStat );
250
251
252//! State for PMSM with unknown Mz
253class IMpmsmMz: public IMpmsm{
254        public:
255                IMpmsmMz()  {dimy=5; dimx = 5; dimu=2;};
256        //! extend eval by Mz
257                vec eval ( const vec &x0, const vec &u0 ) {
258                        vec x(4);
259                        Mz = x0(4); //last of the state is Mz
260               
261                //teh first 4 states are same as before (given that Mz is set)
262                        x=IMpmsm::eval(x0,u0); // including model of drops!
263                        return concat(x,Mz);
264                }
265                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
266                //call initial
267                        if (full) A.clear();
268                        IMpmsm::dfdx_cond(x0,u0,A,full);
269                        A(2,4)=- p/J*dt;
270                        A(4,4)=1.0;
271                }       
272};
273
274UIREGISTER ( IMpmsmMz );
275
276//! State for PMSM with unknown Mz
277class IMpmsmStatMz: public IMpmsmStat{
278        public:
279                IMpmsmStatMz()  {dimy=5; dimx = 5; dimu=2;};
280        //! extend eval by Mz
281                vec eval ( const vec &x0, const vec &u0 ) {
282                        vec x(4);
283                        Mz = x0(4); //last of the state is Mz
284               
285                //teh first 4 states are same as before (given that Mz is set)
286                        x=IMpmsmStat::eval(x0,u0); // including model of drops!
287                        return concat(x,Mz);
288                }
289                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
290                //call initial
291                        if (full) A.clear();
292                        IMpmsmStat::dfdx_cond(x0,u0,A,full);
293                        A(2,4)=- p/J*dt;
294                        A(4,4)=1.0;
295                }       
296};
297
298UIREGISTER ( IMpmsmStatMz );
299
300
301//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
302class OMpmsm: public diffbifn {
303public:
304        OMpmsm() :diffbifn () {dimy=2;dimx=4;dimu=2;};
305
306        vec eval ( const vec &x0, const vec &u0 ) {
307                vec y ( 2 );
308                y ( 0 ) = x0 ( 0 );
309                y ( 1 ) = x0 ( 1 );
310                return y;
311        }
312
313        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
314                A.clear();
315                A ( 0,0 ) = 1.0;
316                A ( 1,1 ) = 1.0;
317        }
318};
319
320UIREGISTER ( OMpmsm );
321
322//! Observation model for PMSM drive and its derivative with respect to \f$x\f$ for full vector of observations
323class OMpmsm4: public diffbifn {
324        public:
325                OMpmsm4() :diffbifn () {dimy=4;dimx=4;dimu=2;};
326
327                vec eval ( const vec &x0, const vec &u0 ) {
328                        vec y ( 4 );
329                        y  = x0 ;
330                        return y;
331                }
332
333                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
334                        if (full) A=eye(4);
335                }
336};
337
338UIREGISTER ( OMpmsm4 );
339
340
341
342
343
344/*!@}*/
345#endif //PMSM_H
Note: See TracBrowser for help on using the browser.