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

# work/git/mixpp/bdm/estim/libKF.h

Go to the documentation of this file.

    00001
    00013 #ifndef KF_H
    00014 #define KF_H
    00015
    00016 #include <itpp/itbase.h>
    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 ( RV rvx0, RV rvy0, RV rvu0 );
    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 &R0,const sq_T &Q0 );
    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& _epdf() 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
    00133 class KalmanCh : public Kalman<chmat>{
    00134 protected:
    00136 mat preA;
    00138 mat postA;
    00139
    00140 public:
    00142         KalmanCh ( RV rvx0, RV rvy0, RV rvu0 ):Kalman<chmat>(rvx0,rvy0,rvu0),preA(dimy+dimx+dimx,dimy+dimx),postA(dimy+dimx,dimy+dimx){};
    00144         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 );
    00145         void set_est ( const vec &mu0, const chmat &P0 ) {
    00146                 est.set_parameters ( mu0,P0 );
    00147         };
    00148
    00149
    00163         void bayes ( const vec &dt );
    00164 };
    00165
    00171 class EKFfull : public KalmanFull, public BM {
    00172
    00174         diffbifn* pfxu;
    00176         diffbifn* phxu;
    00177
    00178         enorm<fsqmat> E;
    00179 public:
    00181         EKFfull ( RV rvx, RV rvy, RV rvu );
    00183         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
    00185         void bayes ( const vec &dt );
    00187         void set_est (vec mu0, mat P0){mu=mu0;P=P0;};
    00189         const epdf& _epdf()const{return E;};
    00190         const enorm<fsqmat>* _e()const{return &E;};
    00191         const mat _R(){return P;}
    00192 };
    00193
    00199 template<class sq_T>
    00200 class EKF : public Kalman<fsqmat> {
    00202         diffbifn* pfxu;
    00204         diffbifn* phxu;
    00205 public:
    00207         EKF ( RV rvx, RV rvy, RV rvu );
    00209         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
    00211         void bayes ( const vec &dt );
    00212 };
    00213
    00220 class EKFCh : public KalmanCh {
    00221         protected:
    00223         diffbifn* pfxu;
    00225         diffbifn* phxu;
    00226 public:
    00228         EKFCh ( RV rvx, RV rvy, RV rvu );
    00230         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
    00232         void bayes ( const vec &dt );
    00233 };
    00234
    00239 class KFcondQR : public Kalman<ldmat>, public BMcond {
    00240 //protected:
    00241 public:
    00243         KFcondQR ( RV rvx, RV rvy, RV rvu, RV rvRQ ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvRQ ) {};
    00244
    00245         void condition ( const vec &RQ );
    00246 };
    00247
    00252 class KFcondR : public Kalman<ldmat>, public BMcond {
    00253 //protected:
    00254 public:
    00256         KFcondR ( RV rvx, RV rvy, RV rvu, RV rvR ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvR ) {};
    00257
    00258         void condition ( const vec &R );
    00259 };
    00260
    00262
    00263 template<class sq_T>
    00264 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ),
    00265                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
    00266                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
    00267                 Q(dimx), R(dimy),
    00268                 est ( rv ), fy ( rvy ), _yp(fy._mu()),_Ry(fy._R()), _mu(est._mu()), _P(est._R()) {
    00269
    00270         this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
    00271
    00272 // copy values in pointers
    00273         _mu = K0._mu;
    00274         _P = K0._P;
    00275         _yp = K0._yp;
    00276         _Ry = K0._Ry;
    00277
    00278 }
    00279
    00280 template<class sq_T>
    00281 Kalman<sq_T>::Kalman ( RV rvx, RV rvy0, RV rvu0 ) : BM ( rvx ),rvy ( rvy0 ),rvu ( rvu0 ),
    00282                 dimx ( rvx.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
    00283                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
    00284                 Q(dimx), R (dimy),
    00285                 est ( rvx ), fy ( rvy ),  _yp(fy._mu()),_Ry(fy._R()),_mu(est._mu()), _P(est._R()) {
    00286 };
    00287
    00288 template<class sq_T>
    00289 void Kalman<sq_T>::set_parameters ( const mat &A0,const  mat &B0, const mat &C0, const mat &D0, const sq_T &R0, const sq_T &Q0 ) {
    00290         it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
    00291         it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
    00292         it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
    00293         it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
    00294         it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
    00295         it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
    00296
    00297         A = A0;
    00298         B = B0;
    00299         C = C0;
    00300         D = D0;
    00301         R = R0;
    00302         Q = Q0;
    00303 }
    00304
    00305 template<class sq_T>
    00306 void Kalman<sq_T>::bayes ( const vec &dt ) {
    00307         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00308
    00309         sq_T iRy(dimy);
    00310         vec u = dt.get ( dimy,dimy+dimu-1 );
    00311         vec y = dt.get ( 0,dimy-1 );
    00312         //Time update
    00313         _mu = A* _mu + B*u;
    00314         //P  = A*P*A.transpose() + Q; in sq_T
    00315         _P.mult_sym ( A );
    00316         _P  +=Q;
    00317
    00318         //Data update
    00319         //_Ry = C*P*C.transpose() + R; in sq_T
    00320         _P.mult_sym ( C, _Ry );
    00321         _Ry  +=R;
    00322
    00323         mat Pfull = _P.to_mat();
    00324
    00325         _Ry.inv ( iRy ); // result is in _iRy;
    00326         _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00327
    00328         sq_T pom ( ( int ) Pfull.rows() );
    00329         iRy.mult_sym_t ( C*Pfull,pom );
    00330         (_P ) -= pom; // P = P -PC'iRy*CP;
    00331         (_yp ) = C* _mu  +D*u; //y prediction
    00332         (_mu ) += _K* ( y- _yp  );
    00333
    00334
    00335         if ( evalll==true ) { //likelihood of observation y
    00336                 ll=fy.evallog ( y );
    00337         }
    00338
    00339 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
    00340
    00341 };
    00342
    00343
    00344
    00345 //TODO why not const pointer??
    00346
    00347 template<class sq_T>
    00348 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
    00349
    00350 template<class sq_T>
    00351 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) {
    00352         pfxu = pfxu0;
    00353         phxu = phxu0;
    00354
    00355         //initialize matrices A C, later, these will be only updated!
    00356         pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
    00357 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
    00358         B.clear();
    00359         phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
    00360 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
    00361         D.clear();
    00362
    00363         R = R0;
    00364         Q = Q0;
    00365 }
    00366
    00367 template<class sq_T>
    00368 void EKF<sq_T>::bayes ( const vec &dt ) {
    00369         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00370
    00371         sq_T iRy(dimy,dimy);
    00372         vec u = dt.get ( dimy,dimy+dimu-1 );
    00373         vec y = dt.get ( 0,dimy-1 );
    00374         //Time update
    00375         _mu = pfxu->eval ( _mu, u );
    00376         pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
    00377
    00378         //P  = A*P*A.transpose() + Q; in sq_T
    00379         _P.mult_sym ( A );
    00380         _P +=Q;
    00381
    00382         //Data update
    00383         phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
    00384         //_Ry = C*P*C.transpose() + R; in sq_T
    00385         _P.mult_sym ( C, _Ry );
    00386         ( _Ry ) +=R;
    00387
    00388         mat Pfull = _P.to_mat();
    00389
    00390         _Ry.inv ( iRy ); // result is in _iRy;
    00391         _K = Pfull*C.transpose() * ( iRy.to_mat() );
    00392
    00393         sq_T pom ( ( int ) Pfull.rows() );
    00394         iRy.mult_sym_t ( C*Pfull,pom );
    00395         (_P ) -= pom; // P = P -PC'iRy*CP;
    00396         _yp = phxu->eval ( _mu,u ); //y prediction
    00397         ( _mu ) += _K* ( y-_yp );
    00398
    00399         if ( evalll==true ) {ll+=fy.evallog ( y );}
    00400 };
    00401
    00402
    00403 }
    00404 #endif // KF_H
    00405
    00406