root/applications/pmsm/pmsmDS.h @ 554

Revision 384, 9.4 kB (checked in by mido, 15 years ago)

possibly broken?

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