[258] | 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> |
---|
[318] | 14 | #include <estim/libKF.h> |
---|
[260] | 15 | #include "simulator.h" |
---|
[258] | 16 | #include "pmsm.h" |
---|
| 17 | |
---|
| 18 | //! Simulator of PMSM machine with predefined profile on omega |
---|
| 19 | class pmsmDS : public DS { |
---|
| 20 | |
---|
| 21 | protected: |
---|
| 22 | //! indeces of logged variables |
---|
[260] | 23 | int L_x, L_ou, L_oy, L_iu, L_optu; |
---|
[258] | 24 | //! Setpoints of omega in timespans given by dt_prof |
---|
| 25 | vec profileWw; |
---|
[318] | 26 | //! Setpoints of Mz in timespans given by dt_prof |
---|
| 27 | vec profileMz; |
---|
[258] | 28 | //! time-step for profiles |
---|
| 29 | double dt_prof; |
---|
| 30 | //! Number of miliseconds per discrete time step |
---|
| 31 | int Dt; |
---|
[280] | 32 | //! options for logging, - log predictions of 'true' voltage |
---|
| 33 | bool opt_modu; |
---|
[318] | 34 | //! options for logging, - |
---|
[258] | 35 | public: |
---|
[260] | 36 | //! Constructor with fixed sampling period |
---|
[280] | 37 | pmsmDS () {Dt=125; Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub }" );} |
---|
[258] | 38 | void set_parameters ( double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0 ) { |
---|
| 39 | pmsmsim_set_parameters ( Rs0, Ls0, Fmag0, Bf0, p0, kp0, J0, Uc0, DT0, dt0 ); |
---|
| 40 | } |
---|
[280] | 41 | //! parse options: "modelu" => opt_modu=true; |
---|
| 42 | void set_options ( string &opt ) { |
---|
| 43 | opt_modu = ( opt.find ( "modelu" ) ==string::npos ); |
---|
| 44 | } |
---|
[258] | 45 | void getdata ( vec &dt ) {dt=vec ( KalmanObs,6 );} |
---|
| 46 | void write ( vec &ut ) {} |
---|
| 47 | |
---|
| 48 | void step() { |
---|
| 49 | static int ind=0; |
---|
| 50 | static double dW; // increase of W |
---|
| 51 | static double Ww; // W |
---|
[318] | 52 | static double Mz; // W |
---|
[258] | 53 | if ( t>=dt_prof*ind ) { |
---|
[260] | 54 | ind++; |
---|
[318] | 55 | // check omega profile and set dW |
---|
[258] | 56 | if ( ind<profileWw.length() ) { |
---|
| 57 | //linear increase |
---|
[260] | 58 | if ( profileWw.length() ==1 ) { |
---|
[280] | 59 | Ww=profileWw ( 0 ); dW=0.0; |
---|
| 60 | } |
---|
[260] | 61 | else { |
---|
| 62 | dW = profileWw ( ind )-profileWw ( ind-1 ); |
---|
| 63 | dW *=125e-6/dt_prof; |
---|
| 64 | } |
---|
[258] | 65 | } |
---|
| 66 | else { |
---|
| 67 | dW = 0; |
---|
| 68 | } |
---|
[318] | 69 | // Check Mz profile and set Mz |
---|
| 70 | if ( ind<profileMz.length() ) { |
---|
| 71 | //sudden increase |
---|
| 72 | Mz = profileMz(ind); |
---|
| 73 | } |
---|
| 74 | else { |
---|
| 75 | Mz = 0; |
---|
| 76 | } |
---|
[258] | 77 | } |
---|
| 78 | Ww += dW; |
---|
| 79 | //Simulate Dt seconds! |
---|
[318] | 80 | for ( int i=0;i<Dt;i++ ) { pmsmsim_step ( Ww , Mz);} |
---|
[326] | 81 | // for ( int i=0;i<Dt;i++ ) { pmsmsim_noreg_step ( Ww , Mz);} |
---|
[258] | 82 | }; |
---|
| 83 | |
---|
| 84 | void log_add ( logger &L ) { |
---|
| 85 | L_x = L.add ( rx, "x" ); |
---|
[280] | 86 | L_oy = L.add ( ry, "o" ); |
---|
| 87 | L_ou = L.add ( ru, "o" ); |
---|
| 88 | L_iu = L.add ( ru, "t" ); |
---|
[260] | 89 | // log differences |
---|
[280] | 90 | if ( opt_modu ) { |
---|
| 91 | L_optu = L.add ( ru, "model" ); |
---|
[260] | 92 | } |
---|
[258] | 93 | } |
---|
| 94 | |
---|
| 95 | void logit ( logger &L ) { |
---|
| 96 | L.logit ( L_x, vec ( x,4 ) ); |
---|
| 97 | L.logit ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) ); |
---|
| 98 | L.logit ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) ); |
---|
| 99 | L.logit ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) ); |
---|
[280] | 100 | if ( opt_modu ) { |
---|
| 101 | double sq3=sqrt ( 3.0 ); |
---|
[260] | 102 | double ua,ub; |
---|
| 103 | double i1=x[0]; |
---|
[280] | 104 | double i2=0.5* ( -i1+sq3*x[1] ); |
---|
| 105 | double i3=0.5* ( -i1-sq3*x[1] ); |
---|
[260] | 106 | double u1=KalmanObs[0]; |
---|
[280] | 107 | double u2=0.5* ( -u1+sq3*KalmanObs[1] ); |
---|
| 108 | double u3=0.5* ( -u1-sq3*KalmanObs[1] ); |
---|
| 109 | |
---|
| 110 | double du1=0.7* ( double ( i1>0.1 ) - double ( i1<-0.1 ) ) +0.05*i1; |
---|
| 111 | double du2=0.7* ( double ( i2>0.1 ) - double ( i2<-0.1 ) ) +0.05*i2; |
---|
| 112 | double du3=0.7* ( double ( i3>0.1 ) - double ( i3<-0.1 ) ) +0.05*i3; |
---|
| 113 | ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0; |
---|
| 114 | ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3; |
---|
| 115 | L.logit ( L_optu , vec_2 ( ua,ub ) ); |
---|
[260] | 116 | } |
---|
[318] | 117 | |
---|
[280] | 118 | } |
---|
[258] | 119 | |
---|
[318] | 120 | void set_profile ( double dt, const vec &Ww, const vec &Mz ) {dt_prof=dt; profileWw=Ww; profileMz=Mz;} |
---|
[258] | 121 | }; |
---|
[318] | 122 | |
---|
| 123 | //! This class behaves like BM but it is evaluating EKF |
---|
[326] | 124 | class pmsmCRB : public EKFfull{ |
---|
[318] | 125 | protected: |
---|
| 126 | vec interr; |
---|
| 127 | vec old_true; |
---|
| 128 | vec secder; |
---|
| 129 | int L_CRB; |
---|
| 130 | int L_err; |
---|
| 131 | int L_sec; |
---|
| 132 | public: |
---|
| 133 | //! constructor |
---|
[326] | 134 | pmsmCRB():EKFfull(){old_true=zeros(6);} |
---|
[318] | 135 | |
---|
| 136 | void bayes(const vec &dt){ |
---|
| 137 | //assume we know state exactly: |
---|
| 138 | vec true_state=vec(x,4); // read from pmsm |
---|
[326] | 139 | E.set_mu(true_state); |
---|
[318] | 140 | |
---|
| 141 | //integration error |
---|
| 142 | old_true(4)=KalmanObs[4]; |
---|
| 143 | old_true(5)=KalmanObs[5];// add U |
---|
| 144 | interr = (true_state - pfxu->eval(old_true)); |
---|
| 145 | |
---|
| 146 | //second derivative |
---|
| 147 | IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu); |
---|
| 148 | if (pf) {secder=pf->eval2o(vec_2(KalmanObs[4],KalmanObs[5]));} |
---|
| 149 | |
---|
[326] | 150 | EKFfull::bayes(dt); |
---|
[318] | 151 | old_true.set_subvector(0,true_state); |
---|
| 152 | } |
---|
| 153 | |
---|
| 154 | void log_add(logger &L, const string &name="" ){ |
---|
| 155 | L_CRB=L.add(rx,"crb"); |
---|
| 156 | L_err=L.add(rx,"err"); |
---|
| 157 | L_sec=L.add(rx,"d2"); |
---|
| 158 | } |
---|
| 159 | void logit(logger &L){ |
---|
| 160 | L.logit(L_err, interr); |
---|
[326] | 161 | L.logit(L_CRB,diag(_R())); |
---|
[318] | 162 | L.logit(L_sec,secder); |
---|
| 163 | } |
---|
| 164 | }; |
---|