- 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
    00028 class KalmanFull {
    00029 protected:
    00030         int dimx, dimy, dimu;
    00031         mat A, B, C, D, R, Q;
    00032
    00033         //cache
    00034         mat _Pp, _Ry, _iRy, _K;
    00035 public:
    00036         //posterior
    00038         vec mu;
    00040         mat P;
    00041
    00042         bool evalll;
    00043         double ll;
    00044 public:
    00046         KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
    00048         void bayes ( const vec &dt );
    00050         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
    00052         KalmanFull() {};
    00053 };
    00054
    00055
    00063 template<class sq_T>
    00064
    00065 class Kalman : public BM {
    00066 protected:
    00068         RV rvy;
    00070         RV rvu;
    00072         int dimx;
    00074         int dimy;
    00076         int dimu;
    00078         mat A;
    00080         mat B;
    00082         mat C;
    00084         mat D;
    00086         sq_T Q;
    00088         sq_T R;
    00089
    00091         enorm<sq_T> est;
    00093         enorm<sq_T> fy;
    00094
    00096         mat _K;
    00098         vec& _yp;
    00100         sq_T& _Ry;
    00102         vec& _mu;
    00104         sq_T& _P;
    00105
    00106 public:
    00108         Kalman ( );
    00110         Kalman ( const Kalman<sq_T> &K0 );
    00112         void set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const sq_T &Q0, const sq_T &R0 );
    00114         void set_est ( const vec &mu0, const sq_T &P0 ) {
    00115                 sq_T pom ( dimy );
    00116                 est.set_parameters ( mu0, P0 );
    00117                 P0.mult_sym ( C, pom );
    00118                 fy.set_parameters ( C*mu0, pom );
    00119         };
    00120
    00122         void bayes ( const vec &dt );
    00124         const epdf& posterior() const {
    00125                 return est;
    00126         }
    00128         mat& __K() {
    00129                 return _K;
    00130         }
    00132         vec _dP() {
    00133                 return _P->getD();
    00134         }
    00135 };
    00136
    00143 class KalmanCh : public Kalman<chmat> {
    00144 protected:
    00146         mat preA;
    00148         mat postA;
    00149
    00150 public:
    00152         BM* _copy_() const {
    00153                 KalmanCh* K = new KalmanCh;
    00154                 K->set_parameters ( A, B, C, D, Q, R );
    00155                 K->set_statistics ( _mu, _P );
    00156                 return K;
    00157         }
    00159         void set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 );
    00160         void set_statistics ( const vec &mu0, const chmat &P0 ) {
    00161                 est.set_parameters ( mu0, P0 );
    00162         };
    00163
    00164
    00178         void bayes ( const vec &dt );
    00179 };
    00180
    00186 class EKFfull : public KalmanFull, public BM {
    00187 protected:
    00189         shared_ptr<diffbifn> pfxu;
    00190
    00192         shared_ptr<diffbifn> phxu;
    00193
    00194         enorm<fsqmat> E;
    00195 public:
    00197         EKFfull ( );
    00198
    00200         void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0 );
    00201
    00203         void bayes ( const vec &dt );
    00205         void set_statistics ( vec mu0, mat P0 ) {
    00206                 mu = mu0;
    00207                 P = P0;
    00208         };
    00210         const epdf& posterior() const {
    00211                 return E;
    00212         };
    00213         const mat _R() {
    00214                 return P;
    00215         }
    00216 };
    00217
    00223 template<class sq_T>
    00224 class EKF : public Kalman<fsqmat> {
    00226         diffbifn* pfxu;
    00228         diffbifn* phxu;
    00229 public:
    00231         EKF ( RV rvx, RV rvy, RV rvu );
    00233         EKF<sq_T>* _copy_() const {
    00234                 return new EKF<sq_T> ( this );
    00235         }
    00237         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
    00239         void bayes ( const vec &dt );
    00240 };
    00241
    00248 class EKFCh : public KalmanCh {
    00249 protected:
    00251         shared_ptr<diffbifn> pfxu;
    00252
    00254         shared_ptr<diffbifn> phxu;
    00255 public:
    00257         BM* _copy_() const {
    00258                 EKFCh* E = new EKFCh;
    00259                 E->set_parameters ( pfxu, phxu, Q, R );
    00260                 E->set_statistics ( _mu, _P );
    00261                 return E;
    00262         }
    00264         void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0 );
    00265
    00267         void bayes ( const vec &dt );
    00268
    00269         void from_setting ( const Setting &set );
    00270
    00271         // TODO dodelat void to_setting( Setting &set ) const;
    00272
    00273         const enorm<chmat>& posterior() {return est;}
    00274 };
    00275
    00276 UIREGISTER ( EKFCh );
    00277 SHAREDPTR ( EKFCh );
    00278
    00279
    00284 class KFcondQR : public Kalman<ldmat> {
    00285 //protected:
    00286 public:
    00287         void condition ( const vec &QR ) {
    00288                 it_assert_debug ( QR.length() == ( dimx + dimy ), "KFcondRQ: conditioning by incompatible vector" );
    00289
    00290                 Q.setD ( QR ( 0, dimx - 1 ) );
    00291                 R.setD ( QR ( dimx, -1 ) );
    00292         };
    00293 };
    00294
    00299 class KFcondR : public Kalman<ldmat> {
    00300 //protected:
    00301 public:
    00303         KFcondR ( ) : Kalman<ldmat> ( ) {};
    00304
    00305         void condition ( const vec &R0 ) {
    00306                 it_assert_debug ( R0.length() == ( dimy ), "KFcondR: conditioning by incompatible vector" );
    00307
    00308                 R.setD ( R0 );
    00309         };
    00310
    00311 };
    00312
    00314
    00315 template<class sq_T>
    00316 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ), rvy ( K0.rvy ), rvu ( K0.rvu ),
    00317                 dimx ( K0.dimx ), dimy ( K0.dimy ), dimu ( K0.dimu ),
    00318                 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
    00319                 Q ( K0.Q ), R ( K0.R ),
    00320                 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) {
    00321
    00322 // copy values in pointers
    00323 //      _mu = K0._mu;
    00324 //      _P = K0._P;
    00325 //      _yp = K0._yp;
    00326 //      _Ry = K0._Ry;
    00327
    00328 }
    00329
    00330 template<class sq_T>
    00331 Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) {
    00332 };
    00333
    00334 template<class sq_T>
    00335 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 ) {
    00336         dimx = A0.rows();
    00337         dimy = C0.rows();
    00338         dimu = B0.cols();
    00339
    00340         it_assert_debug ( A0.cols() == dimx, "Kalman: A is not square" );
    00341         it_assert_debug ( B0.rows() == dimx, "Kalman: B is not compatible" );
    00342         it_assert_debug ( C0.cols() == dimx, "Kalman: C is not square" );
    00343         it_assert_debug ( ( D0.rows() == dimy ) || ( D0.cols() == dimu ),       "Kalman: D is not compatible" );
    00344         it_assert_debug ( ( R0.cols() == dimy ) || ( R0.rows() == dimy ), "Kalman: R is not compatible" );
    00345         it_assert_debug ( ( Q0.cols() == dimx ) || ( Q0.rows() == dimx ), "Kalman: Q is not compatible" );
    00346
    00347         A = A0;
    00348         B = B0;
    00349         C = C0;
    00350         D = D0;
    00351         R = R0;
    00352         Q = Q0;
    00353 }
    00354
    00355 template<class sq_T>
    00356 void Kalman<sq_T>::bayes ( const vec &dt ) {
    00357         it_assert_debug ( dt.length() == ( dimy + dimu ), "KalmanFull::bayes wrong size of dt" );
    00358
    00359         sq_T iRy ( dimy );
    00360         vec u = dt.get ( dimy, dimy + dimu - 1 );
    00361         vec y = dt.get ( 0, dimy - 1 );
    00362         //Time update
    00363         _mu = A * _mu + B * u;
    00364         //P  = A*P*A.transpose() + Q; in sq_T
    00365         _P.mult_sym ( A );
    00366         _P  += Q;
    00367
    00368         //Data update
    00369         //_Ry = C*P*C.transpose() + R; in sq_T
    00370         _P.mult_sym ( C, _Ry );
    00371         _Ry  += R;
    00372
    00373         mat Pfull = _P.to_mat();
    00374
    00375         _Ry.inv ( iRy ); // result is in _iRy;
    00376         _K = Pfull * C.transpose() * ( iRy.to_mat() );
    00377
    00378         sq_T pom ( ( int ) Pfull.rows() );
    00379         iRy.mult_sym_t ( C*Pfull, pom );
    00380         ( _P ) -= pom; // P = P -PC'iRy*CP;
    00381         ( _yp ) = C * _mu  + D * u; //y prediction
    00382         ( _mu ) += _K * ( y - _yp );
    00383
    00384
    00385         if ( evalll == true ) { //likelihood of observation y
    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 protected:
    00403         Array<EKFCh*> Models;
    00405         vec w;
    00407         vec _lls;
    00409         int policy;
    00411         enorm<chmat> est;
    00412 public:
    00413         void set_parameters ( Array<EKFCh*> A, int pol0 = 1 ) {
    00414                 Models = A;//TODO: test if evalll is set
    00415                 w.set_length ( A.length() );
    00416                 _lls.set_length ( A.length() );
    00417                 policy = pol0;
    00418
    00419                 est.set_rv ( RV ( "MM", A ( 0 )->posterior().dimension(), 0 ) );
    00420                 est.set_parameters ( A ( 0 )->posterior().mean(), A ( 0 )->posterior()._R() );
    00421         }
    00422         void bayes ( const vec &dt ) {
    00423                 int n = Models.length();
    00424                 int i;
    00425                 for ( i = 0; i < n; i++ ) {
    00426                         Models ( i )->bayes ( dt );
    00427                         _lls ( i ) = Models ( i )->_ll();
    00428                 }
    00429                 double mlls = max ( _lls );
    00430                 w = exp ( _lls - mlls );
    00431                 w /= sum ( w ); //normalization
    00432                 //set statistics
    00433                 switch ( policy ) {
    00434                 case 1: {
    00435                         int mi = max_index ( w );
    00436                         const enorm<chmat> &st = Models ( mi )->posterior() ;
    00437                         est.set_parameters ( st.mean(), st._R() );
    00438                 }
    00439                 break;
    00440                 default:
    00441                         it_error ( "unknown policy" );
    00442                 }
    00443                 // copy result to all models
    00444                 for ( i = 0; i < n; i++ ) {
    00445                         Models ( i )->set_statistics ( est.mean(), est._R() );
    00446                 }
    00447         }
    00449         const enorm<chmat>& posterior() const {
    00450                 return est;
    00451         }
    00452
    00453         void from_setting ( const Setting &set );
    00454
    00455         // TODO dodelat void to_setting( Setting &set ) const;
    00456
    00457 };
    00458
    00459 UIREGISTER ( MultiModel );
    00460 SHAREDPTR ( MultiModel );
    00461
    00462
    00463
    00464 //TODO why not const pointer??
    00465
    00466 template<class sq_T>
    00467 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0, rvy0, rvu0 ) {}
    00468
    00469 template<class sq_T>
    00470 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0, const sq_T Q0, const sq_T R0 ) {
    00471         pfxu = pfxu0;
    00472         phxu = phxu0;
    00473
    00474         //initialize matrices A C, later, these will be only updated!
    00475         pfxu->dfdx_cond ( _mu, zeros ( dimu ), A, true );
    00476 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
    00477         B.clear();
    00478         phxu->dfdx_cond ( _mu, zeros ( dimu ), C, true );
    00479 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
    00480         D.clear();
    00481
    00482         R = R0;
    00483         Q = Q0;
    00484 }
    00485
    00486 template<class sq_T>
    00487 void EKF<sq_T>::bayes ( const vec &dt ) {
    00488         it_assert_debug ( dt.length() == ( dimy + dimu ), "KalmanFull::bayes wrong size of dt" );
    00489
    00490         sq_T iRy ( dimy, dimy );
    00491         vec u = dt.get ( dimy, dimy + dimu - 1 );
    00492         vec y = dt.get ( 0, dimy - 1 );
    00493         //Time update
    00494         _mu = pfxu->eval ( _mu, u );
    00495         pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx
    00496
    00497         //P  = A*P*A.transpose() + Q; in sq_T
    00498         _P.mult_sym ( A );
    00499         _P += Q;
    00500
    00501         //Data update
    00502         phxu->dfdx_cond ( _mu, u, C, false ); //update C by a derivative hx
    00503         //_Ry = C*P*C.transpose() + R; in sq_T
    00504         _P.mult_sym ( C, _Ry );
    00505         ( _Ry ) += R;
    00506
    00507         mat Pfull = _P.to_mat();
    00508
    00509         _Ry.inv ( iRy ); // result is in _iRy;
    00510         _K = Pfull * C.transpose() * ( iRy.to_mat() );
    00511
    00512         sq_T pom ( ( int ) Pfull.rows() );
    00513         iRy.mult_sym_t ( C*Pfull, pom );
    00514         ( _P ) -= pom; // P = P -PC'iRy*CP;
    00515         _yp = phxu->eval ( _mu, u ); //y prediction
    00516         ( _mu ) += _K * ( y - _yp );
    00517
    00518         if ( evalll == true ) {
    00519                 ll += fy.evallog ( y );
    00520         }
    00521 };
    00522
    00523
    00524 }
    00525 #endif // KF_H
    00526
    00527