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 #include "../math/chmat.h"
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 
00128 class KalmanCh : public BM{
00129 protected:
00131         RV rvy;
00133         RV rvu;
00135         int dimx;
00137         int dimy;
00139         int dimu;
00141         mat A;
00143         mat B; 
00145         mat C;
00147         mat D;
00149         chmat Q;
00151         chmat R;
00152 
00154 mat preA;
00156 mat postA;
00157 
00159         enorm<chmat> pred;
00161         enorm<chmat> fy;
00162 
00163 vec* _mu;
00164 chmat* _P;
00165 vec* _yp;
00166 chmat* _Ry;
00167 
00168 public:
00170         KalmanCh ( RV rvx0, RV rvy0, RV rvu0 );
00172         KalmanCh ( const KalmanCh &K0 );
00174         void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 );
00176         void set_pred ( const vec &mu0, const chmat &P0 ) {
00177                 pred.set_parameters ( mu0,P0 );
00178         };
00179 
00190         void bayes ( const vec &dt );
00192         epdf& _epdf() {it_warning("this is predictor, not estimator");return pred;}
00194         epdf& _pred() {return pred;}
00195 };
00196 
00202 template<class sq_T>
00203 class EKF : public Kalman<ldmat> {
00205         diffbifn* pfxu;
00207         diffbifn* phxu;
00208 public:
00210         EKF ( RV rvx, RV rvy, RV rvu );
00212         void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
00214         void bayes ( const vec &dt );
00215 };
00216 
00221 class KFcondQR : public Kalman<ldmat>, public BMcond {
00222 //protected:
00223 public:
00225         KFcondQR ( RV rvx, RV rvy, RV rvu, RV rvRQ ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvRQ ) {};
00226 
00227         void condition ( const vec &RQ );
00228 };
00229 
00234 class KFcondR : public Kalman<ldmat>, public BMcond {
00235 //protected:
00236 public:
00238         KFcondR ( RV rvx, RV rvy, RV rvu, RV rvR ) : Kalman<ldmat> ( rvx, rvy,rvu ),BMcond ( rvR ) {};
00239 
00240         void condition ( const vec &R );
00241 };
00242 
00244 
00245 template<class sq_T>
00246 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( K0.rv ),rvy ( K0.rvy ),rvu ( K0.rvu ),
00247                 dimx ( rv.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
00248                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),est ( rv ), fy ( rvy ) {
00249 
00250         this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
00251 
00252 //establish pointers
00253         _mu = est._mu();
00254         est._R ( _P,_iP );
00255 
00256 //fy
00257         _yp = fy._mu();
00258         fy._R ( _Ry,_iRy );
00259 
00260 // copy values in pointers
00261         *_mu = *K0._mu;
00262         *_P = *K0._P;
00263         *_iP = *K0._iP;
00264         *_yp = *K0._yp;
00265         *_iRy = *K0._iRy;
00266         *_Ry = *K0._Ry;
00267 
00268 }
00269 
00270 template<class sq_T>
00271 Kalman<sq_T>::Kalman ( RV rvx, RV rvy0, RV rvu0 ) : BM ( rvx ),rvy ( rvy0 ),rvu ( rvu0 ),
00272                 dimx ( rvx.count() ), dimy ( rvy.count() ),dimu ( rvu.count() ),
00273                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
00274                 Q(dimx), R (dimy),
00275                 est ( rvx ), fy ( rvy ) {
00276 //assign cache
00277 //est
00278         _mu = est._mu();
00279         est._R ( _P,_iP );
00280 
00281 //fy
00282         _yp = fy._mu();
00283         fy._R ( _Ry,_iRy );
00284 };
00285 
00286 template<class sq_T>
00287 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 ) {
00288         it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
00289         it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
00290         it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
00291         it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
00292         it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
00293         it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
00294 
00295         A = A0;
00296         B = B0;
00297         C = C0;
00298         D = D0;
00299         R = R0;
00300         Q = Q0;
00301 }
00302 
00303 template<class sq_T>
00304 void Kalman<sq_T>::bayes ( const vec &dt ) {
00305         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
00306 
00307         vec u = dt.get ( dimy,dimy+dimu-1 );
00308         vec y = dt.get ( 0,dimy-1 );
00309         //Time update
00310         *_mu = A* ( *_mu ) + B*u;
00311         //P  = A*P*A.transpose() + Q; in sq_T
00312         _P->mult_sym ( A );
00313         ( *_P ) +=Q;
00314 
00315         //Data update
00316         //_Ry = C*P*C.transpose() + R; in sq_T
00317         _P->mult_sym ( C, *_Ry );
00318         ( *_Ry ) +=R;
00319 
00320         mat Pfull = _P->to_mat();
00321 
00322         _Ry->inv ( *_iRy ); // result is in _iRy;
00323         fy._cached ( true );
00324         _K = Pfull*C.transpose() * ( _iRy->to_mat() );
00325 
00326         sq_T pom ( ( int ) Pfull.rows() );
00327         _iRy->mult_sym_t ( C*Pfull,pom );
00328         ( *_P ) -= pom; // P = P -PC'iRy*CP;
00329         ( *_yp ) = C* ( *_mu ) +D*u; //y prediction
00330         ( *_mu ) += _K* ( y- ( *_yp ) );
00331 
00332 
00333         if ( evalll==true ) { //likelihood of observation y
00334                 ll=fy.evalpdflog ( y );
00335         }
00336 
00337 //cout << "y: " << y-(*_yp) <<" R: " << _Ry->to_mat() << " iR: " << _iRy->to_mat() << " ll: " << ll <<endl;
00338 
00339 };
00340 
00341 //TODO why not const pointer??
00342 
00343 template<class sq_T>
00344 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<ldmat> ( rvx0,rvy0,rvu0 ) {}
00345 
00346 template<class sq_T>
00347 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const sq_T Q0,const sq_T R0 ) {
00348         pfxu = pfxu0;
00349         phxu = phxu0;
00350 
00351         //initialize matrices A C, later, these will be only updated!
00352         pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true );
00353 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
00354         B.clear();
00355         phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
00356 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
00357         D.clear();
00358 
00359         R = R0;
00360         Q = Q0;
00361 }
00362 
00363 template<class sq_T>
00364 void EKF<sq_T>::bayes ( const vec &dt ) {
00365         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
00366 
00367         vec u = dt.get ( dimy,dimy+dimu-1 );
00368         vec y = dt.get ( 0,dimy-1 );
00369         //Time update
00370         *_mu = pfxu->eval ( *_mu, u );
00371         pfxu->dfdx_cond ( *_mu,u,A,false ); //update A by a derivative of fx
00372 
00373         //P  = A*P*A.transpose() + Q; in sq_T
00374         _P->mult_sym ( A );
00375         ( *_P ) +=Q;
00376 
00377         //Data update
00378         phxu->dfdx_cond ( *_mu,u,C,false ); //update C by a derivative hx
00379         //_Ry = C*P*C.transpose() + R; in sq_T
00380         _P->mult_sym ( C, *_Ry );
00381         ( *_Ry ) +=R;
00382 
00383         mat Pfull = _P->to_mat();
00384 
00385         _Ry->inv ( *_iRy ); // result is in _iRy;
00386         fy._cached ( true );
00387         _K = Pfull*C.transpose() * ( _iRy->to_mat() );
00388 
00389         sq_T pom ( ( int ) Pfull.rows() );
00390         _iRy->mult_sym_t ( C*Pfull,pom );
00391         ( *_P ) -= pom; // P = P -PC'iRy*CP;
00392         *_yp = phxu->eval ( *_mu,u ); //y prediction
00393         ( *_mu ) += _K* ( y-*_yp );
00394 
00395         if ( evalll==true ) {ll+=fy.evalpdflog ( y );}
00396 };
00397 
00398 
00399 #endif // KF_H
00400 
00401 

Generated on Wed Mar 12 16:15:44 2008 for mixpp by  doxygen 1.5.3