- Main Page
- Related Pages
- Modules
- Classes
- Files

# libKF.h

Go to the documentation of this file.

    00001
    00013 #ifndef KF_H
    00014 #define KF_H
    00015
    00016
    00017 #include "../stat/libFN.h"
    00018 #include "../stat/libEF.h"
    00019 #include "../math/chmat.h"
    00020
    00021 namespace bdm {
    00022
    00027 class KalmanFull {
    00028 protected:
    00029         int dimx, dimy, dimu;
    00030         mat A, B, C, D, R, Q;
    00031
    00032         //cache
    00033         mat _Pp, _Ry, _iRy, _K;
    00034 public:
    00035         //posterior
    00037         vec mu;
    00039         mat P;
    00040
    00041         bool evalll;
    00042         double ll;
    00043 public:
    00045         KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
    00047         void bayes ( const vec &dt );
    00049         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
    00051         KalmanFull() {};
    00052 };
    00053
    00054
    00062 template<class sq_T>
    00063
    00064 class Kalman : public BM {
    00065 protected:
    00067         RV rvy;
    00069         RV rvu;
    00071         int dimx;
    00073         int dimy;
    00075         int dimu;
    00077         mat A;
    00079         mat B;
    00081         mat C;
    00083         mat D;
    00085         sq_T Q;
    00087         sq_T R;
    00088
    00090         enorm<sq_T> est;
    00092         enorm<sq_T> fy;
    00093
    00095         mat _K;
    00097         vec& _yp;
    00099         sq_T& _Ry;
    00101         vec& _mu;
    00103         sq_T& _P;
    00104
    00105 public:
    00107         Kalman ( );
    00109         Kalman ( const Kalman<sq_T> &K0 );
    00111         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
    00113         void set_est ( const vec &mu0, const sq_T &P0 ) {
    00114                 sq_T pom ( dimy );
    00115                 est.set_parameters ( mu0,P0 );
    00116                 P0.mult_sym ( C,pom );
    00117                 fy.set_parameters ( C*mu0, pom );
    00118         };
    00119
    00121         void bayes ( const vec &dt );
    00123         const epdf& posterior() const {return est;}
    00124         const enorm<sq_T>* _e() const {return &est;}
    00126         mat& __K() {return _K;}
    00128         vec _dP() {return _P->getD();}
    00129 };
    00130
    00137 class KalmanCh : public Kalman<chmat> {
    00138 protected:
    00140         mat preA;
    00142         mat postA;
    00143
    00144 public:
    00146         BM* _copy_() const {
    00147                 KalmanCh* K=new KalmanCh;
    00148                 K->set_parameters ( A,B,C,D,Q,R );
    00149                 K->set_statistics ( _mu,_P );
    00150                 return K;
    00151         }
    00153         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
    00154         void set_statistics ( const vec &mu0, const chmat &P0 ) {
    00155                 est.set_parameters ( mu0,P0 );
    00156         };
    00157
    00158
    00172         void bayes ( const vec &dt );
    00173 };
    00174
    00180 class EKFfull : public KalmanFull, public BM {
    00181
    00183         diffbifn* pfxu;
    00185         diffbifn* phxu;
    00186
    00187         enorm<fsqmat> E;
    00188 public:
    00190         EKFfull ( );
    00192         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
    00194         void bayes ( const vec &dt );
    00196         void set_est ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
    00198         const epdf& posterior() const{return E;};
    00199         const enorm<fsqmat>* _e() const{return &E;};
    00200         const mat _R() {return P;}
    00201 };
    00202
    00208 template<class sq_T>
    00209 class EKF : public Kalman<fsqmat> {
    00211         diffbifn* pfxu;
    00213         diffbifn* phxu;
    00214 public:
    00216         EKF ( RV rvx, RV rvy, RV rvu );
    00218         EKF<sq_T>* _copy_() const { return new EKF<sq_T>(this); }
    00220         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
    00222         void bayes ( const vec &dt );
    00223 };
    00224
    00231 class EKFCh : public KalmanCh {
    00232 protected:
    00234         diffbifn* pfxu;
    00236         diffbifn* phxu;
    00237 public:
    00239         BM* _copy_() const {
    00240                 EKFCh* E=new EKFCh;
    00241                 E->set_parameters ( pfxu,phxu,Q,R );
    00242                 E->set_statistics ( _mu,_P );
    00243                 return E;
    00244         }
    00246         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
    00248         void bayes ( const vec &dt );
    00249 };
    00250
    00255 class KFcondQR : public Kalman<ldmat> {
    00256 //protected:
    00257 public:
    00258         void condition ( const vec &QR ) {
    00259                 it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );
    00260
    00261                 Q.setD ( QR ( 0, dimx-1 ) );
    00262                 R.setD ( QR ( dimx, -1 ) );
    00263         };
    00264 };
    00265
    00270 class KFcondR : public Kalman<ldmat> {
    00271 //protected:
    00272 public:
    00274         KFcondR ( ) : Kalman<ldmat> ( ) {};
    00275
    00276         void condition ( const vec &R0 ) {
    00277                 it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );
    00278
    00279                 R.setD ( R0 );
    00280         };
    00281
    00282 };
    00283
    00285
    00286 template<class sq_T>
    00287 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
    00288                 dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
    00289                 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
    00290                 Q ( K0.Q ), R ( K0.R ),
    00291                 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) {
    00292
    00293 // copy values in pointers
    00294 //      _mu = K0._mu;
    00295 //      _P = K0._P;
    00296 //      _yp = K0._yp;
    00297 //      _Ry = K0._Ry;
    00298
    00299 }
    00300
    00301 template<class sq_T>
    00302 Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (),  _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() ) {
    00303 };
    00304
    00305 template<class sq_T>
    00306 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 ) {
    00307         dimx = A0.rows();
    00308         dimy = C0.rows();
    00309         dimu = B0.cols();
    00310
    00311         it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
    00312         it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
    00313         it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
    00314         it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
    00315         it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
    00316         it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
    00317
    00318         A = A0;
    00319         B = B0;
    00320         C = C0;
    00321         D = D0;
    00322         R = R0;
    00323         Q = Q0;
    00324 }
    00325
    00326 template<class sq_T>
    00327 void Kalman<sq_T>::bayes ( const vec &dt ) {
    00328         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00329
    00330         sq_T iRy ( dimy );
    00331         vec u = dt.get ( dimy,dimy+dimu-1 );
    00332         vec y = dt.get ( 0,dimy-1 );
    00333         //Time update
    00334         _mu = A* _mu + B*u;
    00335         //P  = A*P*A.transpose() + Q; in sq_T
    00336         _P.mult_sym ( A );
    00337         _P  +=Q;
    00338
    00339         //Data update
    00340         //_Ry = C*P*C.transpose() + R; in sq_T
    00341         _P.mult_sym ( C, _Ry );
    00342         _Ry  +=R;
    00343
    00344         mat Pfull = _P.to_mat();
    00345
    00346         _Ry.inv ( iRy ); // result is in _iRy;
    00347         _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00348
    00349         sq_T pom ( ( int ) Pfull.rows() );
    00350         iRy.mult_sym_t ( C*Pfull,pom );
    00351         ( _P ) -= pom; // P = P -PC'iRy*CP;
    00352         ( _yp ) = C* _mu  +D*u; //y prediction
    00353         ( _mu ) += _K* ( y- _yp );
    00354
    00355
    00356         if ( evalll==true ) { //likelihood of observation y
    00357                 ll=fy.evallog ( y );
    00358         }
    00359
    00360 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
    00361
    00362 };
    00363
    00364
    00365
    00366 //TODO why not const pointer??
    00367
    00368 template<class sq_T>
    00369 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
    00370
    00371 template<class sq_T>
    00372 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) {
    00373         pfxu = pfxu0;
    00374         phxu = phxu0;
    00375
    00376         //initialize matrices A C, later, these will be only updated!
    00377         pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
    00378 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
    00379         B.clear();
    00380         phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
    00381 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
    00382         D.clear();
    00383
    00384         R = R0;
    00385         Q = Q0;
    00386 }
    00387
    00388 template<class sq_T>
    00389 void EKF<sq_T>::bayes ( const vec &dt ) {
    00390         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00391
    00392         sq_T iRy ( dimy,dimy );
    00393         vec u = dt.get ( dimy,dimy+dimu-1 );
    00394         vec y = dt.get ( 0,dimy-1 );
    00395         //Time update
    00396         _mu = pfxu->eval ( _mu, u );
    00397         pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
    00398
    00399         //P  = A*P*A.transpose() + Q; in sq_T
    00400         _P.mult_sym ( A );
    00401         _P +=Q;
    00402
    00403         //Data update
    00404         phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
    00405         //_Ry = C*P*C.transpose() + R; in sq_T
    00406         _P.mult_sym ( C, _Ry );
    00407         ( _Ry ) +=R;
    00408
    00409         mat Pfull = _P.to_mat();
    00410
    00411         _Ry.inv ( iRy ); // result is in _iRy;
    00412         _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00413
    00414         sq_T pom ( ( int ) Pfull.rows() );
    00415         iRy.mult_sym_t ( C*Pfull,pom );
    00416         ( _P ) -= pom; // P = P -PC'iRy*CP;
    00417         _yp = phxu->eval ( _mu,u ); //y prediction
    00418         ( _mu ) += _K* ( y-_yp );
    00419
    00420         if ( evalll==true ) {ll+=fy.evallog ( y );}
    00421 };
    00422
    00423
    00424 }
    00425 #endif // KF_H
    00426
    00427