[33] | 1 | /* |
---|
| 2 | \file |
---|
| 3 | \brief Models for synchronous electric drive using IT++ and BDM |
---|
| 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 <itpp/itbase.h> |
---|
| 14 | #include <estim/libKF.h> |
---|
| 15 | #include <estim/libPF.h> |
---|
| 16 | #include <stat/libFN.h> |
---|
| 17 | |
---|
| 18 | #include "pmsm.h" |
---|
| 19 | |
---|
| 20 | using namespace itpp; |
---|
| 21 | /* |
---|
| 22 | // PMSM with Q on Ia and Ib given externally |
---|
| 23 | class EKF_unQ : public EKF<ldmat> , public BMcond { |
---|
| 24 | public: |
---|
| 25 | EKF_unQ( rx,ry,ru,rQ):EKF<ldmat>(rx,ry,ru),BMcond(rQ){}; |
---|
| 26 | void condition(const vec &Q0){}; |
---|
| 27 | };*/ |
---|
| 28 | |
---|
| 29 | int main() { |
---|
| 30 | // Kalman filter |
---|
| 31 | int Ndat = 10000; |
---|
| 32 | |
---|
| 33 | // cout << KF; |
---|
| 34 | // internal model |
---|
| 35 | IMpmsm fxu; |
---|
| 36 | // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) |
---|
| 37 | fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); |
---|
| 38 | // observation model |
---|
| 39 | OMpmsm hxu; |
---|
| 40 | |
---|
| 41 | vec mu0= "100 100 100 1"; |
---|
| 42 | vec Qdiag ( "0.1 0.1 0.01 0.00001" ); |
---|
| 43 | vec Rdiag ( "0.02 0.02" ); |
---|
| 44 | vec vQ = "0.01:0.01:100"; |
---|
| 45 | ldmat Q = ldmat ( Qdiag ); |
---|
| 46 | ldmat R = ldmat ( Rdiag ); |
---|
| 47 | EKF<ldmat> KFE ( rx,ry,ru ); |
---|
| 48 | KFE.set_parameters ( &fxu,&hxu,Q,R ); |
---|
| 49 | KFE.set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) ); |
---|
| 50 | |
---|
| 51 | mat ll(100,Ndat); |
---|
| 52 | |
---|
| 53 | EKF<ldmat>* kfArray[100]; |
---|
| 54 | |
---|
| 55 | for ( int i=0;i<100;i++ ) { |
---|
| 56 | vec Qid ( Qdiag ); |
---|
| 57 | Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i ); |
---|
| 58 | kfArray[i]= new EKF<ldmat> ( rx,ry,ru ); |
---|
| 59 | kfArray[i]->set_parameters ( &fxu,&hxu,ldmat ( Qid ),R ); |
---|
| 60 | kfArray[i]->set_est ( mu0, ldmat ( 1000*ones ( 4 ) ) ); |
---|
| 61 | } |
---|
| 62 | |
---|
| 63 | epdf& KFEep = KFE._epdf(); |
---|
| 64 | |
---|
| 65 | //simulator values |
---|
| 66 | vec dt ( 2 ); |
---|
| 67 | vec wt ( 2 ); |
---|
| 68 | vec ut ( 2 ); |
---|
| 69 | vec xt=mu0; |
---|
| 70 | vec et ( 4 ); |
---|
| 71 | |
---|
| 72 | mat Xt=zeros ( 4,Ndat ); |
---|
| 73 | mat XtE=zeros ( 4,Ndat ); |
---|
| 74 | Xt.set_col ( 0,KFEep.mean() ); |
---|
| 75 | |
---|
| 76 | for ( int t=1;t<Ndat;t++ ) { |
---|
| 77 | //simulator |
---|
| 78 | UniRNG.sample_vector ( 2,wt ); |
---|
| 79 | |
---|
| 80 | if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 ); |
---|
| 81 | else |
---|
| 82 | ut=zeros ( 2 ); |
---|
| 83 | |
---|
| 84 | NorRNG.sample_vector ( 4,et ); |
---|
| 85 | NorRNG.sample_vector ( 2,wt ); |
---|
| 86 | xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et ); |
---|
| 87 | dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt ); |
---|
| 88 | |
---|
| 89 | //estimator |
---|
| 90 | KFE.bayes ( concat ( dt,ut ) ); |
---|
| 91 | for ( int i=0;i<100;i++ ) {kfArray[i]->bayes( concat ( dt,ut ) );ll(i,t)+=kfArray[i]->_ll();} |
---|
| 92 | |
---|
| 93 | Xt.set_col ( t,xt ); |
---|
| 94 | XtE.set_col ( t,KFEep.mean() ); |
---|
| 95 | } |
---|
| 96 | |
---|
| 97 | it_file fou ( "pmsm.it" ); |
---|
| 98 | |
---|
| 99 | fou << Name ( "xth" ) << Xt; |
---|
| 100 | fou << Name ( "xthE" ) << XtE; |
---|
| 101 | fou << Name ( "ll" ) << ll; |
---|
| 102 | //Exit program: |
---|
| 103 | return 0; |
---|
| 104 | |
---|
| 105 | } |
---|