[61] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
| 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertainty |
---|
| 8 | |
---|
| 9 | Using IT++ for numerical operations |
---|
| 10 | ----------------------------------- |
---|
| 11 | */ |
---|
| 12 | |
---|
| 13 | #ifndef EKFfix_H |
---|
| 14 | #define EKFfix_H |
---|
| 15 | |
---|
| 16 | #include <itpp/itbase.h> |
---|
| 17 | #include <estim/libKF.h> |
---|
| 18 | #include "fixed.h" |
---|
| 19 | #include "matrix.h" |
---|
| 20 | #include "reference.h" |
---|
| 21 | #include "parametry_motoru.h" |
---|
| 22 | |
---|
| 23 | using namespace itpp; |
---|
| 24 | |
---|
| 25 | double minQ(double Q); |
---|
| 26 | |
---|
| 27 | /*! |
---|
[145] | 28 | \brief Extended Kalman Filter with full matrices in fixed point arithmetic |
---|
[61] | 29 | |
---|
| 30 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 31 | */ |
---|
| 32 | class EKFfixed : public BM, public BMcond { |
---|
| 33 | public: |
---|
| 34 | void init_ekf(double Tv); |
---|
| 35 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 36 | |
---|
| 37 | /* Declaration of local functions */ |
---|
| 38 | void prediction(int *ux); |
---|
| 39 | void correction(void); |
---|
| 40 | void update_psi(void); |
---|
| 41 | |
---|
| 42 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 43 | int Q[16]; /* matrix [4,4] */ |
---|
| 44 | int R[4]; /* matrix [2,2] */ |
---|
| 45 | |
---|
| 46 | int x_est[4]; |
---|
| 47 | int x_pred[4]; |
---|
| 48 | int P_pred[16]; /* matrix [4,4] */ |
---|
| 49 | int P_est[16]; /* matrix [4,4] */ |
---|
| 50 | int Y_mes[2]; |
---|
| 51 | int ukalm[2]; |
---|
| 52 | int Kalm[8]; /* matrix [5,2] */ |
---|
| 53 | |
---|
| 54 | int PSI[16]; /* matrix [4,4] */ |
---|
| 55 | |
---|
| 56 | int temp15a[16]; |
---|
| 57 | |
---|
| 58 | int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 59 | |
---|
| 60 | long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
| 61 | enorm<fsqmat> E; |
---|
| 62 | mat Ry; |
---|
| 63 | |
---|
| 64 | public: |
---|
| 65 | //! Default constructor |
---|
| 66 | EKFfixed ( RV rvx,RV rvc ):BM(rvx),BMcond(rvc),E(rvx),Ry(2,2){ |
---|
| 67 | int i; |
---|
| 68 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 69 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 70 | |
---|
| 71 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 72 | for(i=0;i<4;i++){x_pred[i]=0;} |
---|
| 73 | for(i=0;i<16;i++){P_pred[i]=0;} |
---|
| 74 | for(i=0;i<16;i++){P_est[i]=0;} |
---|
| 75 | P_est[0]=0x7FFF; |
---|
| 76 | P_est[5]=0x7FFF; |
---|
| 77 | P_est[10]=0x7FFF; |
---|
| 78 | P_est[15]=0x7FFF; |
---|
| 79 | for(i=0;i<2;i++){Y_mes[i]=0;} |
---|
| 80 | for(i=0;i<2;i++){ukalm[i]=0;} |
---|
| 81 | for(i=0;i<8;i++){Kalm[i]=0;} |
---|
| 82 | |
---|
| 83 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 84 | }; |
---|
| 85 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 86 | void bayes ( const vec &dt ); |
---|
| 87 | //!dummy! |
---|
| 88 | epdf& _epdf(){return E;}; |
---|
| 89 | void condition ( const vec &Q0 ) { |
---|
| 90 | |
---|
| 91 | Q[0]=prevod(minQ(Q0(0)),15); // 0.05 |
---|
| 92 | Q[5]=prevod(minQ(Q0(1)),15); |
---|
| 93 | Q[10]=prevod(minQ(Q0(2)),15); // 1e-3 |
---|
| 94 | Q[15]=prevod(minQ(Q0(3)),15); // 1e-3 |
---|
| 95 | |
---|
| 96 | } |
---|
| 97 | }; |
---|
| 98 | |
---|
| 99 | |
---|
| 100 | #endif // KF_H |
---|
| 101 | |
---|