[654] | 1 | #ifndef PMSMFIL_H |
---|
| 2 | #define PMSMFIL_H |
---|
| 3 | |
---|
| 4 | #include "pmsm.h" |
---|
| 5 | #include <estim/particles.h> |
---|
| 6 | |
---|
| 7 | /*! \defgroup PMSM |
---|
| 8 | @{ |
---|
| 9 | */ |
---|
| 10 | |
---|
| 11 | using namespace bdm; |
---|
| 12 | |
---|
| 13 | //* FAILURE :((( |
---|
| 14 | class MPFpmsm: public MPF { |
---|
| 15 | private: |
---|
| 16 | |
---|
| 17 | double Rs, Ls, dt, Ypm, kp, p, J, Mz; |
---|
| 18 | |
---|
| 19 | |
---|
| 20 | //! modelling theta as normal random walk with 2pi correction |
---|
| 21 | class rwtheta: public mpdf_internal<enorm<fsqmat> > { |
---|
| 22 | double om_hat_dt; |
---|
| 23 | double om_var; |
---|
| 24 | double rth; |
---|
| 25 | MPFpmsm& p; |
---|
| 26 | public: |
---|
| 27 | rwtheta ( MPFpmsm& p0 ) :p ( p0 ) {}; |
---|
| 28 | void set_parameters ( double omh0, double omv0, double rth0 ) { |
---|
| 29 | om_hat_dt=omh0*p.dt; |
---|
| 30 | om_var=omv0; |
---|
| 31 | rth=rth0; |
---|
| 32 | |
---|
| 33 | mat R ( 1,1 ); |
---|
| 34 | R ( 0,0 ) =p.dt*p.dt*om_var+rth; |
---|
| 35 | vec mu ( 1 ); |
---|
| 36 | mu ( 0 ) = om_hat_dt; |
---|
| 37 | iepdf.set_parameters ( mu, fsqmat ( R ) ); |
---|
| 38 | } |
---|
| 39 | vec samplecond ( const vec &cond ) { |
---|
| 40 | iepdf._mu() =om_hat_dt+cond ( 0 ); |
---|
| 41 | |
---|
| 42 | // |
---|
| 43 | // iepdf._mu()= x[3];// ---------- |
---|
| 44 | vec th=iepdf.sample(); |
---|
| 45 | if ( th ( 0 ) >pi ) th ( 0 )-=2*pi; |
---|
| 46 | if ( th ( 0 ) <-pi ) th ( 0 ) +=2*pi; |
---|
| 47 | return th; |
---|
| 48 | } |
---|
| 49 | }; |
---|
| 50 | |
---|
| 51 | class PMSMlin: public KalmanFull { |
---|
| 52 | protected: |
---|
| 53 | MPFpmsm &p; |
---|
| 54 | double thm; |
---|
| 55 | public: |
---|
| 56 | PMSMlin ( MPFpmsm &p0 ) :p ( p0 ) {}; |
---|
| 57 | PMSMlin ( const PMSMlin &P0 ) : KalmanFull ( P0 ), p ( P0.p ), thm ( P0.thm ) {} |
---|
| 58 | BM* _copy_() const {return new PMSMlin ( *this );} |
---|
| 59 | |
---|
| 60 | void fillA() { |
---|
| 61 | //ia |
---|
| 62 | //using namespace p; |
---|
| 63 | A ( 0,0 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ; |
---|
| 64 | A ( 0,2 ) = p.Ypm/p.Ls*p.dt* sin ( thm ); |
---|
| 65 | //ib |
---|
| 66 | A ( 1,1 ) = ( 1.0- p.Rs/p.Ls*p.dt ) ; |
---|
| 67 | A ( 1,2 ) = - p.Ypm/p.Ls*p.dt* cos ( thm ); |
---|
| 68 | //om |
---|
| 69 | A ( 2,0 ) = p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( -sin ( thm ) ); |
---|
| 70 | A ( 2,1 ) =p.kp*p.p*p.p * p.Ypm/p.J*p.dt* ( cos ( thm ) ); |
---|
| 71 | A ( 2,2 ) = 1.0; |
---|
| 72 | } |
---|
| 73 | |
---|
| 74 | void condition ( const vec &cond ) { |
---|
| 75 | thm = cond ( 0 ); |
---|
| 76 | fillA(); |
---|
| 77 | } |
---|
| 78 | }; |
---|
| 79 | |
---|
| 80 | shared_ptr<rwtheta> rwt; |
---|
| 81 | |
---|
| 82 | vec dQ; |
---|
| 83 | vec dR; |
---|
| 84 | public: |
---|
| 85 | void from_setting ( const Setting &set ) { |
---|
| 86 | BM::from_setting ( set ); //reads drv |
---|
| 87 | |
---|
| 88 | const SettingResolver& params_b=set["params"]; |
---|
| 89 | const Setting& params=params_b.result; |
---|
| 90 | |
---|
| 91 | Rs= params["Rs"]; |
---|
| 92 | Ls = params["Ls"]; |
---|
| 93 | dt = 125e-6; |
---|
| 94 | Ypm=params["Fmag"]; |
---|
| 95 | kp= params["kp"]; |
---|
| 96 | p= params["p"]; |
---|
| 97 | J = params["J"]; |
---|
| 98 | |
---|
| 99 | UI::get ( dQ, set, "dQ", UI::compulsory ); |
---|
| 100 | UI::get ( dR, set, "dR", UI::compulsory ); |
---|
| 101 | |
---|
| 102 | mat Q = diag ( dQ ( 0,2 ) ); |
---|
| 103 | mat R = diag ( dR ( 0,1 ) ); |
---|
| 104 | |
---|
| 105 | mat A=zeros ( 3,3 ); |
---|
| 106 | mat B=concat_vertical ( dt/Ls*eye ( 2 ), zeros ( 1,2 ) ); |
---|
| 107 | mat C=zeros ( 2,3 ); |
---|
| 108 | C ( 0,0 ) =1.0; |
---|
| 109 | C ( 1,1 ) =1.0; |
---|
| 110 | mat D= zeros ( 2,2 ); |
---|
| 111 | |
---|
| 112 | shared_ptr<PMSMlin> kal=new PMSMlin ( *this ); |
---|
| 113 | kal->set_parameters ( A,B,C,D,Q,R ); |
---|
| 114 | kal->set_statistics ( zeros ( 3 ), eye ( 3 ) ); |
---|
| 115 | kal->condition ( vec_1 ( 0.0 ) ); |
---|
| 116 | kal->validate(); |
---|
| 117 | |
---|
| 118 | pf = new PF; |
---|
| 119 | rwt=new rwtheta ( *this ); |
---|
| 120 | pf->prior_from_set ( set ); |
---|
| 121 | pf->resmethod_from_set ( set ); |
---|
| 122 | pf->set_model ( rwt,new mpdf() ); |
---|
| 123 | |
---|
| 124 | shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::optional ); |
---|
| 125 | if ( rv ) { |
---|
| 126 | set_rv ( *rv ); |
---|
| 127 | } |
---|
| 128 | |
---|
| 129 | if ( posterior()._rv()._dsize() >0 ) { |
---|
| 130 | kal->set_rv ( posterior()._rv().subselect ( "0,1,2" ) ); |
---|
| 131 | pf->set_rv ( posterior()._rv().subselect ( "3" ) ); |
---|
| 132 | } |
---|
| 133 | |
---|
| 134 | this->set_BM ( *kal ); |
---|
| 135 | |
---|
| 136 | string opt; |
---|
| 137 | if ( UI::get ( opt,set,"options",UI::optional ) ) { |
---|
| 138 | set_options ( opt ); |
---|
| 139 | } |
---|
| 140 | |
---|
| 141 | validate(); |
---|
| 142 | } |
---|
[686] | 143 | void bayes ( const vec &yt, const vec &cond ) { |
---|
[654] | 144 | const vec &mu = posterior().mean(); |
---|
| 145 | const vec &Var = posterior().variance(); |
---|
| 146 | rwt->set_parameters ( mu ( 3 ),Var ( 3 ),dQ ( 3 ) ); |
---|
| 147 | // COPY from MPF bayes: |
---|
| 148 | int i; |
---|
| 149 | int n=pf->__w().length(); |
---|
| 150 | vec &lls = pf->_lls(); |
---|
| 151 | |
---|
| 152 | // generate samples - time step |
---|
| 153 | for ( int i = 0; i < n; i++ ) { |
---|
| 154 | const vec &mu = BMs ( i )->posterior().mean(); |
---|
| 155 | const vec &Var = BMs ( i )->posterior().variance(); |
---|
| 156 | rwt->set_parameters ( mu ( 2 ),Var ( 2 ),dQ ( 3 ) ); |
---|
| 157 | |
---|
| 158 | Array<vec> &smp=pf->__samples(); |
---|
| 159 | smp ( i ) = rwt->samplecond ( smp ( i ) ); |
---|
| 160 | lls ( i ) = 0; |
---|
| 161 | } |
---|
| 162 | // weight them - data step |
---|
| 163 | #pragma parallel for |
---|
| 164 | for ( i = 0; i < n; i++ ) { |
---|
[686] | 165 | BMs ( i ) -> bayes ( yt, concat(cond, pf->posterior()._sample ( i )) ); |
---|
[654] | 166 | lls ( i ) += BMs ( i )->_ll(); |
---|
| 167 | } |
---|
| 168 | |
---|
| 169 | pf->bayes_weights(); |
---|
| 170 | |
---|
| 171 | ivec ind; |
---|
| 172 | if ( pf->do_resampling() ) { |
---|
| 173 | pf->resample ( ind ); |
---|
| 174 | |
---|
| 175 | #pragma omp parallel for |
---|
| 176 | for ( i = 0; i < n; i++ ) { |
---|
| 177 | if ( ind ( i ) != i ) {//replace the current Bm by a new one |
---|
| 178 | delete BMs ( i ); |
---|
| 179 | BMs ( i ) = BMs ( ind ( i ) )->_copy_(); //copy constructor |
---|
| 180 | } |
---|
| 181 | }; |
---|
| 182 | } |
---|
| 183 | |
---|
| 184 | |
---|
| 185 | } |
---|
| 186 | }; |
---|
| 187 | |
---|
| 188 | UIREGISTER ( MPFpmsm ); |
---|
| 189 | |
---|
| 190 | |
---|
| 191 | |
---|
| 192 | /*!@}*/ |
---|
| 193 | #endif //PMSMFIL_H |
---|