[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 | |
---|
[1243] | 13 | #ifndef PMSM_DS |
---|
| 14 | #define PMSM_DS |
---|
| 15 | |
---|
[654] | 16 | #include <base/loggers.h> |
---|
[384] | 17 | #include <estim/kalman.h> |
---|
[349] | 18 | #include "simulator_zdenek/simulator.h" |
---|
[258] | 19 | #include "pmsm.h" |
---|
| 20 | |
---|
[366] | 21 | using namespace bdm; |
---|
| 22 | |
---|
[258] | 23 | //! Simulator of PMSM machine with predefined profile on omega |
---|
[357] | 24 | class pmsmDS : public DS |
---|
| 25 | { |
---|
[871] | 26 | LOG_LEVEL(pmsmDS,logvoltage); |
---|
[1380] | 27 | |
---|
[258] | 28 | protected: |
---|
[897] | 29 | //! indices of logged variables |
---|
[357] | 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 |
---|
[871] | 38 | int Dt; |
---|
[258] | 39 | public: |
---|
[1387] | 40 | double x[9]; |
---|
| 41 | double KalmanObs[6]; |
---|
[357] | 42 | //! Constructor with fixed sampling period |
---|
[686] | 43 | pmsmDS () : DS() |
---|
[357] | 44 | { |
---|
| 45 | Dt=125; |
---|
[895] | 46 | Drv=RV ( "{o_ua o_ub o_ia o_ib t_ua t_ub o_om o_th Mz }" ); |
---|
| 47 | dtsize = Drv._dsize(); |
---|
[1380] | 48 | pmsmsim_fill_xy(x,KalmanObs); |
---|
[357] | 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 | } |
---|
[871] | 54 | |
---|
| 55 | void getdata ( vec &dt ) const |
---|
[357] | 56 | { |
---|
| 57 | dt.set_subvector(0,vec ( KalmanObs,6 )); |
---|
[1345] | 58 | // add measurement noise |
---|
[1377] | 59 | dt(2) += (0.02*randun()-0.01); |
---|
| 60 | dt(3) += (0.02*randun()-0.01); |
---|
[1345] | 61 | |
---|
| 62 | // true values |
---|
[357] | 63 | dt(6)=x[2]; |
---|
| 64 | dt(7)=x[3]; |
---|
| 65 | dt(8)=x[8]; |
---|
| 66 | } |
---|
| 67 | void write ( vec &ut ) {} |
---|
[258] | 68 | |
---|
[357] | 69 | void step() |
---|
| 70 | { |
---|
[1380] | 71 | pmsmsim_fill_xy(x,KalmanObs); |
---|
[357] | 72 | static int ind=0; |
---|
| 73 | static double dW; // increase of W |
---|
| 74 | static double Ww; // W |
---|
| 75 | static double Mz; // W |
---|
[1380] | 76 | double t; |
---|
| 77 | t=pmsmsim_get_t(); |
---|
[357] | 78 | if ( t>=dt_prof*ind ) |
---|
| 79 | { |
---|
| 80 | ind++; |
---|
| 81 | // check omega profile and set dW |
---|
[654] | 82 | if ( ind <2 && profileWw.length() ==1 ) |
---|
| 83 | { |
---|
| 84 | Ww=profileWw ( 0 ); |
---|
| 85 | dW=0.0; |
---|
| 86 | } |
---|
| 87 | if ( ind<profileWw.length() ) |
---|
[357] | 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 | } |
---|
[326] | 113 | // for ( int i=0;i<Dt;i++ ) { pmsmsim_noreg_step ( Ww , Mz);} |
---|
[332] | 114 | |
---|
[357] | 115 | //discretization |
---|
[1198] | 116 | /* double ustep=1.2; |
---|
[357] | 117 | KalmanObs [ 0 ] = ustep*itpp::round( KalmanObs [ 0 ]/ ustep) ; |
---|
[1198] | 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); |
---|
[258] | 122 | |
---|
[357] | 123 | }; |
---|
[258] | 124 | |
---|
[1153] | 125 | void log_register ( logger &L , const string &prefix) |
---|
[357] | 126 | { |
---|
[1153] | 127 | DS::log_register ( L, prefix ); |
---|
| 128 | |
---|
| 129 | // log differences |
---|
[871] | 130 | if ( log_level[logvoltage] ) |
---|
[357] | 131 | { |
---|
[1243] | 132 | L.add_vector ( log_level, logvoltage, RV("{ua, ub }"), "model" ); |
---|
[357] | 133 | } |
---|
| 134 | } |
---|
[280] | 135 | |
---|
[676] | 136 | void log_write ( logger &L ) |
---|
[357] | 137 | { |
---|
[871] | 138 | if ( log_level[logvoltage] ) |
---|
[357] | 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] ); |
---|
[258] | 148 | |
---|
[357] | 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; |
---|
[1153] | 154 | log_level.store( logvoltage , vec_2 ( ua,ub ) ); |
---|
[357] | 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 | { |
---|
[654] | 168 | const SettingResolver& params_l(root["params"]); |
---|
| 169 | const Setting ¶ms = params_l.result; |
---|
[366] | 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 | |
---|
[357] | 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; |
---|
[894] | 178 | UI::get( tstep, root, "tstep"); |
---|
[357] | 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; |
---|
[258] | 186 | }; |
---|
[318] | 187 | |
---|
[357] | 188 | UIREGISTER ( pmsmDS ); |
---|
| 189 | |
---|
[1243] | 190 | //! Simulator of PMSM machine with predefined profile on omega |
---|
| 191 | class pmsmDSctrl : public pmsmDS |
---|
| 192 | { |
---|
| 193 | double Ww; |
---|
| 194 | double ua; |
---|
| 195 | double ub; |
---|
| 196 | public: |
---|
| 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 | { |
---|
[1387] | 218 | pmsmsim_fill_xy(x,KalmanObs); |
---|
| 219 | double t; |
---|
[1380] | 220 | |
---|
[1243] | 221 | static int ind=0; |
---|
| 222 | static double dW; // increase of W |
---|
| 223 | static double Mz; // W |
---|
[1387] | 224 | t=pmsmsim_get_t(); |
---|
[1243] | 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); |
---|
| 259 | } |
---|
| 260 | }; |
---|
| 261 | |
---|
| 262 | }; |
---|
| 263 | |
---|
| 264 | UIREGISTER ( pmsmDSctrl ); |
---|
| 265 | |
---|
[357] | 266 | |
---|
[1380] | 267 | #ifdef XXX |
---|
[318] | 268 | //! This class behaves like BM but it is evaluating EKF |
---|
[357] | 269 | class pmsmCRB : public EKFfull |
---|
| 270 | { |
---|
| 271 | protected: |
---|
| 272 | vec interr; |
---|
| 273 | vec old_true; |
---|
| 274 | vec secder; |
---|
| 275 | int L_CRB; |
---|
| 276 | int L_err; |
---|
| 277 | int L_sec; |
---|
| 278 | public: |
---|
| 279 | //! constructor |
---|
| 280 | pmsmCRB():EKFfull() |
---|
| 281 | { |
---|
| 282 | old_true=zeros(6); |
---|
| 283 | } |
---|
[318] | 284 | |
---|
[357] | 285 | void bayes(const vec &dt) |
---|
| 286 | { |
---|
| 287 | static vec umin(2); |
---|
[654] | 288 | vec u(2); |
---|
[686] | 289 | vec &mu = est._mu(); |
---|
[654] | 290 | //assume we know state exactly: |
---|
[1380] | 291 | vec true_state=0;//TODO vec(x,4); // read from pmsm |
---|
[357] | 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 |
---|
[654] | 302 | IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu.get()); |
---|
[357] | 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 | { |
---|
[654] | 315 | shared_ptr<diffbifn> IM = UI::build<diffbifn>(root, "IM"); |
---|
| 316 | shared_ptr<diffbifn> OM = UI::build<diffbifn>(root, "OM"); |
---|
[357] | 317 | |
---|
| 318 | //parameters |
---|
| 319 | |
---|
| 320 | //statistics |
---|
| 321 | int dim=IM->dimension(); |
---|
[359] | 322 | |
---|
[357] | 323 | vec mu0; |
---|
[359] | 324 | if(root.exists("mu0")) |
---|
| 325 | UI::get( mu0, root, "mu0"); |
---|
| 326 | else |
---|
[357] | 327 | mu0=zeros(dim); |
---|
[359] | 328 | |
---|
[357] | 329 | mat P0; |
---|
[359] | 330 | if(root.exists( "dP0" )) |
---|
| 331 | { |
---|
| 332 | vec dP0; |
---|
| 333 | UI::get(dP0,root, "dP0"); |
---|
[357] | 334 | P0=diag(dP0); |
---|
[359] | 335 | } |
---|
| 336 | else if (root.exists("P0")) |
---|
| 337 | UI::get(P0,root, "P0"); |
---|
| 338 | else |
---|
[357] | 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 |
---|
[654] | 350 | shared_ptr<RV> drv = UI::build<RV>(root, "drv"); |
---|
[679] | 351 | set_yrv(*drv); |
---|
[654] | 352 | shared_ptr<RV> rv = UI::build<RV>(root, "rv"); |
---|
[357] | 353 | set_rv(*rv); |
---|
| 354 | } |
---|
| 355 | |
---|
| 356 | // TODO dodelat void to_setting( Setting &root ) const; |
---|
[332] | 357 | }; |
---|
[342] | 358 | |
---|
[357] | 359 | UIREGISTER ( pmsmCRB ); |
---|
| 360 | |
---|
| 361 | |
---|
[342] | 362 | //! This class behaves like BM but it is evaluating EKF |
---|
[357] | 363 | class pmsmCRBMz : public EKFfull |
---|
| 364 | { |
---|
| 365 | protected: |
---|
| 366 | int L_CRB; |
---|
| 367 | public: |
---|
| 368 | //! constructor |
---|
| 369 | pmsmCRBMz():EKFfull() {} |
---|
[342] | 370 | |
---|
[357] | 371 | void bayes(const vec &dt) |
---|
| 372 | { |
---|
[342] | 373 | //assume we know state exactly: |
---|
[357] | 374 | vec true_state(5); |
---|
| 375 | true_state.set_subvector(0,vec(x,4)); // read from pmsm |
---|
| 376 | true_state(4)=x[8]; |
---|
| 377 | |
---|
[686] | 378 | vec &mu = est._mu(); |
---|
[357] | 379 | mu = true_state; |
---|
| 380 | //hack for ut |
---|
| 381 | EKFfull::bayes(dt); |
---|
| 382 | } |
---|
| 383 | |
---|
| 384 | |
---|
| 385 | void from_setting( const Setting &root ) |
---|
| 386 | { |
---|
[654] | 387 | shared_ptr<diffbifn> IM = UI::build<diffbifn>(root,"IM"); |
---|
| 388 | shared_ptr<diffbifn> OM = UI::build<diffbifn>(root,"OM"); |
---|
[357] | 389 | |
---|
| 390 | //statistics |
---|
| 391 | int dim=IM->dimension(); |
---|
| 392 | vec mu0; |
---|
[359] | 393 | if( root.exists( "mu0")) |
---|
| 394 | UI::get(mu0, root, "mu0"); |
---|
| 395 | else |
---|
[357] | 396 | mu0=zeros(dim); |
---|
| 397 | |
---|
[359] | 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 | |
---|
[357] | 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 |
---|
[654] | 420 | shared_ptr<RV> drv = UI::build<RV>(root, "drv"); |
---|
[679] | 421 | set_yrv(*drv); |
---|
[654] | 422 | shared_ptr<RV> rv = UI::build<RV>(root, "rv"); |
---|
[357] | 423 | set_rv(*rv); |
---|
| 424 | } |
---|
| 425 | |
---|
| 426 | // TODO dodelat void to_setting( Setting &root ) const; |
---|
[342] | 427 | }; |
---|
[357] | 428 | |
---|
| 429 | UIREGISTER ( pmsmCRBMz ); |
---|
[1380] | 430 | #endif |
---|
[1243] | 431 | |
---|
| 432 | #endif |
---|