root/applications/pmsm/pmsmDS.h @ 1380

Revision 1380, 9.9 kB (checked in by smidl, 13 years ago)

MPF v C

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