Changeset 232
- Timestamp:
- 01/15/09 10:53:57 (16 years ago)
- Location:
- pmsm
- Files:
-
- 3 modified
Legend:
- Unmodified
- Added
- Removed
-
pmsm/mpf_u_delta.cpp
r230 r232 84 84 85 85 // internal model 86 IMpmsm fxu0;86 IMpmsm2o fxu0; 87 87 IMpmsm_delta fxu; 88 88 // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) … … 94 94 vec mu0= "0.0 0.0 0.0 0.0"; 95 95 vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001 96 vec Rdiag ( "0. 2 0.2" ); //var(diff(xth)) = "0.034 0.034"96 vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034" 97 97 chmat Q ( Qdiag ); 98 mat Q2o=diag(Qdiag); 98 99 chmat R ( Rdiag ); 99 EKFCh KFE ( rx,ry,r u);100 EKFCh KFE ( rx,ry,rU ); 100 101 KFE.set_parameters ( &fxu0,&hxu,Q,R ); 101 102 KFE.set_est ( mu0, chmat ( ones ( 4 ) ) ); … … 110 111 MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp ); 111 112 // initialize 112 vec Ud0="0 0 1 .0";113 evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 0.0001" )));113 vec Ud0="0 0 100.0"; 114 evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 10" ))); 114 115 evolUd.condition ( Ud0 ); 115 116 epdf& pfinit=evolUd._epdf(); 116 117 M.set_est ( pfinit ); 117 evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 4e-3 4e-3 1e-3" )));118 evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 9e-8 9e-8 9e-7" ))); 118 119 119 120 mat Xt=zeros ( Ndat ,4 ); //true state from simulator … … 127 128 pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h ); 128 129 vec dt ( 2 ); 129 vec ut ( 2 ); 130 vec ut ( 4 ); 131 vec utm ( 4 ); 130 132 vec xt ( 4 ); 131 133 vec xtm=zeros ( 4 ); … … 143 145 ut ( 0 ) = KalmanObs[0]; 144 146 ut ( 1 ) = KalmanObs[1]; 147 ut ( 2 ) = utm(0)-ut(0); 148 ut ( 3 ) = utm(1)-ut(1); 145 149 dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t ); 146 150 dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t ); 147 151 xt = vec ( x,4 ); 148 152 153 utm =ut; 154 155 Q2o*=0.9; 156 Q2o+=outer_product(x2o_dbg,x2o_dbg); 149 157 //estimator 150 158 KFE.bayes ( concat ( dt,ut ) ); 159 for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(Q2o));} 151 160 M.bayes ( concat ( dt,ut ) ); 152 161 153 162 Xt.set_row ( tK, xt ); //vec from C-array 154 Dt.set_row ( tK, concat ( dt,ut , vec_2 ( KalmanObs[4],KalmanObs[5] ) ) );163 Dt.set_row ( tK, concat ( dt,ut.left(2), vec_2 ( KalmanObs[4],KalmanObs[5] ) ) ); 155 164 Qtr.set_row ( tK, Qdiag ); 156 165 XtE.set_row ( tK,KFE._e()->mean() ); -
pmsm/pmsm.h
r224 r232 12 12 RV ru ( "{ua ub }"); 13 13 RV ry ( "{oia oib }"); 14 RV rU = concat(ru, RV("{dua dub }")); 14 15 15 16 // class uipmsm : public uibase{ 16 17 // double Rs, Ls, dt, Ypm, kp, p, J, Mz; 17 18 // }; 19 20 vec x2o_dbg(4); 18 21 19 22 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$ … … 74 77 }; 75 78 79 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$ 80 class IMpmsm2o : public diffbifn { 81 protected: 82 double Rs, Ls, dt, Ypm, kp, p, J, Mz; 83 84 public: 85 IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {}; 86 //! Set mechanical and electrical variables 87 void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;} 88 89 vec eval ( const vec &x0, const vec &u0 ) { 90 // last state 91 double iam = x0 ( 0 ); 92 double ibm = x0 ( 1 ); 93 double omm = x0 ( 2 ); 94 double thm = x0 ( 3 ); 95 double uam = u0 ( 0 ); 96 double ubm = u0 ( 1 ); 97 double dua = u0 ( 2 )/dt; 98 double dub = u0 ( 3 )/dt; 99 100 double cth = cos(thm); 101 double sth = sin(thm); 102 double d2t = dt*dt/2; 103 104 double dia = (- Rs/Ls*iam + Ypm/Ls*omm * sth + uam/Ls); 105 double dib = (- Rs/Ls*ibm - Ypm/Ls*omm * cth + ubm/Ls); 106 double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz; 107 double dth = omm; 108 109 double d2ia = (- Rs/Ls*dia + Ypm/Ls*(dom * sth + omm*cth) + dua/Ls); 110 double d2ib = (- Rs/Ls*dib - Ypm/Ls*(dom * cth - omm*sth) + dub/Ls); 111 double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth)); 112 double d2th = dom; 113 114 vec xk=zeros ( 4 ); 115 xk ( 0 ) = iam + dt*dia;// +d2t*d2ia; 116 xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib; 117 xk ( 2 ) = omm +dt*dom;// +d2t*d2om; 118 xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi> 119 120 x2o_dbg(0)=d2t*d2ia; 121 x2o_dbg(1)=d2t*d2ib; 122 x2o_dbg(2)=d2t*d2om; 123 x2o_dbg(3)=d2t*d2th; 124 125 if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi; 126 if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi; 127 return xk; 128 } 129 130 void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) { 131 double iam = x0 ( 0 ); 132 double ibm = x0 ( 1 ); 133 double omm = x0 ( 2 ); 134 double thm = x0 ( 3 ); 135 // d ia 136 A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0; 137 A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) ); 138 // d ib 139 A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt ); 140 A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) ); 141 // d om 142 A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) ); 143 A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) ); 144 A ( 2,2 ) = 1.0; 145 A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) ); 146 // d th 147 A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0; 148 } 149 150 void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );}; 151 152 }; 153 76 154 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$ 77 155 class IMpmsmStat : public IMpmsm { -
pmsm/sim_var.cpp
r224 r232 32 32 int Nsimstep = 125; 33 33 34 dirfilelog L("exp/sim_var ",1000);34 dirfilelog L("exp/sim_var2",1000); 35 35 //memlog L(Ndat); 36 36 37 37 38 // SET SIMULATOR … … 39 40 double Ww = 0.0; 40 41 vec dt ( 2 ); 41 vec ut ( 2 ); 42 vec ut ( 4 ); 43 vec utm ( 4 ); 42 44 vec dut ( 2 ); 43 45 vec dit (2); … … 47 49 vec xt ( 4 ); 48 50 vec ddif=zeros(2); 49 IMpmsm fxu;51 IMpmsm2o fxu; 50 52 // Rs Ls dt Fmag(Ypm) kp p J Bf(Mz) 51 53 fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 ); … … 60 62 mat Q =diag( Qdiag ); 61 63 mat R =diag ( Rdiag ); 62 EKFfull Efix ( rx,ry,r u);64 EKFfull Efix ( rx,ry,rU ); 63 65 Efix.set_est ( mu0, 1*eye ( 4 ) ); 64 66 Efix.set_parameters ( &fxu,&hxu,Q,R); 65 67 66 EKFfull Eop ( rx,ry,r u);68 EKFfull Eop ( rx,ry,rU ); 67 69 Eop.set_est ( mu0, 1*eye ( 4 ) ); 68 70 Eop.set_parameters ( &fxu,&hxu,Q,R); 69 71 70 EKFfull Edi ( rx,ry,r u);72 EKFfull Edi ( rx,ry,rU ); 71 73 Edi.set_est ( mu0, 1*eye ( 4 ) ); 72 74 Edi.set_parameters ( &fxu,&hxu,Q,R); … … 80 82 RV rR( "{R }", "4"); 81 83 RV rUD( "{u_isa u_isb i_isa i_isb }", ones_i(4)); 82 RV rDu("{dux duy duxf duyf }",ones_i(4));84 RV rDu("{dux duy }",ones_i(2)); 83 85 RV rDi("{disa disb }",ones_i(2)); 84 86 int X_log = L.add(rx,"X"); 87 int X2o_log = L.add(rx,"X2o"); 88 int Xdf_log = L.add(rx,"Xdf"); 85 89 int Efix_log = L.add(rx,"XF"); 86 90 int Eop_log = L.add(rx,"XO"); … … 100 104 }; 101 105 // simulation via deterministic model 102 ut ( 0 ) = KalmanObs[0]; 103 ut ( 1 ) = KalmanObs[1]; 106 ut ( 0 ) = KalmanObs[4]; 107 ut ( 1 ) = KalmanObs[5]; 108 ut ( 2 ) = utm(0)-ut(0); 109 ut ( 3 ) = utm(1)-ut(1); 110 111 utm = ut; 112 104 113 dt ( 0 ) = KalmanObs[2]; 105 114 dt ( 1 ) = KalmanObs[3]; … … 147 156 //LOG 148 157 L.logit(X_log, vec(x,4)); //vec from C-array 158 L.logit(X2o_log, x2o_dbg); 159 L.logit(Xdf_log, xdif); 149 160 L.logit(Efix_log, Efix_ep.mean() ); 150 161 L.logit(Eop_log, Eop_ep.mean() );