| 334 | |
| 335 | /*! |
| 336 | * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic |
| 337 | * |
| 338 | * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
| 339 | */ |
| 340 | class EKFfixedCh2 : public BM { |
| 341 | public: |
| 342 | LOG_LEVEL(EKFfixedCh2,logCh, logA, logC, logP); |
| 343 | |
| 344 | void init_ekf2(double Tv); |
| 345 | void ekf2(double ux, double uy, double isxd, double isyd); |
| 346 | |
| 347 | /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ |
| 348 | int16 Q[4]; /* matrix [4,4] */ |
| 349 | int16 R[4]; /* matrix [2,2] */ |
| 350 | |
| 351 | int16 x_est[2]; /* estimate and prediction */ |
| 352 | int16 y_est[2]; /* estimate and prediction */ |
| 353 | int16 y_old[2]; /* estimate and prediction */ |
| 354 | |
| 355 | int16 PSI[4]; /* matrix [4,4] */ |
| 356 | int16 PSICh[4]; /* matrix PIS*U, [4,4] */ |
| 357 | int16 C[4]; /* matrix [4,4] */ |
| 358 | |
| 359 | int16 Chf[4]; // upper triangular of covariance (inplace) |
| 360 | |
| 361 | int16 cA, cB, cC, cG, cH; // cD, cE, cF, cI ... nepouzivane |
| 362 | |
| 363 | enorm<fsqmat> E; |
| 364 | mat Ry; |
| 365 | |
| 366 | public: |
| 367 | //! Default constructor |
| 368 | EKFfixedCh2 ():BM(),E(),Ry(2,2){ |
| 369 | int16 i; |
| 370 | for(i=0;i<4;i++){Q[i]=0;} |
| 371 | for(i=0;i<4;i++){R[i]=0;} |
| 372 | |
| 373 | for(i=0;i<2;i++){x_est[i]=0;} |
| 374 | for(i=0;i<2;i++){y_est[i]=0;} |
| 375 | for(i=0;i<2;i++){y_old[i]=0;} |
| 376 | for(i=0;i<4;i++){Chf[i]=0;} |
| 377 | |
| 378 | for(i=0;i<4;i++){PSI[i]=0;} |
| 379 | for(i=0;i<4;i++){C[i]=0;} |
| 380 | |
| 381 | set_dim(2); |
| 382 | dimc = 2; |
| 383 | dimy = 2; |
| 384 | E._mu()=zeros(2); |
| 385 | E._R()=zeros(2,2); |
| 386 | init_ekf2(0.000125); |
| 387 | }; |
| 388 | //! Here dt = [yt;ut] of appropriate dimensions |
| 389 | void bayes ( const vec &yt, const vec &ut ); |
| 390 | //!dummy! |
| 391 | const epdf& posterior() const {return E;}; |
| 392 | void log_register(logger &L, const string &prefix){ |
| 393 | BM::log_register ( L, prefix ); |
| 394 | |
| 395 | L.add_vector ( log_level, logCh, RV ("Ch2", 4 ), prefix ); |
| 396 | L.add_vector ( log_level, logA, RV ("A2", 4 ), prefix ); |
| 397 | L.add_vector ( log_level, logC, RV ("C2", 4 ), prefix ); |
| 398 | L.add_vector ( log_level, logP, RV ("P2", 4 ), prefix ); |
| 399 | |
| 400 | }; |
| 401 | //void from_setting(); |
| 402 | }; |
| 403 | |
| 404 | UIREGISTER(EKFfixedCh2); |