- Main Page
- Related Pages
- Classes
- Files

- File List
- File Members

# kalman.h

Go to the documentation of this file.

    00001
    00013 #ifndef KF_H
    00014 #define KF_H
    00015
    00016
    00017 #include "../math/functions.h"
    00018 #include "../stat/exp_family.h"
    00019 #include "../math/chmat.h"
    00020 #include "../base/user_info.h"
    00021
    00022 namespace bdm
    00023 {
    00024
    00029         class KalmanFull
    00030         {
    00031                 protected:
    00032                         int dimx, dimy, dimu;
    00033                         mat A, B, C, D, R, Q;
    00034
    00035                         //cache
    00036                         mat _Pp, _Ry, _iRy, _K;
    00037                 public:
    00038                         //posterior
    00040                         vec mu;
    00042                         mat P;
    00043
    00044                         bool evalll;
    00045                         double ll;
    00046                 public:
    00048                         KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
    00050                         void bayes ( const vec &dt );
    00052                         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
    00054                         KalmanFull() {};
    00055         };
    00056
    00057
    00065         template<class sq_T>
    00066
    00067         class Kalman : public BM
    00068         {
    00069                 protected:
    00071                         RV rvy;
    00073                         RV rvu;
    00075                         int dimx;
    00077                         int dimy;
    00079                         int dimu;
    00081                         mat A;
    00083                         mat B;
    00085                         mat C;
    00087                         mat D;
    00089                         sq_T Q;
    00091                         sq_T R;
    00092
    00094                         enorm<sq_T> est;
    00096                         enorm<sq_T> fy;
    00097
    00099                         mat _K;
    00101                         vec& _yp;
    00103                         sq_T& _Ry;
    00105                         vec& _mu;
    00107                         sq_T& _P;
    00108
    00109                 public:
    00111                         Kalman ( );
    00113                         Kalman ( const Kalman<sq_T> &K0 );
    00115                         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
    00117                         void set_est ( const vec &mu0, const sq_T &P0 )
    00118                         {
    00119                                 sq_T pom ( dimy );
    00120                                 est.set_parameters ( mu0,P0 );
    00121                                 P0.mult_sym ( C,pom );
    00122                                 fy.set_parameters ( C*mu0, pom );
    00123                         };
    00124
    00126                         void bayes ( const vec &dt );
    00128                         const epdf& posterior() const {return est;}
    00129                         const enorm<sq_T>* _e() const {return &est;}
    00131                         mat& __K() {return _K;}
    00133                         vec _dP() {return _P->getD();}
    00134         };
    00135
    00142         class KalmanCh : public Kalman<chmat>
    00143         {
    00144                 protected:
    00146                         mat preA;
    00148                         mat postA;
    00149
    00150                 public:
    00152                         BM* _copy_() const
    00153                         {
    00154                                 KalmanCh* K=new KalmanCh;
    00155                                 K->set_parameters ( A,B,C,D,Q,R );
    00156                                 K->set_statistics ( _mu,_P );
    00157                                 return K;
    00158                         }
    00160                         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
    00161                         void set_statistics ( const vec &mu0, const chmat &P0 )
    00162                         {
    00163                                 est.set_parameters ( mu0,P0 );
    00164                         };
    00165
    00166
    00180                         void bayes ( const vec &dt );
    00181         };
    00182
    00188         class EKFfull : public KalmanFull, public BM
    00189         {
    00190                 protected:
    00192                         diffbifn* pfxu;
    00194                         diffbifn* phxu;
    00195
    00196                         enorm<fsqmat> E;
    00197                 public:
    00199                         EKFfull ( );
    00201                         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
    00203                         void bayes ( const vec &dt );
    00205                         void set_statistics ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
    00207                         const epdf& posterior() const{return E;};
    00208                         const enorm<fsqmat>* _e() const{return &E;};
    00209                         const mat _R() {return P;}
    00210         };
    00211
    00217         template<class sq_T>
    00218         class EKF : public Kalman<fsqmat>
    00219         {
    00221                         diffbifn* pfxu;
    00223                         diffbifn* phxu;
    00224                 public:
    00226                         EKF ( RV rvx, RV rvy, RV rvu );
    00228                         EKF<sq_T>* _copy_() const { return new EKF<sq_T> ( this ); }
    00230                         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
    00232                         void bayes ( const vec &dt );
    00233         };
    00234
    00241         class EKFCh : public KalmanCh
    00242         {
    00243                 protected:
    00245                         diffbifn* pfxu;
    00247                         diffbifn* phxu;
    00248                 public:
    00250                         BM* _copy_() const
    00251                         {
    00252                                 EKFCh* E=new EKFCh;
    00253                                 E->set_parameters ( pfxu,phxu,Q,R );
    00254                                 E->set_statistics ( _mu,_P );
    00255                                 return E;
    00256                         }
    00258                         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
    00260                         void bayes ( const vec &dt );
    00261
    00262                         void from_setting( const Setting &set );
    00263
    00264                         // TODO dodelat void to_setting( Setting &set ) const;
    00265
    00266         };
    00267
    00268         UIREGISTER(EKFCh);
    00269
    00270
    00275         class KFcondQR : public Kalman<ldmat>
    00276         {
    00277 //protected:
    00278                 public:
    00279                         void condition ( const vec &QR )
    00280                         {
    00281                                 it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );
    00282
    00283                                 Q.setD ( QR ( 0, dimx-1 ) );
    00284                                 R.setD ( QR ( dimx, -1 ) );
    00285                         };
    00286         };
    00287
    00292         class KFcondR : public Kalman<ldmat>
    00293         {
    00294 //protected:
    00295                 public:
    00297                         KFcondR ( ) : Kalman<ldmat> ( ) {};
    00298
    00299                         void condition ( const vec &R0 )
    00300                         {
    00301                                 it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );
    00302
    00303                                 R.setD ( R0 );
    00304                         };
    00305
    00306         };
    00307
    00309
    00310         template<class sq_T>
    00311         Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
    00312                         dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
    00313                         A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
    00314                         Q ( K0.Q ), R ( K0.R ),
    00315                         est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
    00316         {
    00317
    00318 // copy values in pointers
    00319 //      _mu = K0._mu;
    00320 //      _P = K0._P;
    00321 //      _yp = K0._yp;
    00322 //      _Ry = K0._Ry;
    00323
    00324         }
    00325
    00326         template<class sq_T>
    00327         Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
    00328         {
    00329         };
    00330
    00331         template<class sq_T>
    00332         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 )
    00333         {
    00334                 dimx = A0.rows();
    00335                 dimy = C0.rows();
    00336                 dimu = B0.cols();
    00337
    00338                 it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
    00339                 it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
    00340                 it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
    00341                 it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
    00342                 it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
    00343                 it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
    00344
    00345                 A = A0;
    00346                 B = B0;
    00347                 C = C0;
    00348                 D = D0;
    00349                 R = R0;
    00350                 Q = Q0;
    00351         }
    00352
    00353         template<class sq_T>
    00354         void Kalman<sq_T>::bayes ( const vec &dt )
    00355         {
    00356                 it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00357
    00358                 sq_T iRy ( dimy );
    00359                 vec u = dt.get ( dimy,dimy+dimu-1 );
    00360                 vec y = dt.get ( 0,dimy-1 );
    00361                 //Time update
    00362                 _mu = A* _mu + B*u;
    00363                 //P  = A*P*A.transpose() + Q; in sq_T
    00364                 _P.mult_sym ( A );
    00365                 _P  +=Q;
    00366
    00367                 //Data update
    00368                 //_Ry = C*P*C.transpose() + R; in sq_T
    00369                 _P.mult_sym ( C, _Ry );
    00370                 _Ry  +=R;
    00371
    00372                 mat Pfull = _P.to_mat();
    00373
    00374                 _Ry.inv ( iRy ); // result is in _iRy;
    00375                 _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00376
    00377                 sq_T pom ( ( int ) Pfull.rows() );
    00378                 iRy.mult_sym_t ( C*Pfull,pom );
    00379                 ( _P ) -= pom; // P = P -PC'iRy*CP;
    00380                 ( _yp ) = C* _mu  +D*u; //y prediction
    00381                 ( _mu ) += _K* ( y- _yp );
    00382
    00383
    00384                 if ( evalll==true )   //likelihood of observation y
    00385                 {
    00386                         ll=fy.evallog ( y );
    00387                 }
    00388
    00389 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
    00390
    00391         };
    00392
    00400         class MultiModel: public BM
    00401         {
    00402                 protected:
    00404                         Array<EKFCh*> Models;
    00406                         vec w;
    00408                         vec _lls;
    00410                         int policy;
    00412                         enorm<chmat> est;
    00413                 public:
    00414                         void set_parameters ( Array<EKFCh*> A, int pol0=1 )
    00415                         {
    00416                                 Models=A;//TODO: test if evalll is set
    00417                                 w.set_length ( A.length() );
    00418                                 _lls.set_length ( A.length() );
    00419                                 policy=pol0;
    00420
    00421                                 est.set_rv(RV("MM",A(0)->posterior().dimension(),0));
    00422                                 est.set_parameters(A(0)->_e()->mean(), A(0)->_e()->_R());
    00423                         }
    00424                         void bayes ( const vec &dt )
    00425                         {
    00426                                 int n = Models.length();
    00427                                 int i;
    00428                                 for ( i=0;i<n;i++ )
    00429                                 {
    00430                                         Models ( i )->bayes ( dt );
    00431                                         _lls ( i ) = Models ( i )->_ll();
    00432                                 }
    00433                                 double mlls=max ( _lls );
    00434                                 w=exp ( _lls-mlls );
    00435                                 w/=sum ( w ); //normalization
    00436                                 //set statistics
    00437                                 switch ( policy )
    00438                                 {
    00439                                         case 1:
    00440                                         {
    00441                                                 int mi=max_index ( w );
    00442                                                 const enorm<chmat>* st=(Models(mi)->_e());
    00443                                                 est.set_parameters(st->mean(), st->_R());
    00444                                         }
    00445                                         break;
    00446                                         default: it_error ( "unknown policy" );
    00447                                 }
    00448                                 // copy result to all models
    00449                                 for ( i=0;i<n;i++ )
    00450                                 {
    00451                                                 Models ( i )->set_statistics ( est.mean(),est._R());
    00452                                 }
    00453                         }
    00454                         //all posterior densities are equal => return the first one
    00455                         const enorm<chmat>* _e() const {return &est;}
    00456                                 //all posterior densities are equal => return the first one
    00457                         const enorm<chmat>& posterior() const {return est;}
    00458
    00459                         void from_setting( const Setting &set );
    00460
    00461                         // TODO dodelat void to_setting( Setting &set ) const;
    00462
    00463         };
    00464
    00465         UIREGISTER(MultiModel);
    00466
    00467
    00468
    00469 //TODO why not const pointer??
    00470
    00471         template<class sq_T>
    00472         EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
    00473
    00474         template<class sq_T>
    00475         void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 )
    00476         {
    00477                 pfxu = pfxu0;
    00478                 phxu = phxu0;
    00479
    00480                 //initialize matrices A C, later, these will be only updated!
    00481                 pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
    00482 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
    00483                 B.clear();
    00484                 phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
    00485 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
    00486                 D.clear();
    00487
    00488                 R = R0;
    00489                 Q = Q0;
    00490         }
    00491
    00492         template<class sq_T>
    00493         void EKF<sq_T>::bayes ( const vec &dt )
    00494         {
    00495                 it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00496
    00497                 sq_T iRy ( dimy,dimy );
    00498                 vec u = dt.get ( dimy,dimy+dimu-1 );
    00499                 vec y = dt.get ( 0,dimy-1 );
    00500                 //Time update
    00501                 _mu = pfxu->eval ( _mu, u );
    00502                 pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
    00503
    00504                 //P  = A*P*A.transpose() + Q; in sq_T
    00505                 _P.mult_sym ( A );
    00506                 _P +=Q;
    00507
    00508                 //Data update
    00509                 phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
    00510                 //_Ry = C*P*C.transpose() + R; in sq_T
    00511                 _P.mult_sym ( C, _Ry );
    00512                 ( _Ry ) +=R;
    00513
    00514                 mat Pfull = _P.to_mat();
    00515
    00516                 _Ry.inv ( iRy ); // result is in _iRy;
    00517                 _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00518
    00519                 sq_T pom ( ( int ) Pfull.rows() );
    00520                 iRy.mult_sym_t ( C*Pfull,pom );
    00521                 ( _P ) -= pom; // P = P -PC'iRy*CP;
    00522                 _yp = phxu->eval ( _mu,u ); //y prediction
    00523                 ( _mu ) += _K* ( y-_yp );
    00524
    00525                 if ( evalll==true ) {ll+=fy.evallog ( y );}
    00526         };
    00527
    00528
    00529 }
    00530 #endif // KF_H
    00531
    00532