root/applications/pmsm/pmsmDS.h @ 1377

Revision 1377, 9.7 kB (checked in by smidl, 13 years ago)

realistic noise

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