- Timestamp:
- 08/02/11 16:02:37 (13 years ago)
- Files:
-
- 1 copied
Legend:
- Unmodified
- Added
- Removed
-
applications/pmsm/simulator_zdenek/ekf_example/simulmpf.cpp
r326 r1381 17 17 #include "pwm.h" 18 18 #include "regulace.h" 19 #include " ekf.h"19 #include "mpf_double.h" 20 20 21 21 #define Qm 13 … … 56 56 57 57 static double Isx, Isy; 58 static double Usx, Usy, ua, ub, Umk; 58 59 static double theta; 59 60 static double Uc_mer; … … 64 65 65 66 // EKF - vysledek estimace 66 static double ekf_estim[2]={0.,0.}; // w_est, theta_est 67 static double mpf_estim[2]={0.,0.}; // w_est, theta_est 68 static double Esinth, Ecosth; 67 69 68 70 FILE *fw; … … 113 115 init_pmsm(param, REL1); 114 116 init_regulace(param,h_reg); 115 init_ekf(h_reg,param);116 117 fw=fopen("data/graf 1.txt","w");117 mpf_init(1e0, 1e-5, 1e0);//VS: 118 119 fw=fopen("data/graf2.txt","w"); 118 120 119 121 Idwf=0.; … … 215 217 if (x[2]>1615) {x[2]=1615; if (k_rampa_tmp<24000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}} 216 218 if (x[2]<-1615) x[2]=-1615; /**/ 217 219 218 220 // KALMAN 219 ekf(ekf_estim,*u,*(u+1),Ucn,Uc_mer,Isx,Isy); 221 Umk=*u*Uc/Ucn; 222 ua=Umk*cos(*(u+1)); 223 ub=Umk*cos(*(u+1)-2./3.*M_PI); 224 Usx=ua; // usx 225 Usy=(ua+2.*ub)/sqrt(3.); // usy 226 227 mpf_bayes(Isx,Isy,Usx,Usy); 228 mpf_mean(&Esinth,&Ecosth,&mpf_estim[0]); 229 mpf_estim[1]=atan(Esinth/Ecosth); 220 230 221 231 // vystup z EKF zaveden do regulatoru … … 243 253 244 254 if (t>=t_sense) 245 if (print_counter>199) 246 { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),Urm_max,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),Uc, *(ladeni_regulace+7),ladeni_pila,ladeni_ur,*(ladeni_regulace+8),*(ladeni_regulace+9),ekf_estim[0],ekf_estim[1]); 247 // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max, 255 if (print_counter>125)//sampling period 256 { fprintf(fw,"%f %f %f %f %f %f %f \n", t,x[0],x[1],Usx,Usy,mpf_estim[0],mpf_estim[1]); // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max, 248 257 // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc 249 258 print_counter=0;