[7] | 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 KF_H |
---|
| 14 | #define KF_H |
---|
| 15 | |
---|
| 16 | #include <itpp/itbase.h> |
---|
| 17 | #include "libBM.h" |
---|
| 18 | #include "libDC.h" |
---|
| 19 | |
---|
| 20 | |
---|
| 21 | using namespace itpp; |
---|
| 22 | |
---|
| 23 | /*! |
---|
[8] | 24 | * \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon! |
---|
[7] | 25 | */ |
---|
| 26 | class KalmanFull : public BM { |
---|
| 27 | int dimx, dimy, dimu; |
---|
| 28 | mat A, B, C, D, R, Q; |
---|
| 29 | |
---|
| 30 | //cache |
---|
| 31 | mat _Pp, _Ry, _iRy, _K; |
---|
| 32 | public: |
---|
| 33 | //posterior |
---|
| 34 | //! Mean value of the posterior density |
---|
| 35 | vec mu; |
---|
| 36 | //! Variance of the posterior density |
---|
| 37 | mat P; |
---|
| 38 | |
---|
| 39 | public: |
---|
| 40 | //! Full constructor |
---|
| 41 | KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0); |
---|
| 42 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 43 | void bayes(const vec &dt, bool evalll=true); |
---|
| 44 | |
---|
| 45 | friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); |
---|
| 46 | |
---|
| 47 | }; |
---|
| 48 | |
---|
| 49 | |
---|
| 50 | /*! |
---|
| 51 | * \brief Kalman filter with covaraince matrices in square root form. |
---|
| 52 | */ |
---|
| 53 | template<class sq_T> |
---|
| 54 | class Kalman : public BM { |
---|
| 55 | int dimx, dimy, dimu; |
---|
| 56 | mat A, B, C, D; |
---|
| 57 | sq_T R, Q; |
---|
| 58 | |
---|
| 59 | //cache |
---|
[8] | 60 | mat _K; |
---|
| 61 | vec _yp; |
---|
[7] | 62 | sq_T _Ry,_iRy; |
---|
| 63 | public: |
---|
| 64 | //posterior |
---|
| 65 | //! Mean value of the posterior density |
---|
| 66 | vec mu; |
---|
| 67 | //! Mean value of the posterior density |
---|
| 68 | sq_T P; |
---|
| 69 | |
---|
| 70 | public: |
---|
| 71 | //! Full constructor |
---|
| 72 | Kalman ( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ); |
---|
| 73 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 74 | void bayes(const vec &dt, bool evalll=true); |
---|
| 75 | |
---|
| 76 | friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); |
---|
| 77 | |
---|
| 78 | }; |
---|
| 79 | |
---|
| 80 | //////// INstance |
---|
| 81 | |
---|
| 82 | template<class sq_T> |
---|
| 83 | Kalman<sq_T>::Kalman( mat A0, mat B0, mat C0, mat D0, sq_T R0, sq_T Q0, sq_T P0, vec mu0 ) { |
---|
| 84 | dimx = A0.rows(); |
---|
| 85 | dimu = B0.cols(); |
---|
| 86 | dimy = C0.rows(); |
---|
| 87 | |
---|
| 88 | it_assert_debug( A0.cols()==dimx, "Kalman: A is not square" ); |
---|
| 89 | it_assert_debug( B0.rows()==dimx, "Kalman: B is not compatible" ); |
---|
| 90 | it_assert_debug( C0.cols()==dimx, "Kalman: C is not square" ); |
---|
| 91 | it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ), "Kalman: D is not compatible" ); |
---|
| 92 | it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "Kalman: R is not compatible" ); |
---|
| 93 | it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "Kalman: Q is not compatible" ); |
---|
| 94 | |
---|
| 95 | A = A0; |
---|
| 96 | B = B0; |
---|
| 97 | C = C0; |
---|
| 98 | D = D0; |
---|
| 99 | R = R0; |
---|
| 100 | Q = Q0; |
---|
| 101 | mu = mu0; |
---|
| 102 | P = P0; |
---|
| 103 | |
---|
| 104 | ll = 0; |
---|
| 105 | //Fixme should we assign cache?? |
---|
[8] | 106 | _iRy = eye(dimy); // needed in inv(_iRy) |
---|
[7] | 107 | } |
---|
| 108 | |
---|
| 109 | template<class sq_T> |
---|
| 110 | void Kalman<sq_T>::bayes( const vec &dt , bool evalll) { |
---|
| 111 | it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
| 112 | |
---|
| 113 | vec u = dt.get( dimy,dimy+dimu-1 ); |
---|
| 114 | vec y = dt.get( 0,dimy-1 ); |
---|
| 115 | //Time update |
---|
| 116 | mu = A*mu + B*u; |
---|
| 117 | //P = A*P*A.transpose() + Q; in sq_T |
---|
[8] | 118 | P.mult_sym( A ); |
---|
[7] | 119 | P+=Q; |
---|
| 120 | |
---|
| 121 | //Data update |
---|
| 122 | //_Ry = C*P*C.transpose() + R; in sq_T |
---|
[8] | 123 | _Ry.mult_sym( C, P); |
---|
[7] | 124 | _Ry+=R; |
---|
| 125 | |
---|
| 126 | mat Pfull = P.to_mat(); |
---|
| 127 | |
---|
| 128 | _Ry.inv( _iRy ); // result is in _iRy; |
---|
| 129 | _K = Pfull*C.transpose()*(_iRy.to_mat()); |
---|
| 130 | P -= _K*C*Pfull; // P = P -KCP; |
---|
| 131 | _yp = y-C*mu-D*u; //y prediction |
---|
| 132 | mu += _K*( _yp ); |
---|
| 133 | |
---|
| 134 | if (evalll==true) { |
---|
[8] | 135 | ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \ |
---|
| 136 | +_Ry.logdet() +_iRy.qform(_yp)); |
---|
[7] | 137 | } |
---|
| 138 | }; |
---|
| 139 | |
---|
| 140 | //extern template class Kalman<ldmat>; |
---|
| 141 | |
---|
| 142 | |
---|
| 143 | #endif // KF_H |
---|
| 144 | |
---|