[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 | //!Extended Kalman filter with unknown \c Q |
---|
[37] | 23 | class EKF_unQ : public EKFCh , public BMcond { |
---|
[33] | 24 | public: |
---|
| 25 | //! Default constructor |
---|
[37] | 26 | EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {}; |
---|
| 27 | void condition ( const vec &Q0 ) { |
---|
| 28 | Q.setD ( Q0,0 ); |
---|
| 29 | //from EKF |
---|
| 30 | preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); |
---|
| 31 | }; |
---|
[33] | 32 | }; |
---|
| 33 | |
---|
| 34 | int main() { |
---|
| 35 | // Kalman filter |
---|
| 36 | int Ndat = 10000; |
---|
| 37 | |
---|
| 38 | // cout << KF; |
---|
| 39 | // internal model |
---|
| 40 | IMpmsm fxu; |
---|
| 41 | // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) |
---|
| 42 | fxu.set_parameters ( 0.28, 0.003465, 20*1e-6, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); |
---|
| 43 | // observation model |
---|
| 44 | OMpmsm hxu; |
---|
| 45 | |
---|
| 46 | vec mu0= "100 100 100 1"; |
---|
| 47 | vec Qdiag ( "0.1 0.1 0.01 0.00001" ); |
---|
| 48 | vec Rdiag ( "0.02 0.02" ); |
---|
| 49 | vec vQ = "0.01:0.01:100"; |
---|
| 50 | |
---|
[37] | 51 | chmat Q ( Qdiag ); |
---|
| 52 | chmat R ( Rdiag ); |
---|
[33] | 53 | |
---|
| 54 | RV rQ ( "100","{Q}","2","0" ); |
---|
| 55 | EKF_unQ KFE ( rx,ry,ru,rQ ); |
---|
| 56 | KFE.set_parameters ( &fxu,&hxu,Q,R ); |
---|
[37] | 57 | KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); |
---|
[33] | 58 | |
---|
| 59 | mgamma evolQ ( rQ,rQ ); |
---|
[37] | 60 | //evolQ.set_parameters ( 10000.0 ); //sigma = 1/10 mu |
---|
[33] | 61 | |
---|
| 62 | MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,100,KFE ); |
---|
| 63 | |
---|
| 64 | epdf& KFEep = KFE._epdf(); |
---|
| 65 | epdf& Mep = M._epdf(); |
---|
| 66 | // initialize |
---|
[37] | 67 | evolQ.set_parameters ( 1.0 ); //sigma = 1/10 mu |
---|
[33] | 68 | evolQ.condition ( "0.5 0.5" ); |
---|
| 69 | epdf& pfinit=evolQ._epdf(); |
---|
| 70 | M.set_est ( pfinit ); |
---|
[37] | 71 | evolQ.set_parameters ( 1000.0 ); //sigma = 1/10 mu |
---|
[33] | 72 | |
---|
| 73 | //simulator values |
---|
[37] | 74 | vec dt ( 2 ); // output (isa isb) |
---|
| 75 | vec wt ( 2 ); // noise on dt |
---|
| 76 | vec ut ( 2 ); // |
---|
| 77 | vec xt=mu0; // initial state |
---|
| 78 | vec et ( 4 ); // noise on xt |
---|
[33] | 79 | |
---|
[37] | 80 | mat Xt=zeros ( 4,Ndat ); // True trajetory of xt |
---|
| 81 | mat XtE=zeros ( 4,Ndat ); // Estimate of xt using EKF (known Q) |
---|
| 82 | mat XtM=zeros ( 6,Ndat ); // Estimate of xt using EKF-MPF |
---|
| 83 | Xt.set_col ( 0,mu0 ); |
---|
[33] | 84 | XtM.set_col ( 0,Mep.mean() ); |
---|
| 85 | |
---|
| 86 | for ( int t=1;t<Ndat;t++ ) { |
---|
| 87 | //simulator |
---|
[37] | 88 | // UniRNG.sample_vector ( 2,wt ); |
---|
[33] | 89 | |
---|
| 90 | if ( rem ( t,500 ) <200 ) ut = rem ( t,500 ) *ones ( 2 ); |
---|
| 91 | else |
---|
| 92 | ut=zeros ( 2 ); |
---|
| 93 | |
---|
| 94 | NorRNG.sample_vector ( 4,et ); |
---|
| 95 | NorRNG.sample_vector ( 2,wt ); |
---|
| 96 | xt = fxu.eval ( xt,ut ) + Q.sqrt_mult ( et ); |
---|
| 97 | dt = hxu.eval ( xt,ut ) + R.sqrt_mult ( wt ); |
---|
| 98 | |
---|
| 99 | //estimator |
---|
| 100 | KFE.bayes ( concat ( dt,ut ) ); |
---|
| 101 | M.bayes ( concat ( dt,ut ) ); |
---|
| 102 | |
---|
| 103 | Xt.set_col ( t,xt ); |
---|
| 104 | XtE.set_col ( t,KFEep.mean() ); |
---|
| 105 | XtM.set_col ( t,Mep.mean() ); |
---|
| 106 | } |
---|
| 107 | |
---|
| 108 | it_file fou ( "pmsm.it" ); |
---|
| 109 | |
---|
| 110 | fou << Name ( "xth" ) << Xt; |
---|
| 111 | fou << Name ( "xthE" ) << XtE; |
---|
| 112 | fou << Name ( "xthM" ) << XtM; |
---|
| 113 | //Exit program: |
---|
| 114 | return 0; |
---|
| 115 | |
---|
| 116 | } |
---|