root/bdm/estim/libKF.h @ 279

Revision 279, 9.9 kB (checked in by smidl, 15 years ago)

Transition of pmsm and libKF

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