[228] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Voltage U is multiplied by an unknown weight w which is estimated by MPF |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | \ingroup PMSM |
---|
| 7 | |
---|
| 8 | ----------------------------------- |
---|
| 9 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
| 10 | |
---|
| 11 | Using IT++ for numerical operations |
---|
| 12 | ----------------------------------- |
---|
| 13 | */ |
---|
| 14 | |
---|
| 15 | |
---|
[262] | 16 | |
---|
[228] | 17 | #include <estim/libKF.h> |
---|
| 18 | #include <estim/libPF.h> |
---|
| 19 | #include <stat/libFN.h> |
---|
| 20 | |
---|
| 21 | #include "pmsm.h" |
---|
| 22 | #include "simulator.h" |
---|
| 23 | #include "sim_profiles.h" |
---|
| 24 | |
---|
[254] | 25 | using namespace bdm; |
---|
[228] | 26 | |
---|
| 27 | //!Extended Kalman filter with unknown \c Q |
---|
| 28 | class EKFCh_cond : public EKFCh , public BMcond { |
---|
| 29 | public: |
---|
| 30 | //! Default constructor |
---|
| 31 | EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {}; |
---|
| 32 | void condition ( const vec &val ) { |
---|
| 33 | pfxu->condition ( val ); |
---|
| 34 | }; |
---|
| 35 | }; |
---|
| 36 | |
---|
| 37 | class IMpmsm_w : public IMpmsm { |
---|
| 38 | protected: |
---|
| 39 | double w; |
---|
| 40 | public: |
---|
| 41 | IMpmsm_w() :IMpmsm(),w ( 1.0 ) {}; |
---|
| 42 | //! Set mechanical and electrical variables |
---|
| 43 | |
---|
| 44 | void condition ( const vec &val ) {w = val ( 0 );} |
---|
| 45 | vec eval ( const vec &x0, const vec &u0 ) { |
---|
| 46 | // last state |
---|
| 47 | double iam = x0 ( 0 ); |
---|
| 48 | double ibm = x0 ( 1 ); |
---|
| 49 | double omm = x0 ( 2 ); |
---|
| 50 | double thm = x0 ( 3 ); |
---|
| 51 | double uam = u0 ( 0 ); |
---|
| 52 | double ubm = u0 ( 1 ); |
---|
| 53 | |
---|
| 54 | vec xk=zeros ( 4 ); |
---|
| 55 | //ia |
---|
| 56 | xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + w* uam*dt/Ls; |
---|
| 57 | //ib |
---|
| 58 | xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + w* ubm*dt/Ls; |
---|
| 59 | //om |
---|
| 60 | xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz; |
---|
| 61 | //th |
---|
| 62 | xk ( 3 ) = thm + omm*dt; // <0..2pi> |
---|
| 63 | if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi; |
---|
| 64 | if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi; |
---|
| 65 | return xk; |
---|
| 66 | } |
---|
| 67 | |
---|
| 68 | }; |
---|
| 69 | |
---|
| 70 | |
---|
| 71 | int main() { |
---|
| 72 | // Kalman filter |
---|
| 73 | int Ndat = 9000; |
---|
| 74 | double h = 1e-6; |
---|
| 75 | int Nsimstep = 125; |
---|
| 76 | int Npart = 200; |
---|
| 77 | |
---|
| 78 | mat Rnoise = randn ( 2,Ndat ); |
---|
| 79 | |
---|
| 80 | // internal model |
---|
| 81 | IMpmsm fxu0; |
---|
| 82 | IMpmsm_w fxu; |
---|
| 83 | // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) |
---|
| 84 | fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); |
---|
| 85 | fxu0.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); |
---|
| 86 | // observation model |
---|
| 87 | OMpmsm hxu; |
---|
| 88 | |
---|
| 89 | vec mu0= "0.0 0.0 0.0 0.0"; |
---|
| 90 | vec Qdiag ( "0.1 0.1 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001 |
---|
| 91 | vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034" |
---|
| 92 | chmat Q ( Qdiag ); |
---|
| 93 | chmat R ( Rdiag ); |
---|
| 94 | EKFCh KFE ( rx,ry,ru ); |
---|
| 95 | KFE.set_parameters ( &fxu0,&hxu,Q,R ); |
---|
| 96 | KFE.set_est ( mu0, chmat ( ones ( 4 ) ) ); |
---|
| 97 | |
---|
| 98 | RV rW ( "{w }" ); |
---|
| 99 | EKFCh_cond KFEp ( rx,ry,ru,rW ); |
---|
| 100 | KFEp.set_parameters ( &fxu,&hxu,Q,R ); |
---|
| 101 | KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) ); |
---|
| 102 | |
---|
| 103 | mgamma_fix evolW ( rW,rW ); |
---|
| 104 | MPF<EKFCh_cond> M ( rx,rW,evolW,evolW,Npart,KFEp ); |
---|
| 105 | // initialize |
---|
| 106 | vec W0="0.5"; |
---|
| 107 | evolW.set_parameters ( 10.0, W0, 1.0 ); //sigma = 1/10 mu |
---|
| 108 | evolW.condition ( W0 ); |
---|
| 109 | epdf& pfinit=evolW._epdf(); |
---|
| 110 | M.set_est ( pfinit ); |
---|
| 111 | evolW.set_parameters ( 100.0, W0, 0.99 ); //sigma = 1/10 mu |
---|
| 112 | |
---|
| 113 | mat Xt=zeros ( Ndat ,4 ); //true state from simulator |
---|
| 114 | mat Dt=zeros ( Ndat,2+2 ); //observation |
---|
| 115 | mat XtE=zeros ( Ndat, 4 ); |
---|
| 116 | mat Qtr=zeros ( Ndat, 4 ); |
---|
| 117 | mat XtM=zeros ( Ndat,1+4 ); //W + x |
---|
| 118 | mat XtMTh=zeros ( Ndat,1 ); //W + x |
---|
| 119 | |
---|
| 120 | // SET SIMULATOR |
---|
| 121 | pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h ); |
---|
| 122 | vec dt ( 2 ); |
---|
| 123 | vec ut ( 2 ); |
---|
| 124 | vec xt ( 4 ); |
---|
| 125 | vec xtm=zeros ( 4 ); |
---|
| 126 | double Ww=0.0; |
---|
| 127 | vec vecW="1 2 4 8 4 2 0 -4 -9 -16 -4 0 0 0"; |
---|
| 128 | vecW*=10.0; |
---|
| 129 | |
---|
| 130 | for ( int tK=1;tK<Ndat;tK++ ) { |
---|
| 131 | //Number of steps of a simulator for one step of Kalman |
---|
| 132 | for ( int ii=0; ii<Nsimstep;ii++ ) { |
---|
| 133 | //simulator |
---|
| 134 | sim_profile_vec01t ( Ww,vecW ); |
---|
| 135 | pmsmsim_step ( Ww ); |
---|
| 136 | }; |
---|
| 137 | ut ( 0 ) = KalmanObs[0]; |
---|
| 138 | ut ( 1 ) = KalmanObs[1]; |
---|
| 139 | dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t ); |
---|
| 140 | dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t ); |
---|
| 141 | xt = vec ( x,4 ); |
---|
| 142 | |
---|
| 143 | //estimator |
---|
| 144 | KFE.bayes ( concat ( dt,ut ) ); |
---|
| 145 | M.bayes ( concat ( dt,ut ) ); |
---|
| 146 | |
---|
| 147 | Xt.set_row ( tK, xt ); //vec from C-array |
---|
| 148 | Dt.set_row ( tK, concat ( dt,ut ) ); |
---|
| 149 | Qtr.set_row ( tK, Qdiag ); |
---|
| 150 | XtE.set_row ( tK,KFE._e()->mean() ); |
---|
| 151 | XtM.set_row ( tK,M._e()->mean() ); |
---|
| 152 | { |
---|
| 153 | double sumSin=0.0; |
---|
| 154 | double sumCos=0.0; |
---|
| 155 | vec mea ( 4 ); |
---|
| 156 | vec* _w; |
---|
| 157 | |
---|
| 158 | for ( int p=0; p<Npart;p++ ) { |
---|
| 159 | mea = M._BM ( p )->_e()->mean(); |
---|
| 160 | _w = M.__w(); |
---|
| 161 | sumSin += ( *_w ) ( p ) *sin ( mea ( 3 ) ); |
---|
| 162 | sumCos += ( *_w ) ( p ) *cos ( mea ( 3 ) ); |
---|
| 163 | } |
---|
| 164 | double Th = asin ( sumSin ); |
---|
| 165 | if ( sumCos<0 ) { |
---|
| 166 | if ( sumSin>0 ) { |
---|
| 167 | Th = M_PI-Th; |
---|
| 168 | } |
---|
| 169 | else { |
---|
| 170 | Th = -M_PI-Th; |
---|
| 171 | } |
---|
| 172 | } |
---|
| 173 | |
---|
| 174 | XtMTh.set_row ( tK,vec_1 ( Th ) ); |
---|
| 175 | } |
---|
| 176 | } |
---|
| 177 | |
---|
| 178 | it_file fou ( "mpf_u_weight.it" ); |
---|
| 179 | |
---|
| 180 | fou << Name ( "xth" ) << Xt; |
---|
| 181 | fou << Name ( "Dt" ) << Dt; |
---|
| 182 | fou << Name ( "Qtr" ) << Qtr; |
---|
| 183 | fou << Name ( "xthE" ) << XtE; |
---|
| 184 | fou << Name ( "xthM" ) << XtM; |
---|
| 185 | fou << Name ( "xthMTh" ) << XtMTh; |
---|
| 186 | //Exit program: |
---|
| 187 | |
---|
| 188 | return 0; |
---|
| 189 | } |
---|