Changeset 66
- Timestamp:
- 04/09/08 15:39:48 (17 years ago)
- Location:
- simulator_zdenek/ekf_example
- Files:
-
- 3 modified
Legend:
- Unmodified
- Added
- Removed
-
simulator_zdenek/ekf_example/CMakeLists.txt
r61 r66 1 1 add_executable (simulace fixed.cpp matrix.cpp pmsm_mod.cpp pwm.cpp regulace.cpp ekf.cpp simul.cpp ) 2 2 3 include_directories(../../bdm) 3 4 add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp) -
simulator_zdenek/ekf_example/ekf_obj.cpp
r61 r66 3 3 4 4 #include "ekf_obj.h" 5 #include "../simulator.h" 5 6 6 7 double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};}; … … 9 10 void EKFfixed::bayes(const vec &dt){ 10 11 ekf(dt(2),dt(3),dt(0),dt(1)); 12 vec xhat(4); 13 //UGLY HACK!!! reliance on a predictor!! 14 xhat(0)=zprevod(x_est[0],Qm)*Iref; 15 xhat(1)=zprevod(x_est[1],Qm)*Iref; 16 xhat(2)=zprevod(x_est[2],Qm)*Wref; 17 xhat(3)=zprevod(x_est[3],15); 18 19 E.set_mu(xhat); 11 20 12 21 if ( BM::evalll ) { 13 22 //from enorm 14 vec ydif(2); 15 ydif(0)=dt(0)-zprevod(x_pred[0],Qm)*Iref; 16 ydif(1)=dt(1)-zprevod(x_pred[1],Qm)*Iref; 23 vec xdif(x,4);//first 4 of x 24 //UGLY HACK!!! reliance on a predictor!! 25 /* xdif(0)=x[0]-zprevod(x_pred[0],Qm)*Iref; 26 xdif(1)=x[1]-zprevod(x_pred[1],Qm)*Iref; 27 xdif(2)=x[2]-zprevod(x_pred[2],Qm)*Wref; 28 xdif(3)=x[3]-zprevod(x_pred[3],15);*/ 17 29 18 BM::ll = -0.5* ( 2 * 1.83787706640935 +log ( det ( Ry ) ) +ydif* ( inv(Ry)*ydif ) ); 30 xdif -=xhat; //(xdif=x-xhat) 31 32 mat Pfull(4,4); 33 double* Pp=Pfull._data(); 34 for(int i=0;i<16;i++){*(Pp++) = zprevod(P_est[i],15);} 35 36 BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) ); 19 37 } 20 38 }; -
simulator_zdenek/ekf_example/simul.cpp
r57 r66 1 /* Hlavni simulacni program1 /* Hlavni simulacni program 2 2 3 3 Simulace tramvaje s PMSM v plovouci radove carce