root/applications/pmsm/pmsmDS.h @ 1439

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

noreg accepts Mz

  • 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[9];
41        double KalmanObs[6];
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                        pmsmsim_fill_xy(x,KalmanObs);
219                        double t;
220                       
221                        static int ind=0;
222                        static double dW; // increase of W
223                        static double Mz; // W
224                        t=pmsmsim_get_t();
225                        if ( t>=dt_prof*ind )
226                        {
227                                ind++;
228                                // check omega profile and set dW
229                                if ( ind <2 && profileWw.length() ==1 )
230                                {
231                                        Ww=profileWw ( 0 );
232                                        dW=0.0;
233                                }
234                                if ( ind<profileWw.length() )
235                                {
236                                        dW = profileWw ( ind )-profileWw ( ind-1 );
237                                        dW *=125e-6/dt_prof;
238                                }
239                                else
240                                {
241                                        dW = 0;
242                                }
243                                // Check Mz profile and set Mz
244                                if ( ind<profileMz.length() )
245                                {
246                                        //sudden increase
247                                        Mz = profileMz(ind);
248                                }
249                                else
250                                {
251                                        Mz = 0;
252                                }
253                        }
254                        Ww += dW;
255                        //Simulate Dt seconds!
256                        for ( int i=0; i<Dt; i++ )
257                        {
258                                pmsmsim_noreg_step (ua , ub, Mz);
259                        }
260        };
261       
262        };
263       
264        UIREGISTER ( pmsmDSctrl );
265       
266
267#ifdef XXX
268//! This class behaves like BM but it is evaluating EKF
269class pmsmCRB : public EKFfull
270{
271protected:
272    vec interr;
273    vec old_true;
274    vec secder;
275    int L_CRB;
276    int L_err;
277    int L_sec;
278public:
279    //! constructor
280    pmsmCRB():EKFfull()
281    {
282        old_true=zeros(6);
283    }
284
285    void bayes(const vec &dt)
286    {
287        static vec umin(2);
288                vec u(2);
289                vec &mu = est._mu();
290                //assume we know state exactly:
291        vec true_state=0;//TODO vec(x,4); // read from pmsm
292        mu=true_state;
293
294        //integration error
295        old_true(4)=KalmanObs[4];
296        old_true(5)=KalmanObs[5];// add U
297        u(0) = KalmanObs[0]; // use the required value for derivatives
298        u(1) = KalmanObs[1];
299        interr = (true_state - pfxu->eval(old_true));
300
301        //second derivative
302        IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu.get());
303        if (pf)
304        {
305            secder=pf->eval2o(u-umin);
306        }
307
308        umin =u;
309        EKFfull::bayes(dt);
310        old_true.set_subvector(0,true_state);
311    }
312
313    void from_setting( const Setting &root )
314    {
315        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root, "IM");
316        shared_ptr<diffbifn> OM = UI::build<diffbifn>(root, "OM");
317
318        //parameters
319
320        //statistics
321        int dim=IM->dimension();
322
323        vec mu0;
324        if(root.exists("mu0"))
325                        UI::get( mu0, root, "mu0");
326                else
327            mu0=zeros(dim);
328
329        mat P0;
330        if(root.exists( "dP0" ))
331                {
332                        vec dP0;       
333                        UI::get(dP0,root, "dP0");
334            P0=diag(dP0);
335                }
336                else if (root.exists("P0"))
337                        UI::get(P0,root, "P0");
338                else
339            P0=eye(dim);
340
341        set_statistics(mu0,P0);
342
343        vec dQ;
344        UI::get( dQ, root, "dQ");
345        vec dR;
346        UI::get( dR, root, "dR");
347        set_parameters(IM, OM, diag(dQ) , diag(dR));
348
349        //connect
350        shared_ptr<RV> drv = UI::build<RV>(root, "drv");
351        set_yrv(*drv);
352        shared_ptr<RV> rv = UI::build<RV>(root, "rv");
353        set_rv(*rv);
354    }
355
356    // TODO dodelat void to_setting( Setting &root ) const;
357};
358
359UIREGISTER ( pmsmCRB );
360
361
362//! This class behaves like BM but it is evaluating EKF
363class pmsmCRBMz : public EKFfull
364{
365protected:
366    int L_CRB;
367public:
368    //! constructor
369    pmsmCRBMz():EKFfull() {}
370
371    void bayes(const vec &dt)
372    {
373//assume we know state exactly:
374        vec true_state(5);
375        true_state.set_subvector(0,vec(x,4)); // read from pmsm
376        true_state(4)=x[8];
377
378        vec &mu = est._mu();
379        mu = true_state;
380        //hack for ut
381        EKFfull::bayes(dt);
382    }
383
384
385    void from_setting( const Setting &root )
386    {
387        shared_ptr<diffbifn> IM = UI::build<diffbifn>(root,"IM");
388                shared_ptr<diffbifn> OM = UI::build<diffbifn>(root,"OM");
389
390        //statistics
391        int dim=IM->dimension();
392        vec mu0;
393        if( root.exists( "mu0"))
394                        UI::get(mu0, root, "mu0");
395                else
396            mu0=zeros(dim);
397
398        mat P0;
399
400                if(root.exists("dP0"))
401                {
402                        vec dP0;                               
403                        UI::get(dP0, root, "dP0");
404                        P0=diag(dP0);
405                }
406                else if(root.exists("P0"))
407                        UI::get( P0, root, "P0" );
408                else
409                        P0=eye(dim);
410
411        set_statistics(mu0,P0);
412
413        vec dQ;
414        UI::get(dQ, root, "dQ");
415        vec dR;
416        UI::get(dR, root, "dR");
417        set_parameters(IM, OM, diag(dQ), diag(dR));
418
419        //connect
420                shared_ptr<RV> drv = UI::build<RV>(root, "drv");
421        set_yrv(*drv);
422                shared_ptr<RV> rv = UI::build<RV>(root, "rv");
423        set_rv(*rv);
424    }
425
426    // TODO dodelat void to_setting( Setting &root ) const;
427};
428
429UIREGISTER ( pmsmCRBMz );
430#endif
431
432#endif
Note: See TracBrowser for help on using the browser.