[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 | |
---|
[262] | 16 | |
---|
[384] | 17 | #include <estim/kalman.h> |
---|
[61] | 18 | #include "fixed.h" |
---|
| 19 | #include "matrix.h" |
---|
[1174] | 20 | #include "matrix_vs.h" |
---|
[61] | 21 | #include "reference.h" |
---|
| 22 | #include "parametry_motoru.h" |
---|
| 23 | |
---|
[254] | 24 | using namespace bdm; |
---|
[61] | 25 | |
---|
| 26 | double minQ(double Q); |
---|
| 27 | |
---|
[1174] | 28 | void mat_to_int(const imat &M, int *I); |
---|
| 29 | void vec_to_int(const ivec &v, int *I); |
---|
| 30 | |
---|
[61] | 31 | /*! |
---|
[145] | 32 | \brief Extended Kalman Filter with full matrices in fixed point arithmetic |
---|
[61] | 33 | |
---|
| 34 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 35 | */ |
---|
[283] | 36 | class EKFfixed : public BM { |
---|
[61] | 37 | public: |
---|
| 38 | void init_ekf(double Tv); |
---|
| 39 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 40 | |
---|
| 41 | /* Declaration of local functions */ |
---|
| 42 | void prediction(int *ux); |
---|
| 43 | void correction(void); |
---|
| 44 | void update_psi(void); |
---|
| 45 | |
---|
| 46 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 47 | int Q[16]; /* matrix [4,4] */ |
---|
| 48 | int R[4]; /* matrix [2,2] */ |
---|
| 49 | |
---|
| 50 | int x_est[4]; |
---|
| 51 | int x_pred[4]; |
---|
| 52 | int P_pred[16]; /* matrix [4,4] */ |
---|
| 53 | int P_est[16]; /* matrix [4,4] */ |
---|
| 54 | int Y_mes[2]; |
---|
| 55 | int ukalm[2]; |
---|
| 56 | int Kalm[8]; /* matrix [5,2] */ |
---|
| 57 | |
---|
| 58 | int PSI[16]; /* matrix [4,4] */ |
---|
| 59 | |
---|
| 60 | int temp15a[16]; |
---|
| 61 | |
---|
| 62 | int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 63 | |
---|
| 64 | long temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
| 65 | enorm<fsqmat> E; |
---|
| 66 | mat Ry; |
---|
| 67 | |
---|
| 68 | public: |
---|
| 69 | //! Default constructor |
---|
[283] | 70 | EKFfixed ():BM(),E(),Ry(2,2){ |
---|
[1168] | 71 | int i; |
---|
| 72 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 73 | for(i=0;i<4;i++){R[i]=0;} |
---|
[61] | 74 | |
---|
[1168] | 75 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 76 | for(i=0;i<4;i++){x_pred[i]=0;} |
---|
| 77 | for(i=0;i<16;i++){P_pred[i]=0;} |
---|
| 78 | for(i=0;i<16;i++){P_est[i]=0;} |
---|
| 79 | P_est[0]=0x7FFF; |
---|
| 80 | P_est[5]=0x7FFF; |
---|
| 81 | P_est[10]=0x7FFF; |
---|
| 82 | P_est[15]=0x7FFF; |
---|
| 83 | for(i=0;i<2;i++){Y_mes[i]=0;} |
---|
| 84 | for(i=0;i<2;i++){ukalm[i]=0;} |
---|
| 85 | for(i=0;i<8;i++){Kalm[i]=0;} |
---|
[61] | 86 | |
---|
[1168] | 87 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 88 | |
---|
| 89 | set_dim(4); |
---|
| 90 | E._mu()=zeros(4); |
---|
| 91 | E._R()=zeros(4,4); |
---|
| 92 | init_ekf(0.000125); |
---|
| 93 | }; |
---|
[61] | 94 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[1168] | 95 | void bayes ( const vec &yt, const vec &ut ); |
---|
[61] | 96 | //!dummy! |
---|
[1168] | 97 | const epdf& posterior() const {return E;}; |
---|
| 98 | |
---|
[61] | 99 | }; |
---|
| 100 | |
---|
[1168] | 101 | UIREGISTER(EKFfixed); |
---|
[61] | 102 | |
---|
[1179] | 103 | /*! |
---|
| 104 | \brief Extended Kalman Filter with UD matrices in fixed point arithmetic |
---|
| 105 | |
---|
| 106 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 107 | */ |
---|
| 108 | class EKFfixedUD : public BM { |
---|
| 109 | public: |
---|
| 110 | void init_ekf(double Tv); |
---|
| 111 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 112 | |
---|
| 113 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 114 | int Q[16]; /* matrix [4,4] */ |
---|
| 115 | int R[4]; /* matrix [2,2] */ |
---|
| 116 | |
---|
| 117 | int x_est[4]; /* estimate and prediction */ |
---|
| 118 | |
---|
| 119 | int PSI[16]; /* matrix [4,4] */ |
---|
| 120 | int PSIU[16]; /* matrix PIS*U, [4,4] */ |
---|
| 121 | |
---|
| 122 | int Uf[16]; // upper triangular of covariance (inplace) |
---|
| 123 | int Df[4]; // diagonal covariance |
---|
| 124 | int Dfold[4]; // temp of D |
---|
| 125 | int G[16]; // temp for bierman |
---|
| 126 | |
---|
| 127 | int cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 128 | |
---|
| 129 | enorm<fsqmat> E; |
---|
| 130 | mat Ry; |
---|
| 131 | |
---|
| 132 | public: |
---|
| 133 | //! Default constructor |
---|
| 134 | EKFfixedUD ():BM(),E(),Ry(2,2){ |
---|
| 135 | int i; |
---|
| 136 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 137 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 138 | |
---|
| 139 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 140 | for(i=0;i<16;i++){Uf[i]=0;} |
---|
| 141 | for(i=0;i<4;i++){Df[i]=0;} |
---|
| 142 | for(i=0;i<16;i++){G[i]=0;} |
---|
| 143 | for(i=0;i<4;i++){Dfold[i]=0;} |
---|
| 144 | |
---|
| 145 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 146 | |
---|
| 147 | set_dim(4); |
---|
| 148 | E._mu()=zeros(4); |
---|
| 149 | E._R()=zeros(4,4); |
---|
| 150 | init_ekf(0.000125); |
---|
| 151 | }; |
---|
| 152 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 153 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 154 | //!dummy! |
---|
| 155 | const epdf& posterior() const {return E;}; |
---|
| 156 | |
---|
| 157 | }; |
---|
| 158 | |
---|
| 159 | UIREGISTER(EKFfixedUD); |
---|
| 160 | |
---|
[1174] | 161 | //! EKF for comparison of EKF_UD with its fixed-point implementation |
---|
| 162 | class EKF_UDfix : public BM { |
---|
| 163 | protected: |
---|
| 164 | //! logger |
---|
| 165 | LOG_LEVEL(EKF_UDfix,logU, logG); |
---|
| 166 | //! Internal Model f(x,u) |
---|
| 167 | shared_ptr<diffbifn> pfxu; |
---|
| 168 | |
---|
| 169 | //! Observation Model h(x,u) |
---|
| 170 | shared_ptr<diffbifn> phxu; |
---|
| 171 | |
---|
| 172 | //! U part |
---|
| 173 | mat U; |
---|
| 174 | //! D part |
---|
| 175 | vec D; |
---|
| 176 | |
---|
| 177 | int Uf[25]; |
---|
| 178 | int Df[5]; |
---|
| 179 | int Dfold[5]; |
---|
| 180 | |
---|
| 181 | mat A; |
---|
| 182 | mat C; |
---|
| 183 | mat Q; |
---|
| 184 | vec R; |
---|
| 185 | |
---|
| 186 | int PSI[25]; |
---|
| 187 | int PSIU[25]; |
---|
| 188 | int Gf[25]; |
---|
| 189 | |
---|
| 190 | enorm<ldmat> est; |
---|
| 191 | |
---|
| 192 | |
---|
| 193 | public: |
---|
| 194 | |
---|
| 195 | //! copy constructor duplicated |
---|
| 196 | EKF_UDfix* _copy() const { |
---|
| 197 | return new EKF_UDfix(*this); |
---|
| 198 | } |
---|
| 199 | |
---|
| 200 | const enorm<ldmat>& posterior()const{return est;}; |
---|
| 201 | |
---|
| 202 | enorm<ldmat>& prior() { |
---|
| 203 | return const_cast<enorm<ldmat>&>(posterior()); |
---|
| 204 | } |
---|
| 205 | |
---|
| 206 | EKF_UDfix(){} |
---|
| 207 | |
---|
| 208 | |
---|
| 209 | EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){} |
---|
| 210 | |
---|
| 211 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 212 | void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 ); |
---|
| 213 | |
---|
| 214 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 215 | void bayes ( const vec &yt, const vec &cond = empty_vec ); |
---|
| 216 | |
---|
| 217 | void log_register ( bdm::logger& L, const string& prefix ){ |
---|
| 218 | BM::log_register ( L, prefix ); |
---|
| 219 | |
---|
| 220 | if ( log_level[logU] ) |
---|
| 221 | L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix ); |
---|
| 222 | if ( log_level[logG] ) |
---|
| 223 | L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix ); |
---|
| 224 | |
---|
| 225 | } |
---|
| 226 | /*! Create object from the following structure |
---|
| 227 | |
---|
| 228 | \code |
---|
| 229 | class = 'EKF_UD'; |
---|
| 230 | OM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 231 | IM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 232 | dQ = [...]; % vector containing diagonal of Q |
---|
| 233 | dR = [...]; % vector containing diagonal of R |
---|
| 234 | --- optional fields --- |
---|
| 235 | mu0 = [...]; % vector of statistics mu0 |
---|
| 236 | dP0 = [...]; % vector containing diagonal of P0 |
---|
| 237 | -- or -- |
---|
| 238 | P0 = [...]; % full matrix P0 |
---|
| 239 | --- inherited fields --- |
---|
| 240 | bdm::BM::from_setting |
---|
| 241 | \endcode |
---|
| 242 | If the optional fields are not given, they will be filled as follows: |
---|
| 243 | \code |
---|
| 244 | mu0 = [0,0,0,....]; % empty statistics |
---|
| 245 | P0 = eye( dim ); |
---|
| 246 | \endcode |
---|
| 247 | */ |
---|
| 248 | void from_setting ( const Setting &set ); |
---|
| 249 | |
---|
| 250 | void validate() {}; |
---|
| 251 | // TODO dodelat void to_setting( Setting &set ) const; |
---|
| 252 | |
---|
| 253 | }; |
---|
| 254 | UIREGISTER(EKF_UDfix); |
---|
| 255 | |
---|
| 256 | |
---|
[1179] | 257 | |
---|
| 258 | |
---|
[61] | 259 | #endif // KF_H |
---|
| 260 | |
---|