root/applications/pmsm/pmsmDS.h @ 1319

Revision 1243, 9.6 kB (checked in by smidl, 14 years ago)

Contrallable PMSM DS + PI control

  • Property svn:eol-style set to native
Line 
1/*!
2  \file
3  \brief DataSource for experiments with realistic simulator of the PMSM model
4  \author Vaclav Smidl.
5
6  -----------------------------------
7  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
8
9  Using IT++ for numerical operations
10  -----------------------------------
11*/
12
13#ifndef PMSM_DS
14#define PMSM_DS
15
16#include <base/loggers.h>
17#include <estim/kalman.h>
18#include "simulator_zdenek/simulator.h"
19#include "pmsm.h"
20
21using namespace bdm;
22
23//! Simulator of PMSM machine with predefined profile on omega
24class pmsmDS : public DS
25{
26        LOG_LEVEL(pmsmDS,logvoltage);
27
28protected:
29    //! indices of logged variables
30    int L_x, L_ou, L_oy, L_iu, L_optu;
31    //! Setpoints of omega in timespans given by dt_prof
32    vec profileWw;
33    //! Setpoints of Mz in timespans given by dt_prof
34    vec profileMz;
35    //! time-step for profiles
36    double dt_prof;
37    //! Number of miliseconds per discrete time step
38    int Dt;   
39public:
40    //! Constructor with fixed sampling period
41    pmsmDS () : DS()
42    {
43        Dt=125;
44        Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" );
45                dtsize = Drv._dsize();
46    }
47    void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 )
48    {
49        pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 );
50    }
51
52        void getdata ( vec &dt ) const
53    {
54        dt.set_subvector(0,vec ( KalmanObs,6 ));
55        dt(6)=x[2];
56        dt(7)=x[3];
57        dt(8)=x[8];
58    }
59    void write ( vec &ut ) {}
60
61    void step()
62    {
63        static int ind=0;
64        static double dW; // increase of W
65        static double Ww; // W
66        static double Mz; // W
67        if ( t>=dt_prof*ind )
68        {
69            ind++;
70            // check omega profile and set dW
71                        if ( ind <2 && profileWw.length() ==1 )
72                        {
73                                Ww=profileWw ( 0 );
74                                dW=0.0;
75                        }
76                        if ( ind<profileWw.length() )
77            {
78                    dW = profileWw ( ind )-profileWw ( ind-1 );
79                    dW *=125e-6/dt_prof;
80            }
81            else
82            {
83                dW = 0;
84            }
85            // Check Mz profile and set Mz
86            if ( ind<profileMz.length() )
87            {
88                //sudden increase
89                Mz = profileMz(ind);
90            }
91            else
92            {
93                Mz = 0;
94            }
95        }
96        Ww += dW;
97        //Simulate Dt seconds!
98        for ( int i=0; i<Dt; i++ )
99        {
100            pmsmsim_step ( Ww , Mz);
101        }
102//              for ( int i=0;i<Dt;i++ ) {      pmsmsim_noreg_step ( Ww , Mz);}
103
104        //discretization
105/*        double ustep=1.2;
106        KalmanObs [ 0 ] = ustep*itpp::round( KalmanObs [ 0 ]/ ustep) ;
107        KalmanObs [ 1 ] = ustep*itpp::round(KalmanObs [ 1 ]/ ustep);*/
108//         double istep=0.085;
109//         KalmanObs [ 2 ] = istep*itpp::round( KalmanObs [ 2 ]/ istep) ;
110//         KalmanObs [ 3 ] = istep*itpp::round(KalmanObs [ 3 ]/ istep);
111
112    };
113
114    void log_register ( logger &L , const string &prefix)
115    {
116                DS::log_register ( L, prefix );
117               
118       // log differences
119        if ( log_level[logvoltage] )
120        {
121            L.add_vector ( log_level, logvoltage, RV("{ua, ub }"), "model" );
122        }
123    }
124
125    void log_write ( logger &L )
126    {
127        if ( log_level[logvoltage] )
128        {
129            double sq3=sqrt ( 3.0 );
130            double ua,ub;
131            double i1=x[0];
132            double i2=0.5* ( -i1+sq3*x[1] );
133            double i3=0.5* ( -i1-sq3*x[1] );
134            double u1=KalmanObs[0];
135            double u2=0.5* ( -u1+sq3*KalmanObs[1] );
136            double u3=0.5* ( -u1-sq3*KalmanObs[1] );
137
138            double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
139            double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
140            double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
141            ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
142            ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
143            log_level.store( logvoltage , vec_2 ( ua,ub ) );
144        }
145
146    }
147
148    void set_profile ( double dt, const vec &Ww, const vec &Mz )
149    {
150        dt_prof=dt;
151        profileWw=Ww;
152        profileMz=Mz;
153    }
154
155    void from_setting( const Setting &root )
156    {
157                const SettingResolver& params_l(root["params"]);
158                const Setting &params = params_l.result;
159        set_parameters ( params["Rs"], params["Ls"], params["Fmag"], \
160                         params["Bf"], params["p"], params["kp"], \
161                         params["J"], params["Uc"], params["DT"], 1.0e-6 );
162
163        // Default values of profiles for omega and Mz
164        vec profW=vec("1.0");
165        vec profM=vec("0.0");
166        double tstep=1.0;
167        UI::get( tstep, root,  "tstep");
168        UI::get( profW, root, "profileW" );
169        UI::get( profM, root, "profileM" );
170        set_profile (tstep , profW, profM);
171
172    }
173
174    // TODO dodelat void to_setting( Setting &root ) const;
175};
176
177UIREGISTER ( pmsmDS );
178
179//! Simulator of PMSM machine with predefined profile on omega
180class pmsmDSctrl : public pmsmDS
181{
182        double Ww;
183        double ua;
184        double ub;
185public:
186        //! Constructor with fixed sampling period
187    pmsmDSctrl() : pmsmDS()
188        {
189                Drv.add(RV ( "{Ww }" ));
190                Urv=RV ( "{ua ub }" );
191                dtsize = Drv._dsize();
192                utsize = Urv._dsize();
193        }
194               
195                void getdata ( vec &dt ) const
196                {
197                        pmsmDS::getdata(dt);
198                        dt(9)=Ww;
199                }
200                void write (const vec &ut ) {
201                        ua = ut(0);
202                        ub = ut(1);
203                }
204               
205                void step()
206                {
207                        static int ind=0;
208                        static double dW; // increase of W
209                        static double Mz; // W
210                        t+=125e-6;
211                        if ( t>=dt_prof*ind )
212                        {
213                                ind++;
214                                // check omega profile and set dW
215                                if ( ind <2 && profileWw.length() ==1 )
216                                {
217                                        Ww=profileWw ( 0 );
218                                        dW=0.0;
219                                }
220                                if ( ind<profileWw.length() )
221                                {
222                                        dW = profileWw ( ind )-profileWw ( ind-1 );
223                                        dW *=125e-6/dt_prof;
224                                }
225                                else
226                                {
227                                        dW = 0;
228                                }
229                                // Check Mz profile and set Mz
230                                if ( ind<profileMz.length() )
231                                {
232                                        //sudden increase
233                                        Mz = profileMz(ind);
234                                }
235                                else
236                                {
237                                        Mz = 0;
238                                }
239                        }
240                        Ww += dW;
241                        //Simulate Dt seconds!
242                        for ( int i=0; i<Dt; i++ )
243                        {
244                                pmsmsim_noreg_step (ua , ub);
245                        }
246        };
247       
248        };
249       
250        UIREGISTER ( pmsmDSctrl );
251       
252
253//! This class behaves like BM but it is evaluating EKF
254class pmsmCRB : public EKFfull
255{
256protected:
257    vec interr;
258    vec old_true;
259    vec secder;
260    int L_CRB;
261    int L_err;
262    int L_sec;
263public:
264    //! constructor
265    pmsmCRB():EKFfull()
266    {
267        old_true=zeros(6);
268    }
269
270    void bayes(const vec &dt)
271    {
272        static vec umin(2);
273                vec u(2);
274                vec &mu = est._mu();
275                //assume we know state exactly:
276        vec true_state=vec(x,4); // read from pmsm
277        mu=true_state;
278
279        //integration error
280        old_true(4)=KalmanObs[4];
281        old_true(5)=KalmanObs[5];// add U
282        u(0) = KalmanObs[0]; // use the required value for derivatives
283        u(1) = KalmanObs[1];
284        interr = (true_state - pfxu->eval(old_true));
285
286        //second derivative
287        IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu.get());
288        if (pf)
289        {
290            secder=pf->eval2o(u-umin);
291        }
292
293        umin =u;
294        EKFfull::bayes(dt);
295        old_true.set_subvector(0,true_state);
296    }
297
298    void from_setting( const Setting &root )
299    {
300        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root, "IM");
301        shared_ptr<diffbifn> OM = UI::build<diffbifn>(root, "OM");
302
303        //parameters
304
305        //statistics
306        int dim=IM->dimension();
307
308        vec mu0;
309        if(root.exists("mu0"))
310                        UI::get( mu0, root, "mu0");
311                else
312            mu0=zeros(dim);
313
314        mat P0;
315        if(root.exists( "dP0" ))
316                {
317                        vec dP0;       
318                        UI::get(dP0,root, "dP0");
319            P0=diag(dP0);
320                }
321                else if (root.exists("P0"))
322                        UI::get(P0,root, "P0");
323                else
324            P0=eye(dim);
325
326        set_statistics(mu0,P0);
327
328        vec dQ;
329        UI::get( dQ, root, "dQ");
330        vec dR;
331        UI::get( dR, root, "dR");
332        set_parameters(IM, OM, diag(dQ) , diag(dR));
333
334        //connect
335        shared_ptr<RV> drv = UI::build<RV>(root, "drv");
336        set_yrv(*drv);
337        shared_ptr<RV> rv = UI::build<RV>(root, "rv");
338        set_rv(*rv);
339    }
340
341    // TODO dodelat void to_setting( Setting &root ) const;
342};
343
344UIREGISTER ( pmsmCRB );
345
346
347//! This class behaves like BM but it is evaluating EKF
348class pmsmCRBMz : public EKFfull
349{
350protected:
351    int L_CRB;
352public:
353    //! constructor
354    pmsmCRBMz():EKFfull() {}
355
356    void bayes(const vec &dt)
357    {
358//assume we know state exactly:
359        vec true_state(5);
360        true_state.set_subvector(0,vec(x,4)); // read from pmsm
361        true_state(4)=x[8];
362
363        vec &mu = est._mu();
364        mu = true_state;
365        //hack for ut
366        EKFfull::bayes(dt);
367    }
368
369
370    void from_setting( const Setting &root )
371    {
372        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root,"IM");
373                shared_ptr<diffbifn> OM = UI::build<diffbifn>(root,"OM");
374
375        //statistics
376        int dim=IM->dimension();
377        vec mu0;
378        if( root.exists( "mu0"))
379                        UI::get(mu0, root, "mu0");
380                else
381            mu0=zeros(dim);
382
383        mat P0;
384
385                if(root.exists("dP0"))
386                {
387                        vec dP0;                               
388                        UI::get(dP0, root, "dP0");
389                        P0=diag(dP0);
390                }
391                else if(root.exists("P0"))
392                        UI::get( P0, root, "P0" );
393                else
394                        P0=eye(dim);
395
396        set_statistics(mu0,P0);
397
398        vec dQ;
399        UI::get(dQ, root, "dQ");
400        vec dR;
401        UI::get(dR, root, "dR");
402        set_parameters(IM, OM, diag(dQ), diag(dR));
403
404        //connect
405                shared_ptr<RV> drv = UI::build<RV>(root, "drv");
406        set_yrv(*drv);
407                shared_ptr<RV> rv = UI::build<RV>(root, "rv");
408        set_rv(*rv);
409    }
410
411    // TODO dodelat void to_setting( Setting &root ) const;
412};
413
414UIREGISTER ( pmsmCRBMz );
415
416#endif
Note: See TracBrowser for help on using the browser.