[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> |
---|
[22] | 17 | #include "../stat/libFN.h" |
---|
[28] | 18 | #include "../stat/libEF.h" |
---|
[37] | 19 | #include "../math/chmat.h" |
---|
[7] | 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 | */ |
---|
[32] | 26 | |
---|
| 27 | class KalmanFull { |
---|
[62] | 28 | protected: |
---|
[7] | 29 | int dimx, dimy, dimu; |
---|
| 30 | mat A, B, C, D, R, Q; |
---|
[32] | 31 | |
---|
| 32 | //cache |
---|
[7] | 33 | mat _Pp, _Ry, _iRy, _K; |
---|
| 34 | public: |
---|
[32] | 35 | //posterior |
---|
[7] | 36 | //! Mean value of the posterior density |
---|
| 37 | vec mu; |
---|
| 38 | //! Variance of the posterior density |
---|
| 39 | mat P; |
---|
| 40 | |
---|
[37] | 41 | bool evalll; |
---|
| 42 | double ll; |
---|
[7] | 43 | public: |
---|
[32] | 44 | //! Full constructor |
---|
| 45 | KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 ); |
---|
[7] | 46 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[32] | 47 | void bayes ( const vec &dt ); |
---|
[33] | 48 | //! print elements of KF |
---|
[7] | 49 | friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ); |
---|
[62] | 50 | //! For EKFfull; |
---|
| 51 | KalmanFull(){}; |
---|
[7] | 52 | }; |
---|
| 53 | |
---|
| 54 | |
---|
| 55 | /*! |
---|
[22] | 56 | * \brief Kalman filter with covariance matrices in square root form. |
---|
[33] | 57 | |
---|
| 58 | Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f] |
---|
| 59 | Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f] |
---|
| 60 | Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances. |
---|
[7] | 61 | */ |
---|
| 62 | template<class sq_T> |
---|
[32] | 63 | |
---|
| 64 | class Kalman : public BM { |
---|
[22] | 65 | protected: |
---|
[33] | 66 | //! Indetifier of output rv |
---|
[32] | 67 | RV rvy; |
---|
[33] | 68 | //! Indetifier of exogeneous rv |
---|
[32] | 69 | RV rvu; |
---|
[33] | 70 | //! cache of rv.count() |
---|
| 71 | int dimx; |
---|
| 72 | //! cache of rvy.count() |
---|
| 73 | int dimy; |
---|
| 74 | //! cache of rvu.count() |
---|
| 75 | int dimu; |
---|
| 76 | //! Matrix A |
---|
| 77 | mat A; |
---|
| 78 | //! Matrix B |
---|
| 79 | mat B; |
---|
| 80 | //! Matrix C |
---|
| 81 | mat C; |
---|
| 82 | //! Matrix D |
---|
| 83 | mat D; |
---|
| 84 | //! Matrix Q in square-root form |
---|
| 85 | sq_T Q; |
---|
| 86 | //! Matrix R in square-root form |
---|
| 87 | sq_T R; |
---|
[32] | 88 | |
---|
| 89 | //!posterior density on $x_t$ |
---|
[28] | 90 | enorm<sq_T> est; |
---|
| 91 | //!preditive density on $y_t$ |
---|
| 92 | enorm<sq_T> fy; |
---|
[32] | 93 | |
---|
[33] | 94 | //! placeholder for Kalman gain |
---|
[8] | 95 | mat _K; |
---|
[33] | 96 | //! cache of fy.mu |
---|
[83] | 97 | vec& _yp; |
---|
[33] | 98 | //! cache of fy.R |
---|
[83] | 99 | sq_T& _Ry; |
---|
[33] | 100 | //!cache of est.mu |
---|
[83] | 101 | vec& _mu; |
---|
[33] | 102 | //!cache of est.R |
---|
[83] | 103 | sq_T& _P; |
---|
[32] | 104 | |
---|
[7] | 105 | public: |
---|
[22] | 106 | //! Default constructor |
---|
[32] | 107 | Kalman ( RV rvx0, RV rvy0, RV rvu0 ); |
---|
| 108 | //! Copy constructor |
---|
| 109 | Kalman ( const Kalman<sq_T> &K0 ); |
---|
| 110 | //! Set parameters with check of relevance |
---|
| 111 | void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &R0,const sq_T &Q0 ); |
---|
[28] | 112 | //! Set estimate values, used e.g. in initialization. |
---|
[32] | 113 | void set_est ( const vec &mu0, const sq_T &P0 ) { |
---|
| 114 | sq_T pom(dimy); |
---|
| 115 | est.set_parameters ( mu0,P0 ); |
---|
| 116 | P0.mult_sym(C,pom); |
---|
| 117 | fy.set_parameters ( C*mu0, pom ); |
---|
| 118 | }; |
---|
| 119 | |
---|
[7] | 120 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[32] | 121 | void bayes ( const vec &dt ); |
---|
[33] | 122 | //!access function |
---|
[170] | 123 | const epdf& _epdf() const {return est;} |
---|
[200] | 124 | const enorm<sq_T>* _e() const {return &est;} |
---|
[51] | 125 | //!access function |
---|
| 126 | mat& __K() {return _K;} |
---|
| 127 | //!access function |
---|
| 128 | vec _dP() {return _P->getD();} |
---|
[7] | 129 | }; |
---|
| 130 | |
---|
[37] | 131 | /*! \brief Kalman filter in square root form |
---|
| 132 | */ |
---|
| 133 | class KalmanCh : public Kalman<chmat>{ |
---|
| 134 | protected: |
---|
| 135 | //! pre array (triangular matrix) |
---|
| 136 | mat preA; |
---|
| 137 | //! post array (triangular matrix) |
---|
| 138 | mat postA; |
---|
| 139 | |
---|
| 140 | public: |
---|
| 141 | //! Default constructor |
---|
| 142 | KalmanCh ( RV rvx0, RV rvy0, RV rvu0 ):Kalman<chmat>(rvx0,rvy0,rvu0),preA(dimy+dimx+dimx,dimy+dimx),postA(dimy+dimx,dimy+dimx){}; |
---|
| 143 | //! Set parameters with check of relevance |
---|
| 144 | void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ); |
---|
| 145 | void set_est ( const vec &mu0, const chmat &P0 ) { |
---|
| 146 | est.set_parameters ( mu0,P0 ); |
---|
| 147 | }; |
---|
| 148 | |
---|
| 149 | |
---|
| 150 | /*!\brief Here dt = [yt;ut] of appropriate dimensions |
---|
| 151 | |
---|
| 152 | The following equality hold::\f[ |
---|
| 153 | \left[\begin{array}{cc} |
---|
| 154 | R^{0.5}\\ |
---|
| 155 | P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\ |
---|
| 156 | & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc} |
---|
| 157 | R_{y}^{0.5} & KA'\\ |
---|
| 158 | & P_{t+1|t}^{0.5}\\ |
---|
| 159 | \\\end{array}\right]\f] |
---|
| 160 | |
---|
[132] | 161 | Thus this object evaluates only predictors! Not filtering densities. |
---|
[37] | 162 | */ |
---|
| 163 | void bayes ( const vec &dt ); |
---|
| 164 | }; |
---|
| 165 | |
---|
[22] | 166 | /*! |
---|
[62] | 167 | \brief Extended Kalman Filter in full matrices |
---|
| 168 | |
---|
| 169 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 170 | */ |
---|
| 171 | class EKFfull : public KalmanFull, public BM { |
---|
[67] | 172 | |
---|
[62] | 173 | //! Internal Model f(x,u) |
---|
| 174 | diffbifn* pfxu; |
---|
| 175 | //! Observation Model h(x,u) |
---|
| 176 | diffbifn* phxu; |
---|
[67] | 177 | |
---|
| 178 | enorm<fsqmat> E; |
---|
[62] | 179 | public: |
---|
| 180 | //! Default constructor |
---|
| 181 | EKFfull ( RV rvx, RV rvy, RV rvu ); |
---|
| 182 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 183 | void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 ); |
---|
| 184 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 185 | void bayes ( const vec &dt ); |
---|
| 186 | //! set estimates |
---|
| 187 | void set_est (vec mu0, mat P0){mu=mu0;P=P0;}; |
---|
| 188 | //!dummy! |
---|
[170] | 189 | const epdf& _epdf()const{return E;}; |
---|
[200] | 190 | const enorm<fsqmat>* _e()const{return &E;}; |
---|
[62] | 191 | }; |
---|
| 192 | |
---|
| 193 | /*! |
---|
[32] | 194 | \brief Extended Kalman Filter |
---|
[22] | 195 | |
---|
| 196 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 197 | */ |
---|
| 198 | template<class sq_T> |
---|
[51] | 199 | class EKF : public Kalman<fsqmat> { |
---|
[22] | 200 | //! Internal Model f(x,u) |
---|
| 201 | diffbifn* pfxu; |
---|
| 202 | //! Observation Model h(x,u) |
---|
| 203 | diffbifn* phxu; |
---|
[32] | 204 | public: |
---|
[22] | 205 | //! Default constructor |
---|
[32] | 206 | EKF ( RV rvx, RV rvy, RV rvu ); |
---|
[33] | 207 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
[32] | 208 | void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 ); |
---|
[22] | 209 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
[32] | 210 | void bayes ( const vec &dt ); |
---|
[22] | 211 | }; |
---|
| 212 | |
---|
[32] | 213 | /*! |
---|
[62] | 214 | \brief Extended Kalman Filter in Square root |
---|
[37] | 215 | |
---|
| 216 | An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. |
---|
| 217 | */ |
---|
| 218 | |
---|
| 219 | class EKFCh : public KalmanCh { |
---|
| 220 | //! Internal Model f(x,u) |
---|
| 221 | diffbifn* pfxu; |
---|
| 222 | //! Observation Model h(x,u) |
---|
| 223 | diffbifn* phxu; |
---|
| 224 | public: |
---|
| 225 | //! Default constructor |
---|
| 226 | EKFCh ( RV rvx, RV rvy, RV rvu ); |
---|
| 227 | //! Set nonlinear functions for mean values and covariance matrices. |
---|
| 228 | void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 ); |
---|
| 229 | //! Here dt = [yt;ut] of appropriate dimensions |
---|
| 230 | void bayes ( const vec &dt ); |
---|
| 231 | }; |
---|
| 232 | |
---|
| 233 | /*! |
---|
[32] | 234 | \brief Kalman Filter with conditional diagonal matrices R and Q. |
---|
| 235 | */ |
---|
| 236 | |
---|
| 237 | class KFcondQR : public Kalman<ldmat>, public BMcond { |
---|
| 238 | //protected: |
---|
| 239 | public: |
---|
[33] | 240 | //!Default constructor |
---|
[32] | 241 | KFcondQR ( RV rvx, RV rvy, RV rvu, RV rvRQ ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvRQ ) {}; |
---|
| 242 | |
---|
| 243 | void condition ( const vec &RQ ); |
---|
| 244 | }; |
---|
| 245 | |
---|
| 246 | /*! |
---|
| 247 | \brief Kalman Filter with conditional diagonal matrices R and Q. |
---|
| 248 | */ |
---|
| 249 | |
---|
| 250 | class KFcondR : public Kalman<ldmat>, public BMcond { |
---|
| 251 | //protected: |
---|
| 252 | public: |
---|
[33] | 253 | //!Default constructor |
---|
[32] | 254 | KFcondR ( RV rvx, RV rvy, RV rvu, RV rvR ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvR ) {}; |
---|
| 255 | |
---|
| 256 | void condition ( const vec &R ); |
---|
| 257 | }; |
---|
| 258 | |
---|
[7] | 259 | //////// INstance |
---|
| 260 | |
---|
| 261 | template<class sq_T> |
---|
[32] | 262 | Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ), |
---|
| 263 | dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ), |
---|
[37] | 264 | A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ), |
---|
| 265 | Q(dimx), R(dimy), |
---|
[85] | 266 | est ( rv ), fy ( rvy ), _yp(fy._mu()),_Ry(fy._R()), _mu(est._mu()), _P(est._R()) { |
---|
[32] | 267 | |
---|
| 268 | this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q ); |
---|
| 269 | |
---|
[37] | 270 | // copy values in pointers |
---|
[83] | 271 | _mu = K0._mu; |
---|
| 272 | _P = K0._P; |
---|
| 273 | _yp = K0._yp; |
---|
| 274 | _Ry = K0._Ry; |
---|
[32] | 275 | |
---|
| 276 | } |
---|
| 277 | |
---|
| 278 | template<class sq_T> |
---|
| 279 | Kalman<sq_T>::Kalman ( RV rvx, RV rvy0, RV rvu0 ) : BM ( rvx ),rvy ( rvy0 ),rvu ( rvu0 ), |
---|
| 280 | dimx ( rvx.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ), |
---|
| 281 | A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ), |
---|
| 282 | Q(dimx), R (dimy), |
---|
[85] | 283 | est ( rvx ), fy ( rvy ), _yp(fy._mu()),_Ry(fy._R()),_mu(est._mu()), _P(est._R()) { |
---|
[22] | 284 | }; |
---|
| 285 | |
---|
| 286 | template<class sq_T> |
---|
[32] | 287 | void Kalman<sq_T>::set_parameters ( const mat &A0,const mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0 ) { |
---|
| 288 | it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" ); |
---|
| 289 | it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" ); |
---|
| 290 | it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" ); |
---|
| 291 | it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" ); |
---|
| 292 | it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" ); |
---|
| 293 | it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" ); |
---|
[7] | 294 | |
---|
| 295 | A = A0; |
---|
| 296 | B = B0; |
---|
| 297 | C = C0; |
---|
| 298 | D = D0; |
---|
| 299 | R = R0; |
---|
[32] | 300 | Q = Q0; |
---|
[7] | 301 | } |
---|
| 302 | |
---|
| 303 | template<class sq_T> |
---|
[32] | 304 | void Kalman<sq_T>::bayes ( const vec &dt ) { |
---|
| 305 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
[7] | 306 | |
---|
[83] | 307 | sq_T iRy(dimy); |
---|
[32] | 308 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 309 | vec y = dt.get ( 0,dimy-1 ); |
---|
[7] | 310 | //Time update |
---|
[83] | 311 | _mu = A* _mu + B*u; |
---|
[7] | 312 | //P = A*P*A.transpose() + Q; in sq_T |
---|
[83] | 313 | _P.mult_sym ( A ); |
---|
| 314 | _P +=Q; |
---|
[7] | 315 | |
---|
| 316 | //Data update |
---|
| 317 | //_Ry = C*P*C.transpose() + R; in sq_T |
---|
[83] | 318 | _P.mult_sym ( C, _Ry ); |
---|
| 319 | _Ry +=R; |
---|
[7] | 320 | |
---|
[83] | 321 | mat Pfull = _P.to_mat(); |
---|
[32] | 322 | |
---|
[83] | 323 | _Ry.inv ( iRy ); // result is in _iRy; |
---|
| 324 | _K = Pfull*C.transpose() * ( iRy.to_mat() ); |
---|
[32] | 325 | |
---|
| 326 | sq_T pom ( ( int ) Pfull.rows() ); |
---|
[83] | 327 | iRy.mult_sym_t ( C*Pfull,pom ); |
---|
| 328 | (_P ) -= pom; // P = P -PC'iRy*CP; |
---|
| 329 | (_yp ) = C* _mu +D*u; //y prediction |
---|
| 330 | (_mu ) += _K* ( y- _yp ); |
---|
[32] | 331 | |
---|
| 332 | |
---|
| 333 | if ( evalll==true ) { //likelihood of observation y |
---|
[211] | 334 | ll=fy.evallog ( y ); |
---|
[7] | 335 | } |
---|
[32] | 336 | |
---|
| 337 | //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl; |
---|
| 338 | |
---|
[7] | 339 | }; |
---|
[62] | 340 | |
---|
[7] | 341 | |
---|
[62] | 342 | |
---|
[22] | 343 | //TODO why not const pointer?? |
---|
[7] | 344 | |
---|
[22] | 345 | template<class sq_T> |
---|
[37] | 346 | EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {} |
---|
[28] | 347 | |
---|
| 348 | template<class sq_T> |
---|
[32] | 349 | void EKF<sq_T>::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) { |
---|
| 350 | pfxu = pfxu0; |
---|
| 351 | phxu = phxu0; |
---|
[7] | 352 | |
---|
[32] | 353 | //initialize matrices A C, later, these will be only updated! |
---|
[83] | 354 | pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true ); |
---|
[33] | 355 | // pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); |
---|
| 356 | B.clear(); |
---|
[83] | 357 | phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true ); |
---|
[33] | 358 | // phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); |
---|
| 359 | D.clear(); |
---|
[22] | 360 | |
---|
[32] | 361 | R = R0; |
---|
| 362 | Q = Q0; |
---|
[22] | 363 | } |
---|
| 364 | |
---|
| 365 | template<class sq_T> |
---|
[32] | 366 | void EKF<sq_T>::bayes ( const vec &dt ) { |
---|
| 367 | it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" ); |
---|
[22] | 368 | |
---|
[83] | 369 | sq_T iRy(dimy,dimy); |
---|
[32] | 370 | vec u = dt.get ( dimy,dimy+dimu-1 ); |
---|
| 371 | vec y = dt.get ( 0,dimy-1 ); |
---|
[22] | 372 | //Time update |
---|
[83] | 373 | _mu = pfxu->eval ( _mu, u ); |
---|
| 374 | pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx |
---|
[32] | 375 | |
---|
[22] | 376 | //P = A*P*A.transpose() + Q; in sq_T |
---|
[83] | 377 | _P.mult_sym ( A ); |
---|
| 378 | _P +=Q; |
---|
[22] | 379 | |
---|
| 380 | //Data update |
---|
[83] | 381 | phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx |
---|
[22] | 382 | //_Ry = C*P*C.transpose() + R; in sq_T |
---|
[83] | 383 | _P.mult_sym ( C, _Ry ); |
---|
| 384 | ( _Ry ) +=R; |
---|
[22] | 385 | |
---|
[83] | 386 | mat Pfull = _P.to_mat(); |
---|
[32] | 387 | |
---|
[83] | 388 | _Ry.inv ( iRy ); // result is in _iRy; |
---|
| 389 | _K = Pfull*C.transpose() * ( iRy.to_mat() ); |
---|
[32] | 390 | |
---|
| 391 | sq_T pom ( ( int ) Pfull.rows() ); |
---|
[83] | 392 | iRy.mult_sym_t ( C*Pfull,pom ); |
---|
| 393 | (_P ) -= pom; // P = P -PC'iRy*CP; |
---|
| 394 | _yp = phxu->eval ( _mu,u ); //y prediction |
---|
| 395 | ( _mu ) += _K* ( y-_yp ); |
---|
[32] | 396 | |
---|
[211] | 397 | if ( evalll==true ) {ll+=fy.evallog ( y );} |
---|
[22] | 398 | }; |
---|
| 399 | |
---|
| 400 | |
---|
[7] | 401 | #endif // KF_H |
---|
| 402 | |
---|