| 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 Uncertaint16y |
|---|
| 8 | |
|---|
| 9 | Using IT++ for numerical operations |
|---|
| 10 | ----------------------------------- |
|---|
| 11 | */ |
|---|
| 12 | |
|---|
| 13 | #ifndef EKF_mm_H |
|---|
| 14 | #define EKF_mm_H |
|---|
| 15 | |
|---|
| 16 | #include "fixed.h" |
|---|
| 17 | #include "matrix.h" |
|---|
| 18 | #include "matrix_vs.h" |
|---|
| 19 | #include "reference_Q15.h" |
|---|
| 20 | #include "parametry_motoru.h" |
|---|
| 21 | |
|---|
| 22 | struct ekf_data{ |
|---|
| 23 | int16 Q[4]; /* matrix [4,4] */ |
|---|
| 24 | int16 dR[2]; /* diag of matrix [2,2] */ |
|---|
| 25 | |
|---|
| 26 | int16 x_est[2]; /* estimate and prediction */ |
|---|
| 27 | int16 x_pred[2]; /* estimate and prediction */ |
|---|
| 28 | int16 y_est[2]; /* estimate and prediction */ |
|---|
| 29 | int16 y_old[2]; /* estimate and prediction */ |
|---|
| 30 | |
|---|
| 31 | int16 PSI[4]; /* matrix [4,4] */ |
|---|
| 32 | int16 PSICh[4]; /* matrix PIS*U, [4,4] */ |
|---|
| 33 | int16 C[4]; /* matrix [4,4] */ |
|---|
| 34 | |
|---|
| 35 | int16 Chf[4]; // upper triangular of covariance (inplace) |
|---|
| 36 | |
|---|
| 37 | int16 difz[2]; |
|---|
| 38 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
|---|
| 39 | }; |
|---|
| 40 | |
|---|
| 41 | |
|---|
| 42 | void init_ekfCh2( ekf_data *E, double Tv); |
|---|
| 43 | void ekfCh2(ekf_data *E, int16 ux, int16 uy, int16 isx, int16 isy, int32 *detS, int16 *rem); |
|---|
| 44 | |
|---|
| 45 | void ekfmm(ekf_data *E1, ekf_data *E2, int16 ux, int16 uy, int16 isx, int16 isy); |
|---|
| 46 | |
|---|
| 47 | |
|---|
| 48 | #endif // KF_H |
|---|
| 49 | |
|---|
