root/bdm/estim/libKF.h @ 211

Revision 211, 10.4 kB (checked in by smidl, 16 years ago)

prejmenovani evalpdflog a evalcond

  • Property svn:eol-style set to native
RevLine 
[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
21using namespace itpp;
22
23/*!
[8]24* \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon!
[7]25*/
[32]26
27class KalmanFull {
[62]28protected:
[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;
34public:
[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]43public:
[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
58Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f]
59Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f]
60Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances.
[7]61*/
62template<class sq_T>
[32]63
64class Kalman : public BM {
[22]65protected:
[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]105public:
[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*/
133class KalmanCh : public Kalman<chmat>{
134protected:
135//! pre array (triangular matrix)
136mat preA;
137//! post array (triangular matrix)
138mat postA;
139
140public:
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}
154R^{0.5}\\
155P_{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}
157R_{y}^{0.5} & KA'\\
158 & P_{t+1|t}^{0.5}\\
159\\\end{array}\right]\f]
160
[132]161Thus 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
169An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
170*/
171class 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]179public:
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
196An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
197*/
198template<class sq_T>
[51]199class EKF : public Kalman<fsqmat> {
[22]200        //! Internal Model f(x,u)
201        diffbifn* pfxu;
202        //! Observation Model h(x,u)
203        diffbifn* phxu;
[32]204public:
[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
216An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
217*/
218
219class EKFCh : public KalmanCh {
220        //! Internal Model f(x,u)
221        diffbifn* pfxu;
222        //! Observation Model h(x,u)
223        diffbifn* phxu;
224public:
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
237class KFcondQR : public Kalman<ldmat>, public BMcond {
238//protected:
239public:
[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
250class KFcondR : public Kalman<ldmat>, public BMcond {
251//protected:
252public:
[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
261template<class sq_T>
[32]262Kalman<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
278template<class sq_T>
279Kalman<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
286template<class sq_T>
[32]287void 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
303template<class sq_T>
[32]304void 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]345template<class sq_T>
[37]346EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
[28]347
348template<class sq_T>
[32]349void 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
365template<class sq_T>
[32]366void 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
Note: See TracBrowser for help on using the browser.