root/pmsm/pmsm.h @ 233

Revision 232, 7.0 kB (checked in by smidl, 16 years ago)

test 2 order Taylor

  • Property svn:eol-style set to native
Line 
1#ifndef PMSM_H
2#define PMSM_H
3
4#include <stat/libFN.h>
5
6/*! \defgroup PMSM
7@{
8*/
9
10//TODO hardcoded RVs!!!
11RV rx ( "{ia ib om th }");
12RV ru ( "{ua ub }");
13RV ry ( "{oia oib }");
14RV rU = concat(ru, RV("{dua dub }"));
15
16// class uipmsm : public uibase{
17//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
18// };
19
20vec x2o_dbg(4);
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 (rx.count(), rx, ru ) {};
29        //! Set mechanical and electrical variables
30        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        vec eval ( const vec &x0, const vec &u0 ) {
33                // last state
34                double iam = x0 ( 0 );
35                double ibm = x0 ( 1 );
36                double omm = x0 ( 2 );
37                double thm = x0 ( 3 );
38                double uam = u0 ( 0 );
39                double ubm = u0 ( 1 );
40
41                vec xk=zeros ( 4 );
42                //ia
43                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
44                //ib
45                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
46                //om
47                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
48                //th
49                xk ( 3 ) = thm + omm*dt; // <0..2pi>
50                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
51                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
52                return xk;
53        }
54
55        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
56                double iam = x0 ( 0 );
57                double ibm = x0 ( 1 );
58                double omm = x0 ( 2 );
59                double thm = x0 ( 3 );
60                // d ia
61                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
62                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
63                // d ib
64                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
65                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
66                // d om
67                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
68                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
69                A ( 2,2 ) = 1.0;
70                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
71                // d th
72                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
73        }
74
75        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
76
77};
78
79//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
80class IMpmsm2o : public diffbifn {
81        protected:
82                double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
83
84        public:
85                IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {};
86        //! Set mechanical and electrical variables
87                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;}
88
89                vec eval ( const vec &x0, const vec &u0 ) {
90                // last state
91                        double iam = x0 ( 0 );
92                        double ibm = x0 ( 1 );
93                        double omm = x0 ( 2 );
94                        double thm = x0 ( 3 );
95                        double uam = u0 ( 0 );
96                        double ubm = u0 ( 1 );
97                        double dua = u0 ( 2 )/dt;
98                        double dub = u0 ( 3 )/dt;
99
100                        double cth = cos(thm);
101                        double sth = sin(thm);
102                        double d2t = dt*dt/2;
103                       
104                        double dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
105                        double dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
106                        double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
107                        double dth = omm;
108                       
109                        double d2ia = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
110                        double d2ib = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
111                        double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
112                        double d2th = dom;
113                       
114                        vec xk=zeros ( 4 );
115                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
116                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
117                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
118                        xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi>
119                       
120                        x2o_dbg(0)=d2t*d2ia;
121                        x2o_dbg(1)=d2t*d2ib;
122                        x2o_dbg(2)=d2t*d2om;
123                        x2o_dbg(3)=d2t*d2th;
124                       
125                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
126                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
127                        return xk;
128                }
129
130                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
131                        double iam = x0 ( 0 );
132                        double ibm = x0 ( 1 );
133                        double omm = x0 ( 2 );
134                        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};
153
154//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
155class IMpmsmStat : public IMpmsm {
156        IMpmsmStat() :IMpmsm() {};
157        //! Set mechanical and electrical variables
158        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;}
159
160        vec eval ( const vec &x0, const vec &u0 ) {
161                // last state
162                double iam = x0 ( 0 );
163                double ibm = x0 ( 1 );
164                double omm = x0 ( 2 );
165                double thm = x0 ( 3 );
166                double uam = u0 ( 0 );
167                double ubm = u0 ( 1 );
168
169                vec xk=zeros ( 4 );
170                //ia
171                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
172                //ib
173                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
174                //om
175                xk ( 2 ) = omm;// + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
176                //th
177                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
178                return xk;
179        }
180
181        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
182//              double iam = x0 ( 0 );
183//              double ibm = x0 ( 1 );
184                double omm = x0 ( 2 );
185                double thm = x0 ( 3 );
186                // d ia
187                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
188                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
189                // d ib
190                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
191                A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
192                // d om
193                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
194                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
195                A ( 2,2 ) = 1.0;
196                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
197                // d th
198                A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
199        }
200
201        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
202
203};
204
205//! Observation model for PMSM drive and its derivative with respect to \f$x\f$
206class OMpmsm: public diffbifn {
207public:
208        OMpmsm() :diffbifn (2, rx,ru ) {};
209
210        vec eval ( const vec &x0, const vec &u0 ) {
211                vec y ( 2 );
212                y ( 0 ) = x0 ( 0 );
213                y ( 1 ) = x0 ( 1 );
214                return y;
215        }
216
217        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
218                A.clear();
219                A ( 0,0 ) = 1.0;
220                A ( 1,1 ) = 1.0;
221        }
222};
223
224/*!@}*/
225#endif //PMSM_H
Note: See TracBrowser for help on using the browser.