root/pmsm/pmsm.h @ 81

Revision 81, 4.4 kB (checked in by smidl, 16 years ago)

opravy + pridani simulace kovarianci

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <stat/libFN.h>
5#include <userinfo.h>
6
7
8//TODO hardcoded RVs!!!
9RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 ));
10RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 ));
11RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 ));
12
13class uipmsm : public uicompound{
14        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
15};
16
17//! State evolution model for a PMSM drive and its derivative with respect to \$x\$
18class IMpmsm : public diffbifn {
19protected:
20        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
21
22public:
23        IMpmsm() :diffbifn (rx.count(), rx, ru ) {};
24        //! Set mechanical and electrical variables
25        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;}
26
27        vec eval ( const vec &x0, const vec &u0 ) {
28                // last state
29                double iam = x0 ( 0 );
30                double ibm = x0 ( 1 );
31                double omm = x0 ( 2 );
32                double thm = x0 ( 3 );
33                double uam = u0 ( 0 );
34                double ubm = u0 ( 1 );
35
36                vec xk=zeros ( 4 );
37                //ia
38                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
39                //ib
40                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
41                //om
42                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
43                //th
44                xk ( 3 ) = thm + omm*dt; // <0..2pi>
45                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
46                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
47                return xk;
48        }
49
50        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
51                double iam = x0 ( 0 );
52                double ibm = x0 ( 1 );
53                double omm = x0 ( 2 );
54                double thm = x0 ( 3 );
55                // d ia
56                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
57                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
58                // d ib
59                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
60                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
61                // d om
62                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
63                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
64                A ( 2,2 ) = 1.0;
65                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
66                // d th
67                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
68        }
69
70        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
71
72};
73
74class IMpmsmStat : public IMpmsm {
75        IMpmsmStat() :IMpmsm() {};
76        //! Set mechanical and electrical variables
77        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;}
78
79        vec eval ( const vec &x0, const vec &u0 ) {
80                // last state
81                double iam = x0 ( 0 );
82                double ibm = x0 ( 1 );
83                double omm = x0 ( 2 );
84                double thm = x0 ( 3 );
85                double uam = u0 ( 0 );
86                double ubm = u0 ( 1 );
87
88                vec xk=zeros ( 4 );
89                //ia
90                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
91                //ib
92                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
93                //om
94                xk ( 2 ) = omm;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
95                //th
96                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
97                return xk;
98        }
99
100        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
101                double iam = x0 ( 0 );
102                double ibm = x0 ( 1 );
103                double omm = x0 ( 2 );
104                double thm = x0 ( 3 );
105                // d ia
106                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
107                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
108                // d ib
109                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
110                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
111                // d om
112                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
113                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
114                A ( 2,2 ) = 1.0;
115                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
116                // d th
117                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
118        }
119
120        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
121
122};
123
124//! Observation model for PMSM drive and its derivative with respect to \$x\$
125class OMpmsm: public diffbifn {
126public:
127        OMpmsm() :diffbifn (2, rx,ru ) {};
128
129        vec eval ( const vec &x0, const vec &u0 ) {
130                vec y ( 2 );
131                y ( 0 ) = x0 ( 0 );
132                y ( 1 ) = x0 ( 1 );
133                return y;
134        }
135
136        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
137                A.clear();
138                A ( 0,0 ) = 1.0;
139                A ( 1,1 ) = 1.0;
140        }
141};
142
143#endif //PMSM_H
Note: See TracBrowser for help on using the browser.