root/tests/pmsm.h @ 71

Revision 63, 4.2 kB (checked in by smidl, 17 years ago)

odstraneni net a drobne upravy

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