Changeset 226
- Timestamp:
- 01/12/09 13:28:19 (16 years ago)
- Files:
-
- 1 modified
Legend:
- Unmodified
- Added
- Removed
-
pmsm/mpf_u_delta.cpp
r224 r226 31 31 EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {}; 32 32 void condition ( const vec &val ) { 33 pfxu->condition ( val );33 pfxu->condition ( val ); 34 34 }; 35 35 }; 36 36 37 37 class IMpmsm_delta : public IMpmsm { 38 39 40 41 IMpmsm_delta() :IMpmsm(),ud(2) {};38 protected: 39 vec ud; 40 public: 41 IMpmsm_delta() :IMpmsm(),ud ( 2 ) {ud=zeros ( 2 );}; 42 42 //! Set mechanical and electrical variables 43 43 44 void condition(const vec &val){ud = val;}45 44 void condition ( const vec &val ) {ud = val;} 45 vec eval ( const vec &x0, const vec &u0 ) { 46 46 // last state 47 48 49 50 51 52 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 53 54 54 vec xk=zeros ( 4 ); 55 55 //ia 56 xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + (uam+ud(0))*dt/Ls;56 xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + ( uam+ud ( 0 ) ) *dt/Ls; 57 57 //ib 58 xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + (ubm+ud(1))*dt/Ls;58 xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ( ubm+ud ( 1 ) ) *dt/Ls; 59 59 //om 60 60 xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz; 61 61 //th 62 63 64 65 66 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 67 68 68 }; … … 76 76 int Npart = 200; 77 77 78 mat Rnoise = randn ( 2,Ndat ); 79 78 80 // internal model 81 IMpmsm fxu0; 79 82 IMpmsm_delta fxu; 80 83 // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) 81 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 ); 82 86 // observation model 83 87 OMpmsm hxu; 84 88 85 89 vec mu0= "0.0 0.0 0.0 0.0"; 86 vec Qdiag ( "0. 6 0.60.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.000187 vec Rdiag ( " 1e-8 1e-8" ); //var(diff(xth)) = "0.034 0.034"90 vec Qdiag ( "0.4 0.4 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" 88 92 chmat Q ( Qdiag ); 89 93 chmat R ( Rdiag ); 90 94 EKFCh KFE ( rx,ry,ru ); 91 KFE.set_parameters ( &fxu ,&hxu,Q,R );92 KFE.set_est ( mu0, chmat ( zeros ( 4 ) ) );95 KFE.set_parameters ( &fxu0,&hxu,Q,R ); 96 KFE.set_est ( mu0, chmat ( ones ( 4 ) ) ); 93 97 94 98 RV rUd ( "{ud }", "2" ); 95 99 EKFCh_cond KFEp ( rx,ry,ru,rUd ); 96 100 KFEp.set_parameters ( &fxu,&hxu,Q,R ); 97 KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );101 KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) ); 98 102 99 103 mlnorm<ldmat> evolUd ( rUd,rUd ); … … 101 105 // initialize 102 106 vec Ud0="0 0"; 103 evolUd.set_parameters ( eye (2), vec_2(0.0,0.0), ldmat(eye(2)) );104 evolUd.condition ( Ud0 );107 evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 10.0*eye ( 2 ) ) ); 108 evolUd.condition ( Ud0 ); 105 109 epdf& pfinit=evolUd._epdf(); 106 110 M.set_est ( pfinit ); 107 evolUd.set_parameters ( eye (2), vec_2(0.0,0.0), ldmat(0.1*eye(2)) );108 111 evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 0.1*eye ( 2 ) ) ); 112 109 113 mat Xt=zeros ( Ndat ,4 ); //true state from simulator 110 mat Dt=zeros ( Ndat,2+2 ); //observation114 mat Dt=zeros ( Ndat,2+2+2 ); //observation 111 115 mat XtE=zeros ( Ndat, 4 ); 112 116 mat Qtr=zeros ( Ndat, 4 ); 113 117 mat XtM=zeros ( Ndat,2+4 ); //W + x 118 mat XtMTh=zeros ( Ndat,1 ); 114 119 115 120 // SET SIMULATOR … … 118 123 vec ut ( 2 ); 119 124 vec xt ( 4 ); 120 vec xtm=zeros (4);125 vec xtm=zeros ( 4 ); 121 126 double Ww=0.0; 122 vec vecW="1 2 4 94 2 0 -4 -9 -16 -4 0 0 0";127 vec vecW="1 2 4 8 4 2 0 -4 -9 -16 -4 0 0 0"; 123 128 vecW*=10.0; 124 129 … … 127 132 for ( int ii=0; ii<Nsimstep;ii++ ) { 128 133 //simulator 129 sim_profile_vec01t (Ww,vecW);134 sim_profile_vec01t ( Ww,vecW ); 130 135 pmsmsim_step ( Ww ); 131 136 }; 132 ut (0) = KalmanObs[0];133 ut (1) = KalmanObs[1];134 dt (0) = KalmanObs[2];135 dt (1) = KalmanObs[3];136 xt = vec (x,4);137 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 138 143 //estimator 139 144 KFE.bayes ( concat ( dt,ut ) ); 140 145 M.bayes ( concat ( dt,ut ) ); 141 146 142 Xt.set_row ( tK, xt ); //vec from C-array143 Dt.set_row ( tK, concat ( dt,ut ));144 Qtr.set_row ( tK, Qdiag );147 Xt.set_row ( tK, xt ); //vec from C-array 148 Dt.set_row ( tK, concat ( dt,ut, vec_2 ( KalmanObs[4],KalmanObs[5] ) ) ); 149 Qtr.set_row ( tK, Qdiag ); 145 150 XtE.set_row ( tK,KFE._e()->mean() ); 146 151 XtM.set_row ( tK,M._e()->mean() ); 152 // correction for theta 153 154 { 155 double sumSin=0.0; 156 double sumCos=0.0; 157 vec mea ( 4 ); 158 vec* _w; 159 160 for ( int p=0; p<Npart;p++ ) { 161 mea = M._BM ( p )->_e()->mean(); 162 _w = M.__w(); 163 sumSin += ( *_w ) ( p ) *sin ( mea ( 3 ) ); 164 sumCos += ( *_w ) ( p ) *cos ( mea ( 3 ) ); 165 } 166 double Th = atan2 ( sumSin, sumCos ); 167 168 XtMTh.set_row ( tK,vec_1 ( Th ) ); 169 } 170 147 171 } 148 172 … … 154 178 fou << Name ( "xthE" ) << XtE; 155 179 fou << Name ( "xthM" ) << XtM; 180 fou << Name ( "xthMTh" ) << XtMTh; 156 181 //Exit program: 157 182