- Main Page
- Classes
- Files
- Related Pages

# work/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
    00020
    00021 using namespace itpp;
    00022
    00027 class KalmanFull {
    00028         int dimx, dimy, dimu;
    00029         mat A, B, C, D, R, Q;
    00030
    00031         //cache
    00032         mat _Pp, _Ry, _iRy, _K;
    00033 public:
    00034         //posterior
    00036         vec mu;
    00038         mat P;
    00039
    00040 public:
    00042         KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
    00044         void bayes ( const vec &dt );
    00046         friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
    00047
    00048 };
    00049
    00050
    00058 template<class sq_T>
    00059
    00060 class Kalman : public BM {
    00061 protected:
    00063         RV rvy;
    00065         RV rvu;
    00067         int dimx;
    00069         int dimy;
    00071         int dimu;
    00073         mat A;
    00075         mat B;
    00077         mat C;
    00079         mat D;
    00081         sq_T Q;
    00083         sq_T R;
    00084
    00086         enorm<sq_T> est;
    00088         enorm<sq_T> fy;
    00089
    00091         mat _K;
    00093         vec* _yp;
    00095         sq_T* _Ry;
    00097         sq_T* _iRy;
    00099         vec* _mu;
    00101         sq_T* _P;
    00103         sq_T* _iP;
    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         epdf& _epdf() {return est;}
    00124 };
    00125
    00131 template<class sq_T>
    00132
    00133 class EKF : public Kalman<ldmat> {
    00135         diffbifn* pfxu;
    00137         diffbifn* phxu;
    00138 public:
    00140         EKF ( RV rvx, RV rvy, RV rvu );
    00142         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
    00144         void bayes ( const vec &dt );
    00145 };
    00146
    00151 class KFcondQR : public Kalman<ldmat>, public BMcond {
    00152 //protected:
    00153 public:
    00155         KFcondQR ( RV rvx, RV rvy, RV rvu, RV rvRQ ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvRQ ) {};
    00156
    00157         void condition ( const vec &RQ );
    00158 };
    00159
    00164 class KFcondR : public Kalman<ldmat>, public BMcond {
    00165 //protected:
    00166 public:
    00168         KFcondR ( RV rvx, RV rvy, RV rvu, RV rvR ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvR ) {};
    00169
    00170         void condition ( const vec &R );
    00171 };
    00172
    00174
    00175 template<class sq_T>
    00176 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ),
    00177                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
    00178                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) {
    00179
    00180         this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
    00181
    00182 //establish pointers
    00183         _mu = est._mu();
    00184         est._R ( _P,_iP );
    00185
    00186 //fy
    00187         _yp = fy._mu();
    00188         fy._R ( _Ry,_iRy );
    00189
    00190 //reset copy values in pointers
    00191         *_mu = *K0._mu;
    00192         *_P = *K0._P;
    00193         *_iP = *K0._iP;
    00194         *_yp = *K0._yp;
    00195         *_iRy = *K0._iRy;
    00196         *_Ry = *K0._Ry;
    00197
    00198 }
    00199
    00200 template<class sq_T>
    00201 Kalman<sq_T>::Kalman ( RV rvx, RV rvy0, RV rvu0 ) : BM ( rvx ),rvy ( rvy0 ),rvu ( rvu0 ),
    00202                 dimx ( rvx.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
    00203                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
    00204                 Q(dimx), R (dimy),
    00205                 est ( rvx ), fy ( rvy ) {
    00206 //assign cache
    00207 //est
    00208         _mu = est._mu();
    00209         est._R ( _P,_iP );
    00210
    00211 //fy
    00212         _yp = fy._mu();
    00213         fy._R ( _Ry,_iRy );
    00214 };
    00215
    00216 template<class sq_T>
    00217 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 ) {
    00218         it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
    00219         it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
    00220         it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
    00221         it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
    00222         it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
    00223         it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
    00224
    00225         A = A0;
    00226         B = B0;
    00227         C = C0;
    00228         D = D0;
    00229         R = R0;
    00230         Q = Q0;
    00231 }
    00232
    00233 template<class sq_T>
    00234 void Kalman<sq_T>::bayes ( const vec &dt ) {
    00235         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00236
    00237         vec u = dt.get ( dimy,dimy+dimu-1 );
    00238         vec y = dt.get ( 0,dimy-1 );
    00239         //Time update
    00240         *_mu = A* ( *_mu ) + B*u;
    00241         //P  = A*P*A.transpose() + Q; in sq_T
    00242         _P->mult_sym ( A );
    00243         ( *_P ) +=Q;
    00244
    00245         //Data update
    00246         //_Ry = C*P*C.transpose() + R; in sq_T
    00247         _P->mult_sym ( C, *_Ry );
    00248         ( *_Ry ) +=R;
    00249
    00250         mat Pfull = _P->to_mat();
    00251
    00252         _Ry->inv ( *_iRy ); // result is in _iRy;
    00253         fy._cached ( true );
    00254         _K = Pfull*C.transpose() * ( _iRy->to_mat() );
    00255
    00256         sq_T pom ( ( int ) Pfull.rows() );
    00257         _iRy->mult_sym_t ( C*Pfull,pom );
    00258         ( *_P ) -= pom; // P = P -PC'iRy*CP;
    00259         ( *_yp ) = C* ( *_mu ) +D*u; //y prediction
    00260         ( *_mu ) += _K* ( y- ( *_yp ) );
    00261
    00262
    00263         if ( evalll==true ) { //likelihood of observation y
    00264                 ll=fy.evalpdflog ( y );
    00265         }
    00266
    00267 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
    00268
    00269 };
    00270
    00271 //TODO why not const pointer??
    00272
    00273 template<class sq_T>
    00274 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<ldmat> ( rvx0,rvy0,rvu0 ) {}
    00275
    00276 template<class sq_T>
    00277 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) {
    00278         pfxu = pfxu0;
    00279         phxu = phxu0;
    00280
    00281         //initialize matrices A C, later, these will be only updated!
    00282         pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true );
    00283 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
    00284         B.clear();
    00285         phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
    00286 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
    00287         D.clear();
    00288
    00289         R = R0;
    00290         Q = Q0;
    00291 }
    00292
    00293 template<class sq_T>
    00294 void EKF<sq_T>::bayes ( const vec &dt ) {
    00295         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
    00296
    00297         vec u = dt.get ( dimy,dimy+dimu-1 );
    00298         vec y = dt.get ( 0,dimy-1 );
    00299         //Time update
    00300         *_mu = pfxu->eval ( *_mu, u );
    00301         pfxu->dfdx_cond ( *_mu,u,A,false ); //update A by a derivative of fx
    00302
    00303         //P  = A*P*A.transpose() + Q; in sq_T
    00304         _P->mult_sym ( A );
    00305         ( *_P ) +=Q;
    00306
    00307         //Data update
    00308         phxu->dfdx_cond ( *_mu,u,C,false ); //update C by a derivative hx
    00309         //_Ry = C*P*C.transpose() + R; in sq_T
    00310         _P->mult_sym ( C, *_Ry );
    00311         ( *_Ry ) +=R;
    00312
    00313         mat Pfull = _P->to_mat();
    00314
    00315         _Ry->inv ( *_iRy ); // result is in _iRy;
    00316         fy._cached ( true );
    00317         _K = Pfull*C.transpose() * ( _iRy->to_mat() );
    00318
    00319         sq_T pom ( ( int ) Pfull.rows() );
    00320         _iRy->mult_sym_t ( C*Pfull,pom );
    00321         ( *_P ) -= pom; // P = P -PC'iRy*CP;
    00322         *_yp = phxu->eval ( *_mu,u ); //y prediction
    00323         ( *_mu ) += _K* ( y-*_yp );
    00324
    00325         if ( evalll==true ) {ll+=fy.evalpdflog ( y );}
    00326 };
    00327
    00328
    00329 #endif // KF_H
    00330
    00331