[61] | 1 | /*! |
---|
| 2 | \file |
---|
| 3 | \brief Bayesian Filtering for linear Gaussian models (Kalman Filter) and extensions |
---|
| 4 | \author Vaclav Smidl. |
---|
| 5 | |
---|
| 6 | ----------------------------------- |
---|
[1240] | 7 | BDM++ - C++ library for Bayesian Decision Making under Uncertaint16y |
---|
[61] | 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" |
---|
[1226] | 21 | #include "reference_Q15.h" |
---|
[61] | 22 | #include "parametry_motoru.h" |
---|
| 23 | |
---|
[254] | 24 | using namespace bdm; |
---|
[61] | 25 | |
---|
| 26 | double minQ(double Q); |
---|
| 27 | |
---|
[1240] | 28 | void mat_to_int16(const imat &M, int16 *I); |
---|
| 29 | void vec_to_int16(const ivec &v, int16 *I); |
---|
[1201] | 30 | void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref); |
---|
[1240] | 31 | |
---|
| 32 | #ifdef XXX |
---|
[61] | 33 | /*! |
---|
[1240] | 34 | \brief Extended Kalman Filter with full matrices in fixed point16 arithmetic |
---|
[61] | 35 | |
---|
| 36 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 37 | */ |
---|
[283] | 38 | class EKFfixed : public BM { |
---|
[61] | 39 | public: |
---|
| 40 | void init_ekf(double Tv); |
---|
| 41 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 42 | |
---|
| 43 | /* Declaration of local functions */ |
---|
[1240] | 44 | void prediction(int16 *ux); |
---|
[61] | 45 | void correction(void); |
---|
| 46 | void update_psi(void); |
---|
| 47 | |
---|
| 48 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 49 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 50 | int16 R[4]; /* matrix [2,2] */ |
---|
[61] | 51 | |
---|
[1240] | 52 | int16 x_est[4]; |
---|
| 53 | int16 x_pred[4]; |
---|
| 54 | int16 P_pred[16]; /* matrix [4,4] */ |
---|
| 55 | int16 P_est[16]; /* matrix [4,4] */ |
---|
| 56 | int16 Y_mes[2]; |
---|
| 57 | int16 ukalm[2]; |
---|
| 58 | int16 Kalm[8]; /* matrix [5,2] */ |
---|
[61] | 59 | |
---|
[1240] | 60 | int16 PSI[16]; /* matrix [4,4] */ |
---|
[61] | 61 | |
---|
[1240] | 62 | int16 temp15a[16]; |
---|
[61] | 63 | |
---|
[1240] | 64 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[61] | 65 | |
---|
[1240] | 66 | int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ |
---|
[61] | 67 | enorm<fsqmat> E; |
---|
| 68 | mat Ry; |
---|
| 69 | |
---|
| 70 | public: |
---|
| 71 | //! Default constructor |
---|
[283] | 72 | EKFfixed ():BM(),E(),Ry(2,2){ |
---|
[1240] | 73 | int16 i; |
---|
[1168] | 74 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 75 | for(i=0;i<4;i++){R[i]=0;} |
---|
[61] | 76 | |
---|
[1168] | 77 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 78 | for(i=0;i<4;i++){x_pred[i]=0;} |
---|
| 79 | for(i=0;i<16;i++){P_pred[i]=0;} |
---|
| 80 | for(i=0;i<16;i++){P_est[i]=0;} |
---|
| 81 | P_est[0]=0x7FFF; |
---|
| 82 | P_est[5]=0x7FFF; |
---|
| 83 | P_est[10]=0x7FFF; |
---|
| 84 | P_est[15]=0x7FFF; |
---|
| 85 | for(i=0;i<2;i++){Y_mes[i]=0;} |
---|
| 86 | for(i=0;i<2;i++){ukalm[i]=0;} |
---|
| 87 | for(i=0;i<8;i++){Kalm[i]=0;} |
---|
[61] | 88 | |
---|
[1168] | 89 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 90 | |
---|
| 91 | set_dim(4); |
---|
| 92 | E._mu()=zeros(4); |
---|
| 93 | E._R()=zeros(4,4); |
---|
| 94 | init_ekf(0.000125); |
---|
| 95 | }; |
---|
[61] | 96 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[1168] | 97 | void bayes ( const vec &yt, const vec &ut ); |
---|
[61] | 98 | //!dummy! |
---|
[1168] | 99 | const epdf& posterior() const {return E;}; |
---|
| 100 | |
---|
[61] | 101 | }; |
---|
| 102 | |
---|
[1168] | 103 | UIREGISTER(EKFfixed); |
---|
[61] | 104 | |
---|
[1240] | 105 | #endif |
---|
| 106 | |
---|
[1264] | 107 | //! EKF for testing q44 |
---|
[1294] | 108 | class EKFtest: public EKF_UD{ |
---|
[1264] | 109 | void bayes ( const vec &yt, const vec &cond ) { |
---|
[1294] | 110 | EKF_UD::bayes(yt,cond); |
---|
| 111 | vec D = prior()._R()._D(); |
---|
| 112 | |
---|
| 113 | if (D(3)>10) D(3) = 10; |
---|
| 114 | |
---|
| 115 | prior()._R().__D()=D; |
---|
[1264] | 116 | } |
---|
| 117 | }; |
---|
| 118 | UIREGISTER(EKFtest); |
---|
| 119 | |
---|
[1179] | 120 | /*! |
---|
[1240] | 121 | \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
[1179] | 122 | |
---|
| 123 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 124 | */ |
---|
| 125 | class EKFfixedUD : public BM { |
---|
| 126 | public: |
---|
[1226] | 127 | LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP); |
---|
[1201] | 128 | |
---|
[1179] | 129 | void init_ekf(double Tv); |
---|
| 130 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 131 | |
---|
| 132 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 133 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 134 | int16 R[4]; /* matrix [2,2] */ |
---|
[1179] | 135 | |
---|
[1240] | 136 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1179] | 137 | |
---|
[1240] | 138 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 139 | int16 PSIU[16]; /* matrix PIS*U, [4,4] */ |
---|
[1179] | 140 | |
---|
[1240] | 141 | int16 Uf[16]; // upper triangular of covariance (inplace) |
---|
| 142 | int16 Df[4]; // diagonal covariance |
---|
| 143 | int16 Dfold[4]; // temp of D |
---|
| 144 | int16 G[16]; // temp for bierman |
---|
[1179] | 145 | |
---|
[1240] | 146 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1179] | 147 | |
---|
| 148 | enorm<fsqmat> E; |
---|
| 149 | mat Ry; |
---|
| 150 | |
---|
| 151 | public: |
---|
| 152 | //! Default constructor |
---|
| 153 | EKFfixedUD ():BM(),E(),Ry(2,2){ |
---|
[1240] | 154 | int16 i; |
---|
[1179] | 155 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 156 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 157 | |
---|
| 158 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 159 | for(i=0;i<16;i++){Uf[i]=0;} |
---|
| 160 | for(i=0;i<4;i++){Df[i]=0;} |
---|
| 161 | for(i=0;i<16;i++){G[i]=0;} |
---|
| 162 | for(i=0;i<4;i++){Dfold[i]=0;} |
---|
| 163 | |
---|
| 164 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 165 | |
---|
| 166 | set_dim(4); |
---|
[1304] | 167 | dimy = 2; |
---|
| 168 | dimc = 2; |
---|
[1179] | 169 | E._mu()=zeros(4); |
---|
| 170 | E._R()=zeros(4,4); |
---|
| 171 | init_ekf(0.000125); |
---|
| 172 | }; |
---|
| 173 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 174 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 175 | //!dummy! |
---|
| 176 | const epdf& posterior() const {return E;}; |
---|
[1201] | 177 | void log_register(logger &L, const string &prefix){ |
---|
| 178 | BM::log_register ( L, prefix ); |
---|
| 179 | |
---|
| 180 | L.add_vector ( log_level, logG, RV("G",16), prefix ); |
---|
| 181 | L.add_vector ( log_level, logU, RV ("U", 16 ), prefix ); |
---|
| 182 | L.add_vector ( log_level, logD, RV ("D", 4 ), prefix ); |
---|
| 183 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
[1226] | 184 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
[1201] | 185 | |
---|
| 186 | }; |
---|
| 187 | //void from_setting(); |
---|
[1179] | 188 | }; |
---|
| 189 | |
---|
| 190 | UIREGISTER(EKFfixedUD); |
---|
| 191 | |
---|
[1226] | 192 | /*! |
---|
[1305] | 193 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 194 | * |
---|
| 195 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 196 | */ |
---|
| 197 | class EKFfixedUD2 : public BM { |
---|
| 198 | public: |
---|
[1311] | 199 | LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logC, logP); |
---|
[1305] | 200 | |
---|
| 201 | void init_ekf2(double Tv); |
---|
| 202 | void ekf2(double ux, double uy, double isxd, double isyd); |
---|
| 203 | |
---|
| 204 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 205 | int16 Q[4]; /* matrix [4,4] */ |
---|
| 206 | int16 R[4]; /* matrix [2,2] */ |
---|
| 207 | |
---|
| 208 | int16 x_est[2]; /* estimate and prediction */ |
---|
[1311] | 209 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 210 | int16 y_old[2]; /* estimate and prediction */ |
---|
[1305] | 211 | |
---|
| 212 | int16 PSI[4]; /* matrix [4,4] */ |
---|
| 213 | int16 PSIU[4]; /* matrix PIS*U, [4,4] */ |
---|
[1311] | 214 | int16 C[4]; /* matrix [4,4] */ |
---|
[1305] | 215 | |
---|
| 216 | int16 Uf[4]; // upper triangular of covariance (inplace) |
---|
| 217 | int16 Df[2]; // diagonal covariance |
---|
| 218 | int16 Dfold[2]; // temp of D |
---|
| 219 | int16 G[4]; // temp for bierman |
---|
| 220 | |
---|
| 221 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 222 | |
---|
| 223 | enorm<fsqmat> E; |
---|
| 224 | mat Ry; |
---|
| 225 | |
---|
| 226 | public: |
---|
| 227 | //! Default constructor |
---|
| 228 | EKFfixedUD2 ():BM(),E(),Ry(2,2){ |
---|
| 229 | int16 i; |
---|
| 230 | for(i=0;i<4;i++){Q[i]=0;} |
---|
| 231 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 232 | |
---|
| 233 | for(i=0;i<2;i++){x_est[i]=0;} |
---|
[1311] | 234 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 235 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
[1305] | 236 | for(i=0;i<4;i++){Uf[i]=0;} |
---|
| 237 | for(i=0;i<2;i++){Df[i]=0;} |
---|
| 238 | for(i=0;i<4;i++){G[i]=0;} |
---|
| 239 | for(i=0;i<2;i++){Dfold[i]=0;} |
---|
| 240 | |
---|
| 241 | for(i=0;i<4;i++){PSI[i]=0;} |
---|
[1311] | 242 | for(i=0;i<4;i++){C[i]=0;} |
---|
[1305] | 243 | |
---|
| 244 | set_dim(2); |
---|
[1311] | 245 | dimc = 2; |
---|
| 246 | dimy = 2; |
---|
[1305] | 247 | E._mu()=zeros(2); |
---|
| 248 | E._R()=zeros(2,2); |
---|
| 249 | init_ekf2(0.000125); |
---|
| 250 | }; |
---|
| 251 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 252 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 253 | //!dummy! |
---|
| 254 | const epdf& posterior() const {return E;}; |
---|
| 255 | void log_register(logger &L, const string &prefix){ |
---|
| 256 | BM::log_register ( L, prefix ); |
---|
| 257 | |
---|
[1311] | 258 | L.add_vector ( log_level, logG, RV("G2",4), prefix ); |
---|
| 259 | L.add_vector ( log_level, logU, RV ("U2", 4 ), prefix ); |
---|
| 260 | L.add_vector ( log_level, logD, RV ("D2", 2 ), prefix ); |
---|
| 261 | L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); |
---|
| 262 | L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); |
---|
| 263 | L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); |
---|
[1305] | 264 | |
---|
| 265 | }; |
---|
| 266 | //void from_setting(); |
---|
| 267 | }; |
---|
| 268 | |
---|
| 269 | UIREGISTER(EKFfixedUD2); |
---|
| 270 | |
---|
| 271 | /*! |
---|
[1341] | 272 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 273 | * |
---|
| 274 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 275 | */ |
---|
| 276 | class EKFfixedUD3 : public BM { |
---|
| 277 | public: |
---|
| 278 | LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP); |
---|
| 279 | |
---|
| 280 | void init_ekf3(double Tv); |
---|
| 281 | void ekf3(double ux, double uy, double isxd, double isyd); |
---|
| 282 | |
---|
| 283 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 284 | int16 Q[9]; /* matrix [4,4] */ |
---|
| 285 | int16 R[4]; /* matrix [2,2] */ |
---|
| 286 | |
---|
| 287 | int16 x_est[3]; /* estimate and prediction */ |
---|
| 288 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 289 | int16 y_old[2]; /* estimate and prediction */ |
---|
| 290 | |
---|
| 291 | int16 PSI[9]; /* matrix [4,4] */ |
---|
| 292 | int16 PSIU[9]; /* matrix PIS*U, [4,4] */ |
---|
| 293 | int16 C[6]; /* matrix [4,4] */ |
---|
| 294 | |
---|
| 295 | int16 Uf[9]; // upper triangular of covariance (inplace) |
---|
| 296 | int16 Df[3]; // diagonal covariance |
---|
| 297 | int16 Dfold[3]; // temp of D |
---|
| 298 | int16 G[9]; // temp for bierman |
---|
| 299 | |
---|
| 300 | int16 cA, cB, cC, cG, cF, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 301 | |
---|
| 302 | enorm<fsqmat> E; |
---|
| 303 | mat Ry; |
---|
| 304 | |
---|
| 305 | public: |
---|
| 306 | //! Default constructor |
---|
| 307 | EKFfixedUD3 ():BM(),E(),Ry(2,2){ |
---|
| 308 | int16 i; |
---|
| 309 | for(i=0;i<9;i++){Q[i]=0;} |
---|
| 310 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 311 | |
---|
| 312 | for(i=0;i<3;i++){x_est[i]=0;} |
---|
| 313 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 314 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
| 315 | for(i=0;i<9;i++){Uf[i]=0;} |
---|
| 316 | for(i=0;i<3;i++){Df[i]=0;} |
---|
| 317 | for(i=0;i<4;i++){G[i]=0;} |
---|
| 318 | for(i=0;i<3;i++){Dfold[i]=0;} |
---|
| 319 | |
---|
| 320 | for(i=0;i<9;i++){PSI[i]=0;} |
---|
| 321 | for(i=0;i<6;i++){C[i]=0;} |
---|
| 322 | |
---|
| 323 | set_dim(3); |
---|
| 324 | dimc = 2; |
---|
| 325 | dimy = 2; |
---|
| 326 | E._mu()=zeros(3); |
---|
| 327 | E._R()=zeros(3,3); |
---|
| 328 | init_ekf3(0.000125); |
---|
| 329 | }; |
---|
| 330 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 331 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 332 | //!dummy! |
---|
| 333 | const epdf& posterior() const {return E;}; |
---|
| 334 | void log_register(logger &L, const string &prefix){ |
---|
| 335 | BM::log_register ( L, prefix ); |
---|
| 336 | }; |
---|
| 337 | //void from_setting(); |
---|
| 338 | }; |
---|
| 339 | |
---|
| 340 | UIREGISTER(EKFfixedUD3); |
---|
| 341 | |
---|
| 342 | /*! |
---|
[1240] | 343 | * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic |
---|
[1226] | 344 | * |
---|
| 345 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 346 | */ |
---|
| 347 | class EKFfixedCh : public BM { |
---|
| 348 | public: |
---|
| 349 | LOG_LEVEL(EKFfixedCh,logCh, logA, logP); |
---|
| 350 | |
---|
| 351 | void init_ekf(double Tv); |
---|
| 352 | void ekf(double ux, double uy, double isxd, double isyd); |
---|
| 353 | |
---|
| 354 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
[1240] | 355 | int16 Q[16]; /* matrix [4,4] */ |
---|
| 356 | int16 R[4]; /* matrix [2,2] */ |
---|
[1226] | 357 | |
---|
[1240] | 358 | int16 x_est[4]; /* estimate and prediction */ |
---|
[1226] | 359 | |
---|
[1240] | 360 | int16 PSI[16]; /* matrix [4,4] */ |
---|
| 361 | int16 PSICh[16]; /* matrix PIS*U, [4,4] */ |
---|
[1226] | 362 | |
---|
[1240] | 363 | int16 Chf[16]; // upper triangular of covariance (inplace) |
---|
[1226] | 364 | |
---|
[1240] | 365 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
[1226] | 366 | |
---|
| 367 | enorm<chmat> E; |
---|
| 368 | mat Ry; |
---|
| 369 | |
---|
| 370 | public: |
---|
| 371 | //! Default constructor |
---|
| 372 | EKFfixedCh ():BM(),E(),Ry(2,2){ |
---|
[1240] | 373 | int16 i; |
---|
[1226] | 374 | for(i=0;i<16;i++){Q[i]=0;} |
---|
| 375 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 376 | |
---|
| 377 | for(i=0;i<4;i++){x_est[i]=0;} |
---|
| 378 | for(i=0;i<16;i++){Chf[i]=0;} |
---|
| 379 | |
---|
| 380 | for(i=0;i<16;i++){PSI[i]=0;} |
---|
| 381 | |
---|
| 382 | set_dim(4); |
---|
[1321] | 383 | dimc = 2; |
---|
| 384 | dimy =2; |
---|
[1226] | 385 | E._mu()=zeros(4); |
---|
| 386 | E._R()=zeros(4,4); |
---|
| 387 | init_ekf(0.000125); |
---|
| 388 | }; |
---|
| 389 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 390 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 391 | //!dummy! |
---|
| 392 | const epdf& posterior() const {return E;}; |
---|
| 393 | void log_register(logger &L, const string &prefix){ |
---|
| 394 | BM::log_register ( L, prefix ); |
---|
| 395 | |
---|
| 396 | L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix ); |
---|
| 397 | L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); |
---|
| 398 | L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); |
---|
| 399 | |
---|
| 400 | }; |
---|
| 401 | //void from_setting(); |
---|
| 402 | }; |
---|
| 403 | |
---|
| 404 | UIREGISTER(EKFfixedCh); |
---|
| 405 | |
---|
[1326] | 406 | /*! |
---|
| 407 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
---|
| 408 | * |
---|
| 409 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 410 | */ |
---|
| 411 | class EKFfixedCh2 : public BM { |
---|
| 412 | public: |
---|
| 413 | LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP); |
---|
| 414 | |
---|
| 415 | void init_ekf2(double Tv); |
---|
| 416 | void ekf2(double ux, double uy, double isxd, double isyd); |
---|
| 417 | |
---|
| 418 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
---|
| 419 | int16 Q[4]; /* matrix [4,4] */ |
---|
| 420 | int16 R[4]; /* matrix [2,2] */ |
---|
| 421 | |
---|
| 422 | int16 x_est[2]; /* estimate and prediction */ |
---|
| 423 | int16 y_est[2]; /* estimate and prediction */ |
---|
| 424 | int16 y_old[2]; /* estimate and prediction */ |
---|
| 425 | |
---|
| 426 | int16 PSI[4]; /* matrix [4,4] */ |
---|
| 427 | int16 PSICh[4]; /* matrix PIS*U, [4,4] */ |
---|
| 428 | int16 C[4]; /* matrix [4,4] */ |
---|
| 429 | |
---|
| 430 | int16 Chf[4]; // upper triangular of covariance (inplace) |
---|
| 431 | |
---|
| 432 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
---|
| 433 | |
---|
| 434 | enorm<fsqmat> E; |
---|
| 435 | mat Ry; |
---|
| 436 | |
---|
| 437 | public: |
---|
| 438 | //! Default constructor |
---|
| 439 | EKFfixedCh2 ():BM(),E(),Ry(2,2){ |
---|
| 440 | int16 i; |
---|
| 441 | for(i=0;i<4;i++){Q[i]=0;} |
---|
| 442 | for(i=0;i<4;i++){R[i]=0;} |
---|
| 443 | |
---|
| 444 | for(i=0;i<2;i++){x_est[i]=0;} |
---|
| 445 | for(i=0;i<2;i++){y_est[i]=0;} |
---|
| 446 | for(i=0;i<2;i++){y_old[i]=0;} |
---|
| 447 | for(i=0;i<4;i++){Chf[i]=0;} |
---|
| 448 | |
---|
| 449 | for(i=0;i<4;i++){PSI[i]=0;} |
---|
| 450 | for(i=0;i<4;i++){C[i]=0;} |
---|
| 451 | |
---|
| 452 | set_dim(2); |
---|
| 453 | dimc = 2; |
---|
| 454 | dimy = 2; |
---|
| 455 | E._mu()=zeros(2); |
---|
| 456 | E._R()=zeros(2,2); |
---|
| 457 | init_ekf2(0.000125); |
---|
| 458 | }; |
---|
| 459 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 460 | void bayes ( const vec &yt, const vec &ut ); |
---|
| 461 | //!dummy! |
---|
| 462 | const epdf& posterior() const {return E;}; |
---|
| 463 | void log_register(logger &L, const string &prefix){ |
---|
| 464 | BM::log_register ( L, prefix ); |
---|
| 465 | |
---|
| 466 | L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix ); |
---|
| 467 | L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); |
---|
| 468 | L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); |
---|
| 469 | L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); |
---|
| 470 | |
---|
| 471 | }; |
---|
| 472 | //void from_setting(); |
---|
| 473 | }; |
---|
[1226] | 474 | |
---|
[1326] | 475 | UIREGISTER(EKFfixedCh2); |
---|
| 476 | |
---|
| 477 | |
---|
[1240] | 478 | //! EKF for comparison of EKF_UD with its fixed-point16 implementation |
---|
[1174] | 479 | class EKF_UDfix : public BM { |
---|
| 480 | protected: |
---|
| 481 | //! logger |
---|
| 482 | LOG_LEVEL(EKF_UDfix,logU, logG); |
---|
| 483 | //! Internal Model f(x,u) |
---|
| 484 | shared_ptr<diffbifn> pfxu; |
---|
| 485 | |
---|
| 486 | //! Observation Model h(x,u) |
---|
| 487 | shared_ptr<diffbifn> phxu; |
---|
| 488 | |
---|
| 489 | //! U part |
---|
| 490 | mat U; |
---|
| 491 | //! D part |
---|
| 492 | vec D; |
---|
[1201] | 493 | |
---|
[1174] | 494 | mat A; |
---|
| 495 | mat C; |
---|
| 496 | mat Q; |
---|
| 497 | vec R; |
---|
| 498 | |
---|
| 499 | enorm<ldmat> est; |
---|
| 500 | |
---|
| 501 | |
---|
| 502 | public: |
---|
| 503 | |
---|
| 504 | //! copy constructor duplicated |
---|
| 505 | EKF_UDfix* _copy() const { |
---|
| 506 | return new EKF_UDfix(*this); |
---|
| 507 | } |
---|
| 508 | |
---|
| 509 | const enorm<ldmat>& posterior()const{return est;}; |
---|
| 510 | |
---|
| 511 | enorm<ldmat>& prior() { |
---|
| 512 | return const_cast<enorm<ldmat>&>(posterior()); |
---|
| 513 | } |
---|
| 514 | |
---|
| 515 | EKF_UDfix(){} |
---|
| 516 | |
---|
| 517 | |
---|
| 518 | EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){} |
---|
| 519 | |
---|
| 520 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 521 | void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 ); |
---|
| 522 | |
---|
| 523 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 524 | void bayes ( const vec &yt, const vec &cond = empty_vec ); |
---|
| 525 | |
---|
| 526 | void log_register ( bdm::logger& L, const string& prefix ){ |
---|
| 527 | BM::log_register ( L, prefix ); |
---|
| 528 | |
---|
| 529 | if ( log_level[logU] ) |
---|
| 530 | L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix ); |
---|
| 531 | if ( log_level[logG] ) |
---|
| 532 | L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix ); |
---|
| 533 | |
---|
| 534 | } |
---|
| 535 | /*! Create object from the following structure |
---|
| 536 | |
---|
| 537 | \code |
---|
| 538 | class = 'EKF_UD'; |
---|
| 539 | OM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 540 | IM = configuration of bdm::diffbifn; % any offspring of diffbifn, bdm::diffbifn::from_setting |
---|
| 541 | dQ = [...]; % vector containing diagonal of Q |
---|
| 542 | dR = [...]; % vector containing diagonal of R |
---|
| 543 | --- optional fields --- |
---|
| 544 | mu0 = [...]; % vector of statistics mu0 |
---|
| 545 | dP0 = [...]; % vector containing diagonal of P0 |
---|
| 546 | -- or -- |
---|
| 547 | P0 = [...]; % full matrix P0 |
---|
| 548 | --- inherited fields --- |
---|
| 549 | bdm::BM::from_setting |
---|
| 550 | \endcode |
---|
| 551 | If the optional fields are not given, they will be filled as follows: |
---|
| 552 | \code |
---|
| 553 | mu0 = [0,0,0,....]; % empty statistics |
---|
| 554 | P0 = eye( dim ); |
---|
| 555 | \endcode |
---|
| 556 | */ |
---|
| 557 | void from_setting ( const Setting &set ); |
---|
| 558 | |
---|
| 559 | void validate() {}; |
---|
| 560 | // TODO dodelat void to_setting( Setting &set ) const; |
---|
| 561 | |
---|
| 562 | }; |
---|
| 563 | UIREGISTER(EKF_UDfix); |
---|
| 564 | |
---|
| 565 | |
---|
[1179] | 566 | |
---|
| 567 | |
---|
[61] | 568 | #endif // KF_H |
---|
| 569 | |
---|