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 {
00024
00029 class KalmanFull
00030 {
00031 protected:
00032 int dimx, dimy, dimu;
00033 mat A, B, C, D, R, Q;
00034
00035
00036 mat _Pp, _Ry, _iRy, _K;
00037 public:
00038
00040 vec mu;
00042 mat P;
00043
00044 bool evalll;
00045 double ll;
00046 public:
00048 KalmanFull ( mat A, mat B, mat C, mat D, mat R, mat Q, mat P0, vec mu0 );
00050 void bayes ( const vec &dt );
00052 friend std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf );
00054 KalmanFull() {};
00055 };
00056
00057
00065 template<class sq_T>
00066
00067 class Kalman : public BM
00068 {
00069 protected:
00071 RV rvy;
00073 RV rvu;
00075 int dimx;
00077 int dimy;
00079 int dimu;
00081 mat A;
00083 mat B;
00085 mat C;
00087 mat D;
00089 sq_T Q;
00091 sq_T R;
00092
00094 enorm<sq_T> est;
00096 enorm<sq_T> fy;
00097
00099 mat _K;
00101 vec& _yp;
00103 sq_T& _Ry;
00105 vec& _mu;
00107 sq_T& _P;
00108
00109 public:
00111 Kalman ( );
00113 Kalman ( const Kalman<sq_T> &K0 );
00115 void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const sq_T &Q0,const sq_T &R0 );
00117 void set_est ( const vec &mu0, const sq_T &P0 )
00118 {
00119 sq_T pom ( dimy );
00120 est.set_parameters ( mu0,P0 );
00121 P0.mult_sym ( C,pom );
00122 fy.set_parameters ( C*mu0, pom );
00123 };
00124
00126 void bayes ( const vec &dt );
00128 const epdf& posterior() const {return est;}
00129 const enorm<sq_T>* _e() const {return &est;}
00131 mat& __K() {return _K;}
00133 vec _dP() {return _P->getD();}
00134 };
00135
00142 class KalmanCh : public Kalman<chmat>
00143 {
00144 protected:
00146 mat preA;
00148 mat postA;
00149
00150 public:
00152 BM* _copy_() const
00153 {
00154 KalmanCh* K=new KalmanCh;
00155 K->set_parameters ( A,B,C,D,Q,R );
00156 K->set_statistics ( _mu,_P );
00157 return K;
00158 }
00160 void set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &Q0,const chmat &R0 );
00161 void set_statistics ( const vec &mu0, const chmat &P0 )
00162 {
00163 est.set_parameters ( mu0,P0 );
00164 };
00165
00166
00180 void bayes ( const vec &dt );
00181 };
00182
00188 class EKFfull : public KalmanFull, public BM
00189 {
00190 protected:
00192 diffbifn* pfxu;
00194 diffbifn* phxu;
00195
00196 enorm<fsqmat> E;
00197 public:
00199 EKFfull ( );
00201 void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const mat Q0, const mat R0 );
00203 void bayes ( const vec &dt );
00205 void set_statistics ( vec mu0, mat P0 ) {mu=mu0;P=P0;};
00207 const epdf& posterior() const{return E;};
00208 const enorm<fsqmat>* _e() const{return &E;};
00209 const mat _R() {return P;}
00210 };
00211
00217 template<class sq_T>
00218 class EKF : public Kalman<fsqmat>
00219 {
00221 diffbifn* pfxu;
00223 diffbifn* phxu;
00224 public:
00226 EKF ( RV rvx, RV rvy, RV rvu );
00228 EKF<sq_T>* _copy_() const { return new EKF<sq_T> ( this ); }
00230 void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const sq_T Q0, const sq_T R0 );
00232 void bayes ( const vec &dt );
00233 };
00234
00241 class EKFCh : public KalmanCh
00242 {
00243 protected:
00245 diffbifn* pfxu;
00247 diffbifn* phxu;
00248 public:
00250 BM* _copy_() const
00251 {
00252 EKFCh* E=new EKFCh;
00253 E->set_parameters ( pfxu,phxu,Q,R );
00254 E->set_statistics ( _mu,_P );
00255 return E;
00256 }
00258 void set_parameters ( diffbifn* pfxu, diffbifn* phxu, const chmat Q0, const chmat R0 );
00260 void bayes ( const vec &dt );
00261
00262 void from_setting( const Setting &set );
00263
00264
00265
00266 };
00267
00268 UIREGISTER(EKFCh);
00269
00270
00275 class KFcondQR : public Kalman<ldmat>
00276 {
00277
00278 public:
00279 void condition ( const vec &QR )
00280 {
00281 it_assert_debug ( QR.length() == ( dimx+dimy ),"KFcondRQ: conditioning by incompatible vector" );
00282
00283 Q.setD ( QR ( 0, dimx-1 ) );
00284 R.setD ( QR ( dimx, -1 ) );
00285 };
00286 };
00287
00292 class KFcondR : public Kalman<ldmat>
00293 {
00294
00295 public:
00297 KFcondR ( ) : Kalman<ldmat> ( ) {};
00298
00299 void condition ( const vec &R0 )
00300 {
00301 it_assert_debug ( R0.length() == ( dimy ),"KFcondR: conditioning by incompatible vector" );
00302
00303 R.setD ( R0 );
00304 };
00305
00306 };
00307
00309
00310 template<class sq_T>
00311 Kalman<sq_T>::Kalman ( const Kalman<sq_T> &K0 ) : BM ( ),rvy ( K0.rvy ),rvu ( K0.rvu ),
00312 dimx ( K0.dimx ), dimy ( K0.dimy ),dimu ( K0.dimu ),
00313 A ( K0.A ), B ( K0.B ), C ( K0.C ), D ( K0.D ),
00314 Q ( K0.Q ), R ( K0.R ),
00315 est ( K0.est ), fy ( K0.fy ), _yp ( fy._mu() ),_Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
00316 {
00317
00318
00319
00320
00321
00322
00323
00324 }
00325
00326 template<class sq_T>
00327 Kalman<sq_T>::Kalman ( ) : BM (), est ( ), fy (), _yp ( fy._mu() ), _Ry ( fy._R() ), _mu ( est._mu() ), _P ( est._R() )
00328 {
00329 };
00330
00331 template<class sq_T>
00332 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 )
00333 {
00334 dimx = A0.rows();
00335 dimy = C0.rows();
00336 dimu = B0.cols();
00337
00338 it_assert_debug ( A0.cols() ==dimx, "Kalman: A is not square" );
00339 it_assert_debug ( B0.rows() ==dimx, "Kalman: B is not compatible" );
00340 it_assert_debug ( C0.cols() ==dimx, "Kalman: C is not square" );
00341 it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "Kalman: D is not compatible" );
00342 it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "Kalman: R is not compatible" );
00343 it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "Kalman: Q is not compatible" );
00344
00345 A = A0;
00346 B = B0;
00347 C = C0;
00348 D = D0;
00349 R = R0;
00350 Q = Q0;
00351 }
00352
00353 template<class sq_T>
00354 void Kalman<sq_T>::bayes ( const vec &dt )
00355 {
00356 it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
00357
00358 sq_T iRy ( dimy );
00359 vec u = dt.get ( dimy,dimy+dimu-1 );
00360 vec y = dt.get ( 0,dimy-1 );
00361
00362 _mu = A* _mu + B*u;
00363
00364 _P.mult_sym ( A );
00365 _P +=Q;
00366
00367
00368
00369 _P.mult_sym ( C, _Ry );
00370 _Ry +=R;
00371
00372 mat Pfull = _P.to_mat();
00373
00374 _Ry.inv ( iRy );
00375 _K = Pfull*C.transpose() * ( iRy.to_mat() );
00376
00377 sq_T pom ( ( int ) Pfull.rows() );
00378 iRy.mult_sym_t ( C*Pfull,pom );
00379 ( _P ) -= pom;
00380 ( _yp ) = C* _mu +D*u;
00381 ( _mu ) += _K* ( y- _yp );
00382
00383
00384 if ( evalll==true )
00385 {
00386 ll=fy.evallog ( y );
00387 }
00388
00389
00390
00391 };
00392
00400 class MultiModel: public BM
00401 {
00402 protected:
00404 Array<EKFCh*> Models;
00406 vec w;
00408 vec _lls;
00410 int policy;
00412 enorm<chmat> est;
00413 public:
00414 void set_parameters ( Array<EKFCh*> A, int pol0=1 )
00415 {
00416 Models=A;
00417 w.set_length ( A.length() );
00418 _lls.set_length ( A.length() );
00419 policy=pol0;
00420
00421 est.set_rv(RV("MM",A(0)->posterior().dimension(),0));
00422 est.set_parameters(A(0)->_e()->mean(), A(0)->_e()->_R());
00423 }
00424 void bayes ( const vec &dt )
00425 {
00426 int n = Models.length();
00427 int i;
00428 for ( i=0;i<n;i++ )
00429 {
00430 Models ( i )->bayes ( dt );
00431 _lls ( i ) = Models ( i )->_ll();
00432 }
00433 double mlls=max ( _lls );
00434 w=exp ( _lls-mlls );
00435 w/=sum ( w );
00436
00437 switch ( policy )
00438 {
00439 case 1:
00440 {
00441 int mi=max_index ( w );
00442 const enorm<chmat>* st=(Models(mi)->_e());
00443 est.set_parameters(st->mean(), st->_R());
00444 }
00445 break;
00446 default: it_error ( "unknown policy" );
00447 }
00448
00449 for ( i=0;i<n;i++ )
00450 {
00451 Models ( i )->set_statistics ( est.mean(),est._R());
00452 }
00453 }
00454
00455 const enorm<chmat>* _e() const {return &est;}
00456
00457 const enorm<chmat>& posterior() const {return est;}
00458
00459 void from_setting( const Setting &set );
00460
00461
00462
00463 };
00464
00465 UIREGISTER(MultiModel);
00466
00467
00468
00469
00470
00471 template<class sq_T>
00472 EKF<sq_T>::EKF ( RV rvx0, RV rvy0, RV rvu0 ) : Kalman<sq_T> ( rvx0,rvy0,rvu0 ) {}
00473
00474 template<class sq_T>
00475 void EKF<sq_T>::set_parameters ( diffbifn* pfxu0, diffbifn* phxu0,const sq_T Q0,const sq_T R0 )
00476 {
00477 pfxu = pfxu0;
00478 phxu = phxu0;
00479
00480
00481 pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
00482
00483 B.clear();
00484 phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
00485
00486 D.clear();
00487
00488 R = R0;
00489 Q = Q0;
00490 }
00491
00492 template<class sq_T>
00493 void EKF<sq_T>::bayes ( const vec &dt )
00494 {
00495 it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
00496
00497 sq_T iRy ( dimy,dimy );
00498 vec u = dt.get ( dimy,dimy+dimu-1 );
00499 vec y = dt.get ( 0,dimy-1 );
00500
00501 _mu = pfxu->eval ( _mu, u );
00502 pfxu->dfdx_cond ( _mu,u,A,false );
00503
00504
00505 _P.mult_sym ( A );
00506 _P +=Q;
00507
00508
00509 phxu->dfdx_cond ( _mu,u,C,false );
00510
00511 _P.mult_sym ( C, _Ry );
00512 ( _Ry ) +=R;
00513
00514 mat Pfull = _P.to_mat();
00515
00516 _Ry.inv ( iRy );
00517 _K = Pfull*C.transpose() * ( iRy.to_mat() );
00518
00519 sq_T pom ( ( int ) Pfull.rows() );
00520 iRy.mult_sym_t ( C*Pfull,pom );
00521 ( _P ) -= pom;
00522 _yp = phxu->eval ( _mu,u );
00523 ( _mu ) += _K* ( y-_yp );
00524
00525 if ( evalll==true ) {ll+=fy.evallog ( y );}
00526 };
00527
00528
00529 }
00530 #endif // KF_H
00531
00532
