ekf_obj.h itpp/itbase.h estim/libKF.h fixed.h matrix.h reference.h parametry_motoru.h work/git/mixpp/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp EKFfixed double double minQ (double Q) minQ double Q Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions. Vaclav Smidl. ----------------------------------- BDM++ - C++ library for Bayesian Decision Making under UncertaintyUsing IT++ for numerical operations ----------------------------------- #ifndefEKFfix_H #defineEKFfix_H #include<itpp/itbase.h> #include<estim/libKF.h> #include"fixed.h" #include"matrix.h" #include"reference.h" #include"parametry_motoru.h" usingnamespaceitpp; doubleminQ(doubleQ); classEKFfixed:publicBM,publicBMcond{ public: voidinit_ekf(doubleTv); voidekf(doubleux,doubleuy,doubleisxd,doubleisyd); /*Declarationoflocalfunctions*/ voidprediction(int*ux); voidcorrection(void); voidupdate_psi(void); /*Constants-definovatjakokonstanty???kdejevyhodnejsiabyvpametibyli?*/ intQ[16];/*matrix[4,4]*/ intR[4];/*matrix[2,2]*/ intx_est[4]; intx_pred[4]; intP_pred[16];/*matrix[4,4]*/ intP_est[16];/*matrix[4,4]*/ intY_mes[2]; intukalm[2]; intKalm[8];/*matrix[5,2]*/ intPSI[16];/*matrix[4,4]*/ inttemp15a[16]; intcA,cB,cC,cG,cH;//cD,cE,cF,cI...nepouzivane longtemp30a[4];/*matrix[2,2]-temporarymatrixforinversion*/ enorm<fsqmat>E; matRy; public: EKFfixed(RVrvx,RVrvc):BM(rvx),BMcond(rvc),E(rvx),Ry(2,2){ inti; for(i=0;i<16;i++){Q[i]=0;} for(i=0;i<4;i++){R[i]=0;} for(i=0;i<4;i++){x_est[i]=0;} for(i=0;i<4;i++){x_pred[i]=0;} for(i=0;i<16;i++){P_pred[i]=0;} for(i=0;i<16;i++){P_est[i]=0;} P_est[0]=0x7FFF; P_est[5]=0x7FFF; P_est[10]=0x7FFF; P_est[15]=0x7FFF; for(i=0;i<2;i++){Y_mes[i]=0;} for(i=0;i<2;i++){ukalm[i]=0;} for(i=0;i<8;i++){Kalm[i]=0;} for(i=0;i<16;i++){PSI[i]=0;} }; voidbayes(constvec&dt); epdf&_epdf(){returnE;}; voidcondition(constvec&Q0){ Q[0]=prevod(minQ(Q0(0)),15);//0.05 Q[5]=prevod(minQ(Q0(1)),15); Q[10]=prevod(minQ(Q0(2)),15);//1e-3 Q[15]=prevod(minQ(Q0(3)),15);//1e-3 } }; #endif//KF_H