- Main Page
- Related Pages
- Classes
- Files

- File List
- File Members

# kalman.h

Go to the documentation of this file.

    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
    00032 template<class sq_T>
    00033 class StateSpace
    00034 {
    00035         protected:
    00037                 int dimx;
    00039                 int dimy;
    00041                 int dimu;
    00043                 mat A;
    00045                 mat B;
    00047                 mat C;
    00049                 mat D;
    00051                 sq_T Q;
    00053                 sq_T R;
    00054         public:
    00055                 StateSpace() : dimx (0), dimy (0), dimu (0), A(), B(), C(), D(), Q(), R() {}
    00056                 void set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0);
    00057                 void validate();
    00059                 void from_setting (const Setting &set) {
    00060                         UI::get (A, set, "A", UI::compulsory);
    00061                         UI::get (B, set, "B", UI::compulsory);
    00062                         UI::get (C, set, "C", UI::compulsory);
    00063                         UI::get (D, set, "D", UI::compulsory);
    00064                         mat Qtm, Rtm;
    00065                         if(!UI::get(Qtm, set, "Q", UI::optional)){
    00066                                 vec dq;
    00067                                 UI::get(dq, set, "dQ", UI::compulsory);
    00068                                 Qtm=diag(dq);
    00069                         }
    00070                         if(!UI::get(Rtm, set, "R", UI::optional)){
    00071                                 vec dr;
    00072                                 UI::get(dr, set, "dQ", UI::compulsory);
    00073                                 Rtm=diag(dr);
    00074                         }
    00075                         R=Rtm; // automatic conversion
    00076                         Q=Qtm;
    00077
    00078                         validate();
    00079                 }
    00081                 int _dimx(){return dimx;}
    00083                 int _dimy(){return dimy;}
    00085                 int _dimu(){return dimu;}
    00087                 mat _A(){return A;}
    00089                 mat _B(){return B;}
    00091                 mat _C(){return C;}
    00093                 mat _D(){return D;}
    00095                 sq_T _Q(){return Q;}
    00097                 sq_T _R(){return R;}
    00098 };
    00099
    00101 template<class sq_T>
    00102 class Kalman: public BM, public StateSpace<sq_T>
    00103 {
    00104         protected:
    00106                 RV yrv;
    00108                 RV urv;
    00110                 mat  _K;
    00112                 shared_ptr<enorm<sq_T> > est;
    00114                 enorm<sq_T>  fy;
    00115         public:
    00116                 Kalman() : BM(), StateSpace<sq_T>(), yrv(),urv(), _K(),  est(new enorm<sq_T>){}
    00117                 void set_statistics (const vec &mu0, const mat &P0) {est->set_parameters (mu0, P0); };
    00118                 void set_statistics (const vec &mu0, const sq_T &P0) {est->set_parameters (mu0, P0); };
    00120                 const enorm<sq_T>& posterior() const {return *est.get();}
    00122                 shared_ptr<epdf> shared_posterior() {return est;}
    00124                 void from_setting (const Setting &set) {
    00125                         StateSpace<sq_T>::from_setting(set);
    00126
    00127                         mat P0; vec mu0;
    00128                         UI::get(mu0, set, "mu0", UI::optional);
    00129                         UI::get(P0, set,  "P0", UI::optional);
    00130                         set_statistics(mu0,P0);
    00131                         // Initial values
    00132                         UI::get (yrv, set, "yrv", UI::optional);
    00133                         UI::get (urv, set, "urv", UI::optional);
    00134                         set_drv(concat(yrv,urv));
    00135
    00136                         validate();
    00137                 }
    00138                 void validate() {
    00139                         StateSpace<sq_T>::validate();
    00140                         bdm_assert_debug(est->dimension(), "Statistics and model parameters mismatch");
    00141                 }
    00142 };
    00147 class KalmanFull : public Kalman<fsqmat>
    00148 {
    00149         public:
    00151                 KalmanFull() :Kalman<fsqmat>(){};
    00153                 void bayes (const vec &dt);
    00154 };
    00155 UIREGISTER(KalmanFull);
    00156
    00157
    00165 class KalmanCh : public Kalman<chmat>
    00166 {
    00167         protected:
    00170                 mat preA;
    00172                 mat postA;
    00174         public:
    00176                 BM* _copy_() const {
    00177                         KalmanCh* K = new KalmanCh;
    00178                         K->set_parameters (A, B, C, D, Q, R);
    00179                         K->set_statistics (est->_mu(), est->_R());
    00180                         return K;
    00181                 }
    00183                 void set_parameters (const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0);
    00185                 void initialize();
    00186
    00200                 void bayes (const vec &dt);
    00201
    00202                 void from_setting(const Setting &set){
    00203                         Kalman<chmat>::from_setting(set);
    00204                         initialize();
    00205                 }
    00206 };
    00207 UIREGISTER(KalmanCh);
    00208
    00214 class EKFfull : public KalmanFull
    00215 {
    00216         protected:
    00218                 shared_ptr<diffbifn> pfxu;
    00219
    00221                 shared_ptr<diffbifn> phxu;
    00222
    00223         public:
    00225                 EKFfull ();
    00226
    00228                 void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const mat R0);
    00229
    00231                 void bayes (const vec &dt);
    00233                 void set_statistics (const vec &mu0, const mat &P0) {
    00234                         est->set_parameters (mu0, P0);
    00235                 };
    00236                 const mat _R() {
    00237                         return est->_R().to_mat();
    00238                 }
    00239 };
    00240 UIREGISTER(EKFfull);
    00241
    00242
    00249 class EKFCh : public KalmanCh
    00250 {
    00251         protected:
    00253                 shared_ptr<diffbifn> pfxu;
    00254
    00256                 shared_ptr<diffbifn> phxu;
    00257         public:
    00259                 BM* _copy_() const {
    00260                         EKFCh* E = new EKFCh;
    00261                         E->set_parameters (pfxu, phxu, Q, R);
    00262                         E->set_statistics (est->_mu(), est->_R());
    00263                         return E;
    00264                 }
    00266                 void set_parameters (const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const chmat Q0, const chmat R0);
    00267
    00269                 void bayes (const vec &dt);
    00270
    00271                 void from_setting (const Setting &set);
    00272
    00273                 // TODO dodelat void to_setting( Setting &set ) const;
    00274
    00275 };
    00276
    00277 UIREGISTER (EKFCh);
    00278 SHAREDPTR (EKFCh);
    00279
    00280
    00282
    00290 class MultiModel: public BM
    00291 {
    00292         protected:
    00294                 Array<EKFCh*> Models;
    00296                 vec w;
    00298                 vec _lls;
    00300                 int policy;
    00302                 enorm<chmat> est;
    00303         public:
    00304                 void set_parameters (Array<EKFCh*> A, int pol0 = 1) {
    00305                         Models = A;//TODO: test if evalll is set
    00306                         w.set_length (A.length());
    00307                         _lls.set_length (A.length());
    00308                         policy = pol0;
    00309
    00310                         est.set_rv (RV ("MM", A (0)->posterior().dimension(), 0));
    00311                         est.set_parameters (A (0)->posterior().mean(), A (0)->posterior()._R());
    00312                 }
    00313                 void bayes (const vec &dt) {
    00314                         int n = Models.length();
    00315                         int i;
    00316                         for (i = 0; i < n; i++) {
    00317                                 Models (i)->bayes (dt);
    00318                                 _lls (i) = Models (i)->_ll();
    00319                         }
    00320                         double mlls = max (_lls);
    00321                         w = exp (_lls - mlls);
    00322                         w /= sum (w);   //normalization
    00323                         //set statistics
    00324                         switch (policy) {
    00325                                 case 1: {
    00326                                         int mi = max_index (w);
    00327                                         const enorm<chmat> &st = Models (mi)->posterior() ;
    00328                                         est.set_parameters (st.mean(), st._R());
    00329                                 }
    00330                                 break;
    00331                                 default:
    00332                                         bdm_error ("unknown policy");
    00333                         }
    00334                         // copy result to all models
    00335                         for (i = 0; i < n; i++) {
    00336                                 Models (i)->set_statistics (est.mean(), est._R());
    00337                         }
    00338                 }
    00340                 const enorm<chmat>& posterior() const {
    00341                         return est;
    00342                 }
    00343
    00344                 void from_setting (const Setting &set);
    00345
    00346                 // TODO dodelat void to_setting( Setting &set ) const;
    00347
    00348 };
    00349
    00350 UIREGISTER (MultiModel);
    00351 SHAREDPTR (MultiModel);
    00352
    00354
    00361 template<class sq_T>
    00362 shared_ptr<StateSpace<fsqmat> > to_state_space(const mlnorm<sq_T > &ml, ivec &theta_in_A, ivec &theta_in_C){
    00363         //get ids of yrv
    00364
    00365                         ivec yids= unique(ml._rv()._ids()); //yrv is now stored in _yrv
    00366                         ivec dids= unique(ml._rvc()._ids()); //yrv is now stored in _yrv
    00367                         ivec uids=unique_complement(dids, yids);
    00368
    00369                         RV yrv_uni = RV(yids,zeros_i(yids.length()));
    00370                         RV urv_uni = RV(uids,zeros_i(yids.length()));
    00371                         //We can do only 1d now... :(
    00372                         bdm_assert_debug(yrv_uni._dsize()==urv_uni._dsize()==1, "Only for SISO so far..." );
    00373
    00374                         RV xrv; //empty
    00375                         RV Crv; //empty
    00376                         int td=ml._rvc().mintd();
    00377                         for (int t=-1;t>=td;t--){
    00378                                 xrv.add(yrv_uni);
    00379                                 Crv.add(urv_uni);
    00380                         }
    00381
    00382                         int dimx = xrv._dsize();
    00383
    00384                         theta_in_A = ml._rv().dataind(xrv);
    00385                         theta_in_C = ml._rvc().dataind(xrv);
    00386                         // some chcek of corretness
    00387
    00388                         vec A1row = zeros(xrv._dsize());
    00389                         vec C1row = zeros(xrv._dsize());
    00390                         vec theta = ml._A().get_row(0); // this
    00391                         set_subvector( A1row, theta_in_A, theta);
    00392                         set_subvector( C1row, theta_in_C, theta);
    00393
    00394                         StateSpace<fsqmat> stsp=new StateSpace<fsqmat>();
    00395                         mat A=zeros(dimx,dimx);
    00396                         A.set_row(0,A1row);
    00397                         for (int j=1; j<dimx; j++){A(j,j-1)=1.0;} // off diagonal
    00398                         mat B=zeros(dimx,1);
    00399                         B(0) = 1.0;
    00400                         mat C=zeros(1,dimx);
    00401                         C.set_row(0,C1row);
    00402                         stsp.set_parameters(A,B,C,zeros(1,1), zeros(dimx,dimx), ml._R());
    00403                         return stsp;
    00404 }
    00405
    00407
    00408 template<class sq_T>
    00409 void StateSpace<sq_T>::set_parameters (const mat &A0, const  mat &B0, const  mat &C0, const  mat &D0, const  sq_T &Q0, const sq_T &R0)
    00410 {
    00411         dimx = A0.rows();
    00412         dimu = B0.cols();
    00413         dimy = C0.rows();
    00414
    00415         A = A0;
    00416         B = B0;
    00417         C = C0;
    00418         D = D0;
    00419         R = R0;
    00420         Q = Q0;
    00421         validate();
    00422 }
    00423
    00424 template<class sq_T>
    00425 void StateSpace<sq_T>::validate(){
    00426         bdm_assert_debug (A.cols() == dimx, "KalmanFull: A is not square");
    00427         bdm_assert_debug (B.rows() == dimx, "KalmanFull: B is not compatible");
    00428         bdm_assert_debug (C.cols() == dimx, "KalmanFull: C is not square");
    00429         bdm_assert_debug ( (D.rows() == dimy) || (D.cols() == dimu), "KalmanFull: D is not compatible");
    00430         bdm_assert_debug ( (Q.cols() == dimx) || (Q.rows() == dimx), "KalmanFull: Q is not compatible");
    00431         bdm_assert_debug ( (R.cols() == dimy) || (R.rows() == dimy), "KalmanFull: R is not compatible");
    00432 }
    00433
    00434 }
    00435 #endif // KF_H
    00436
    00437