root/bdm/estim/libKF.h @ 357

Revision 357, 13.1 kB (checked in by mido, 15 years ago)

mnoho zmen:
1) presun FindXXX modulu do \system
2) zalozeni dokumentace \doc\local\library_structure.dox
3) presun obsahu \tests\UI primo do \tests
4) namisto \INSTALL zalozen \install.html, je to vhodnejsi pro uzivatele WINDOWS, a snad i obecne
5) snaha o predelani veskerych UI podle nove koncepce, soubory pmsm_ui.h, arx_ui.h, KF_ui.h, libDS_ui.h, libEF_ui.h a loggers_ui.h ponechavam
jen zdokumentacnich duvodu, nic by na nich jiz nemelo zaviset, a po zkontrolovani spravnosti provedenych uprav by mely byt smazany
6) predelani estimatoru tak, aby fungoval s novym UI konceptem
7) vytazeni tridy bdmroot do samostatneho souboru \bdm\bdmroot.h
8) pridana dokumentace pro zacleneni programu ASTYLE do Visual studia, ASTYLE pridan do instalacniho balicku pro Windows

  • 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        /*!
25        * \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon!
26        */
27
28        class KalmanFull
29        {
30                protected:
31                        int dimx, dimy, dimu;
32                        mat A, B, C, D, R, Q;
33
34                        //cache
35                        mat _Pp, _Ry, _iRy, _K;
36                public:
37                        //posterior
38                        //! Mean value of the posterior density
39                        vec mu;
40                        //! Variance of the posterior density
41                        mat P;
42
43                        bool evalll;
44                        double ll;
45                public:
46                        //! Full constructor
47                        KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
48                        //! Here dt = [yt;ut] of appropriate dimensions
49                        void bayes ( const vec &dt );
50                        //! print elements of KF
51                        friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
52                        //! For EKFfull;
53                        KalmanFull() {};
54        };
55
56
57        /*!
58        * \brief Kalman filter with covariance matrices in square root form.
59
60        Parameter evolution model:\f[ x_t = A x_{t-1} + B u_t + Q^{1/2} e_t \f]
61        Observation model: \f[ y_t = C x_{t-1} + C u_t + Q^{1/2} w_t. \f]
62        Where $e_t$ and $w_t$ are independent vectors Normal(0,1)-distributed disturbances.
63        */
64        template<class sq_T>
65
66        class Kalman : public BM
67        {
68                protected:
69                        //! Indetifier of output rv
70                        RV rvy;
71                        //! Indetifier of exogeneous rv
72                        RV rvu;
73                        //! cache of rv.count()
74                        int dimx;
75                        //! cache of rvy.count()
76                        int dimy;
77                        //! cache of rvu.count()
78                        int dimu;
79                        //! Matrix A
80                        mat A;
81                        //! Matrix B
82                        mat B;
83                        //! Matrix C
84                        mat C;
85                        //! Matrix D
86                        mat D;
87                        //! Matrix Q in square-root form
88                        sq_T Q;
89                        //! Matrix R in square-root form
90                        sq_T R;
91
92                        //!posterior density on $x_t$
93                        enorm<sq_T> est;
94                        //!preditive density on $y_t$
95                        enorm<sq_T> fy;
96
97                        //! placeholder for Kalman gain
98                        mat _K;
99                        //! cache of fy.mu
100                        vec& _yp;
101                        //! cache of fy.R
102                        sq_T& _Ry;
103                        //!cache of est.mu
104                        vec& _mu;
105                        //!cache of est.R
106                        sq_T& _P;
107
108                public:
109                        //! Default constructor
110                        Kalman ( );
111                        //! Copy constructor
112                        Kalman ( const Kalman<sq_T> &K0 );
113                        //! Set parameters with check of relevance
114                        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
115                        //! Set estimate values, used e.g. in initialization.
116                        void set_est ( const vec &mu0, const sq_T &P0 )
117                        {
118                                sq_T pom ( dimy );
119                                est.set_parameters ( mu0,P0 );
120                                P0.mult_sym ( C,pom );
121                                fy.set_parameters ( C*mu0, pom );
122                        };
123
124                        //! Here dt = [yt;ut] of appropriate dimensions
125                        void bayes ( const vec &dt );
126                        //!access function
127                        const epdf& posterior() const {return est;}
128                        const enorm<sq_T>* _e() const {return &est;}
129                        //!access function
130                        mat& __K() {return _K;}
131                        //!access function
132                        vec _dP() {return _P->getD();}
133        };
134
135        /*! \brief Kalman filter in square root form
136
137        Trivial example:
138        \include kalman_simple.cpp
139
140        */
141        class KalmanCh : public Kalman<chmat>
142        {
143                protected:
144//! pre array (triangular matrix)
145                        mat preA;
146//! post array (triangular matrix)
147                        mat postA;
148
149                public:
150                        //! copy constructor
151                        BM* _copy_() const
152                        {
153                                KalmanCh* K=new KalmanCh;
154                                K->set_parameters ( A,B,C,D,Q,R );
155                                K->set_statistics ( _mu,_P );
156                                return K;
157                        }
158                        //! Set parameters with check of relevance
159                        void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
160                        void set_statistics ( const vec &mu0, const chmat &P0 )
161                        {
162                                est.set_parameters ( mu0,P0 );
163                        };
164
165
166                        /*!\brief  Here dt = [yt;ut] of appropriate dimensions
167
168                        The following equality hold::\f[
169                        \left[\begin{array}{cc}
170                        R^{0.5}\\
171                        P_{t|t-1}^{0.5}C' & P_{t|t-1}^{0.5}CA'\\
172                        & Q^{0.5}\end{array}\right]<\mathrm{orth.oper.}>=\left[\begin{array}{cc}
173                        R_{y}^{0.5} & KA'\\
174                        & P_{t+1|t}^{0.5}\\
175                        \\\end{array}\right]\f]
176
177                        Thus this object evaluates only predictors! Not filtering densities.
178                        */
179                        void bayes ( const vec &dt );
180        };
181
182        /*!
183        \brief Extended Kalman Filter in full matrices
184
185        An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
186        */
187        class EKFfull : public KalmanFull, public BM
188        {
189                protected:
190                        //! Internal Model f(x,u)
191                        diffbifn* pfxu;
192                        //! Observation Model h(x,u)
193                        diffbifn* phxu;
194
195                        enorm<fsqmat> E;
196                public:
197                        //! Default constructor
198                        EKFfull ( );
199                        //! Set nonlinear functions for mean values and covariance matrices.
200                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
201                        //! Here dt = [yt;ut] of appropriate dimensions
202                        void bayes ( const vec &dt );
203                        //! set estimates
204                        void set_statistics ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
205                        //!dummy!
206                        const epdf& posterior() const{return E;};
207                        const enorm<fsqmat>* _e() const{return &E;};
208                        const mat _R() {return P;}
209        };
210
211        /*!
212        \brief Extended Kalman Filter
213
214        An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
215        */
216        template<class sq_T>
217        class EKF : public Kalman<fsqmat>
218        {
219                        //! Internal Model f(x,u)
220                        diffbifn* pfxu;
221                        //! Observation Model h(x,u)
222                        diffbifn* phxu;
223                public:
224                        //! Default constructor
225                        EKF ( RV rvx, RV rvy, RV rvu );
226                        //! copy constructor
227                        EKF<sq_T>* _copy_() const { return new EKF<sq_T> ( this ); }
228                        //! Set nonlinear functions for mean values and covariance matrices.
229                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
230                        //! Here dt = [yt;ut] of appropriate dimensions
231                        void bayes ( const vec &dt );
232        };
233
234        /*!
235        \brief Extended Kalman Filter in Square root
236
237        An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
238        */
239
240        class EKFCh : public KalmanCh
241        {
242                protected:
243                        //! Internal Model f(x,u)
244                        diffbifn* pfxu;
245                        //! Observation Model h(x,u)
246                        diffbifn* phxu;
247                public:
248                        //! copy constructor duplicated - calls different set_parameters
249                        BM* _copy_() const
250                        {
251                                EKFCh* E=new EKFCh;
252                                E->set_parameters ( pfxu,phxu,Q,R );
253                                E->set_statistics ( _mu,_P );
254                                return E;
255                        }
256                        //! Set nonlinear functions for mean values and covariance matrices.
257                        void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
258                        //! Here dt = [yt;ut] of appropriate dimensions
259                        void bayes ( const vec &dt );
260
261                        void from_setting( const Setting &root );
262
263                        // TODO dodelat void to_setting( Setting &root ) const;
264
265        };
266
267        UIREGISTER(EKFCh);
268
269
270        /*!
271        \brief Kalman Filter with conditional diagonal matrices R and Q.
272        */
273
274        class KFcondQR : public Kalman<ldmat>
275        {
276//protected:
277                public:
278                        void condition ( const vec &QR )
279                        {
280                                it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );
281
282                                Q.setD ( QR ( 0, dimx-1 ) );
283                                R.setD ( QR ( dimx, -1 ) );
284                        };
285        };
286
287        /*!
288        \brief Kalman Filter with conditional diagonal matrices R and Q.
289        */
290
291        class KFcondR : public Kalman<ldmat>
292        {
293//protected:
294                public:
295                        //!Default constructor
296                        KFcondR ( ) : Kalman<ldmat> ( ) {};
297
298                        void condition ( const vec &R0 )
299                        {
300                                it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );
301
302                                R.setD ( R0 );
303                        };
304
305        };
306
307//////// INstance
308
309        template<class sq_T>
310        Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
311                        dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
312                        A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
313                        Q ( K0.Q ), R ( K0.R ),
314                        est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
315        {
316
317// copy values in pointers
318//      _mu = K0._mu;
319//      _P = K0._P;
320//      _yp = K0._yp;
321//      _Ry = K0._Ry;
322
323        }
324
325        template<class sq_T>
326        Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
327        {
328        };
329
330        template<class sq_T>
331        void 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 )
332        {
333                dimx = A0.rows();
334                dimy = C0.rows();
335                dimu = B0.cols();
336
337                it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
338                it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
339                it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
340                it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
341                it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
342                it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
343
344                A = A0;
345                B = B0;
346                C = C0;
347                D = D0;
348                R = R0;
349                Q = Q0;
350        }
351
352        template<class sq_T>
353        void Kalman<sq_T>::bayes ( const vec &dt )
354        {
355                it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
356
357                sq_T iRy ( dimy );
358                vec u = dt.get ( dimy,dimy+dimu-1 );
359                vec y = dt.get ( 0,dimy-1 );
360                //Time update
361                _mu = A* _mu + B*u;
362                //P  = A*P*A.transpose() + Q; in sq_T
363                _P.mult_sym ( A );
364                _P  +=Q;
365
366                //Data update
367                //_Ry = C*P*C.transpose() + R; in sq_T
368                _P.mult_sym ( C, _Ry );
369                _Ry  +=R;
370
371                mat Pfull = _P.to_mat();
372
373                _Ry.inv ( iRy ); // result is in _iRy;
374                _K = Pfull*C.transpose() * ( iRy.to_mat() );
375
376                sq_T pom ( ( int ) Pfull.rows() );
377                iRy.mult_sym_t ( C*Pfull,pom );
378                ( _P ) -= pom; // P = P -PC'iRy*CP;
379                ( _yp ) = C* _mu  +D*u; //y prediction
380                ( _mu ) += _K* ( y- _yp );
381
382
383                if ( evalll==true )   //likelihood of observation y
384                {
385                        ll=fy.evallog ( y );
386                }
387
388//cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
389
390        };
391
392        /*! \brief (Switching) Multiple Model
393        The model runs several models in parallel and evaluates thier weights (fittness).
394
395        The statistics of the resulting density are merged using (geometric?) combination.
396
397        The next step is performed with the new statistics for all models.
398        */
399        class MultiModel: public BM
400        {
401                protected:
402                        //! List of models between which we switch
403                        Array<EKFCh*> Models;
404                        //! vector of model weights
405                        vec w;
406                        //! cache of model lls
407                        vec _lls;
408                        //! type of switching policy [1=maximum,2=...]
409                        int policy;
410                        //! internal statistics
411                        enorm<chmat> est;
412                public:
413                        void set_parameters ( Array<EKFCh*> A, int pol0=1 )
414                        {
415                                Models=A;//TODO: test if evalll is set
416                                w.set_length ( A.length() );
417                                _lls.set_length ( A.length() );
418                                policy=pol0;
419                               
420                                est.set_rv(RV("MM",A(0)->posterior().dimension(),0));
421                                est.set_parameters(A(0)->_e()->mean(), A(0)->_e()->_R());
422                        }
423                        void bayes ( const vec &dt )
424                        {
425                                int n = Models.length();
426                                int i;
427                                for ( i=0;i<n;i++ )
428                                {
429                                        Models ( i )->bayes ( dt );
430                                        _lls ( i ) = Models ( i )->_ll();
431                                }
432                                double mlls=max ( _lls );
433                                w=exp ( _lls-mlls );
434                                w/=sum ( w ); //normalization
435                                //set statistics
436                                switch ( policy )
437                                {
438                                        case 1:
439                                        {
440                                                int mi=max_index ( w );
441                                                const enorm<chmat>* st=(Models(mi)->_e());
442                                                est.set_parameters(st->mean(), st->_R());
443                                        }
444                                        break;
445                                        default: it_error ( "unknown policy" );
446                                }
447                                // copy result to all models
448                                for ( i=0;i<n;i++ )
449                                {
450                                                Models ( i )->set_statistics ( est.mean(),est._R());
451                                }
452                        }
453                        //all posterior densities are equal => return the first one
454                        const enorm<chmat>* _e() const {return &est;}
455                                //all posterior densities are equal => return the first one
456                        const enorm<chmat>& posterior() const {return est;}
457
458                        void from_setting( const Setting &root );
459
460                        // TODO dodelat void to_setting( Setting &root ) const;
461
462        };
463
464        UIREGISTER(MultiModel);
465
466
467
468//TODO why not const pointer??
469
470        template<class sq_T>
471        EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
472
473        template<class sq_T>
474        void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 )
475        {
476                pfxu = pfxu0;
477                phxu = phxu0;
478
479                //initialize matrices A C, later, these will be only updated!
480                pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
481//      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
482                B.clear();
483                phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
484//      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
485                D.clear();
486
487                R = R0;
488                Q = Q0;
489        }
490
491        template<class sq_T>
492        void EKF<sq_T>::bayes ( const vec &dt )
493        {
494                it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
495
496                sq_T iRy ( dimy,dimy );
497                vec u = dt.get ( dimy,dimy+dimu-1 );
498                vec y = dt.get ( 0,dimy-1 );
499                //Time update
500                _mu = pfxu->eval ( _mu, u );
501                pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
502
503                //P  = A*P*A.transpose() + Q; in sq_T
504                _P.mult_sym ( A );
505                _P +=Q;
506
507                //Data update
508                phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
509                //_Ry = C*P*C.transpose() + R; in sq_T
510                _P.mult_sym ( C, _Ry );
511                ( _Ry ) +=R;
512
513                mat Pfull = _P.to_mat();
514
515                _Ry.inv ( iRy ); // result is in _iRy;
516                _K = Pfull*C.transpose() * ( iRy.to_mat() );
517
518                sq_T pom ( ( int ) Pfull.rows() );
519                iRy.mult_sym_t ( C*Pfull,pom );
520                ( _P ) -= pom; // P = P -PC'iRy*CP;
521                _yp = phxu->eval ( _mu,u ); //y prediction
522                ( _mu ) += _K* ( y-_yp );
523
524                if ( evalll==true ) {ll+=fy.evallog ( y );}
525        };
526
527
528}
529#endif // KF_H
530
Note: See TracBrowser for help on using the browser.