root/library/bdm/estim/particles.h @ 887

Revision 887, 17.0 kB (checked in by smidl, 14 years ago)

new base for particle filtering

  • Property svn:eol-style set to native
Line 
1/*!
2  \file
3  \brief Bayesian Filtering using stochastic sampling (Particle Filters)
4  \author Vaclav Smidl.
5
6  -----------------------------------
7  BDM++ - C++ library for Bayesian Decision Making under Uncertainty
8
9  Using IT++ for numerical operations
10  -----------------------------------
11*/
12
13#ifndef PARTICLES_H
14#define PARTICLES_H
15
16
17#include "../estim/arx_ext.h"
18#include "../stat/emix.h"
19
20namespace bdm {
21       
22//! class used in PF
23class MarginalizedParticle : public BM{
24        protected:
25        //! discrte particle
26        dirac est_emp;
27        //! internal Bayes Model
28        shared_ptr<BM> bm; 
29        //! pdf with for transitional par
30        shared_ptr<pdf> par; // pdf for non-linear part
31        //! link from this to bm
32        shared_ptr<datalink_part> cond2bm;
33        //! link from cond to par
34        shared_ptr<datalink_part> cond2par;
35        //! link from emp 2 par
36        shared_ptr<datalink_part> emp2bm;
37        //! link from emp 2 par
38        shared_ptr<datalink_part> emp2par;
39       
40        //! custom posterior - product
41        class eprod_2:public eprod_base {
42                protected:
43                MarginalizedParticle &mp;
44                public:
45                eprod_2(MarginalizedParticle &m):mp(m){}
46                const epdf* factor(int i) const {return (i==0) ? &mp.bm->posterior() : &mp.est_emp;}
47                const int no_factors() const {return 2;}
48        } est;
49       
50        public:
51                MarginalizedParticle():est(*this){};
52                BM* _copy() const{return new MarginalizedParticle(*this);};
53                void bayes(const vec &dt, const vec &cond){
54                        vec par_cond(par->dimensionc());
55                        cond2par->filldown(cond,par_cond); // copy ut
56                        emp2par->filldown(est_emp._point(),par_cond); // copy xt-1
57                       
58                        //sample new particle
59                        est_emp.set_point(par->samplecond(par_cond));
60                        //if (evalll)
61                        vec bm_cond(bm->dimensionc());
62                        cond2bm->filldown(cond, bm_cond);// set e.g. ut
63                        emp2bm->filldown(est_emp._point(), bm_cond);// set e.g. ut
64                        bm->bayes(dt, bm_cond);
65                        ll=bm->_ll();
66                }
67                const eprod_2& posterior() const {return est;}
68       
69                void set_prior(const epdf *pdf0){
70                        const eprod *ep=dynamic_cast<const eprod*>(pdf0);
71                        if (ep){ // full prior
72                                bdm_assert(ep->no_factors()==2,"Incompatible prod");
73                                bm->set_prior(ep->factor(0));
74                                est_emp.set_point(ep->factor(1)->sample());
75                        } else {
76                                // assume prior is only for emp;
77                                est_emp.set_point(pdf0->sample());
78                        }
79                }
80
81                /*! parse structure
82                \code
83                class = "BootstrapParticle";
84                parameter_pdf = {class = 'epdf_offspring', ...};
85                observation_pdf = {class = 'epdf_offspring',...};
86                \endcode
87                If rvs are set, then it checks for compatibility.
88                */
89                void from_setting(const Setting &set){
90                        BM::from_setting ( set );
91                        par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );
92                        bm = UI::build<BM> ( set, "bm", UI::compulsory );
93                }
94                void validate(){
95                        yrv = bm->_rv();
96                        dimy = bm->dimension();
97                        set_rv( par->_rv());
98                        set_dim( par->dimension());
99
100                        rvc = par->_rvc().subt(par->_rv().copy_t(-1));
101                        rvc.add(bm->_rvc()); //
102                       
103                        cond2bm=new datalink_part;
104                        cond2par=new datalink_part;
105                        emp2bm  =new datalink_part;
106                        emp2par =new datalink_part;
107                        cond2bm->set_connection(bm->_rvc(), rvc);
108                        cond2par->set_connection(par->_rvc(), rvc);
109                        emp2bm->set_connection(bm->_rvc(), _rv());
110                        emp2par->set_connection(par->_rvc(), _rv().copy_t(-1));
111                       
112                        dimc = rvc._dsize();
113                };
114};
115UIREGISTER(MarginalizedParticle);
116
117//! class used in PF
118class BootstrapParticle : public BM{
119        dirac est;
120        shared_ptr<pdf> par;
121        shared_ptr<pdf> obs;
122        shared_ptr<datalink_part> cond2par;
123        shared_ptr<datalink_part> cond2obs;
124        shared_ptr<datalink_part> xt2obs;
125        shared_ptr<datalink_part> xtm2par;
126        public:
127                BM* _copy() const{return new BootstrapParticle(*this);};
128                void bayes(const vec &dt, const vec &cond){
129                        vec par_cond(par->dimensionc());
130                        cond2par->filldown(cond,par_cond); // copy ut
131                        xtm2par->filldown(est._point(),par_cond); // copy xt-1
132                       
133                        //sample new particle
134                        est.set_point(par->samplecond(par_cond));
135                        //if (evalll)
136                        vec obs_cond(obs->dimensionc());
137                        cond2obs->filldown(cond, obs_cond);// set e.g. ut
138                        xt2obs->filldown(est._point(), obs_cond);// set e.g. ut
139                        ll=obs->evallogcond(dt,obs_cond);
140                }
141                const dirac& posterior() const {return est;}
142               
143                void set_prior(const epdf *pdf0){est.set_point(pdf0->sample());}
144               
145                /*! parse structure
146                \code
147                class = "BootstrapParticle";
148                parameter_pdf = {class = 'epdf_offspring', ...};
149                observation_pdf = {class = 'epdf_offspring',...};
150                \endcode
151                If rvs are set, then it checks for compatibility.
152                */
153                void from_setting(const Setting &set){
154                        BM::from_setting ( set );
155                        par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );
156                        obs = UI::build<pdf> ( set, "observation_pdf", UI::compulsory );
157                }
158                void validate(){
159                        yrv = obs->_rv();
160                        dimy = obs->dimension();
161                        set_rv( par->_rv());
162                        set_dim( par->dimension());
163                       
164                        rvc = par->_rvc().subt(par->_rv().copy_t(-1));
165                        rvc.add(obs->_rvc()); //
166                       
167                        cond2obs=new datalink_part;
168                        cond2par=new datalink_part;
169                        xt2obs  =new datalink_part;
170                        xtm2par =new datalink_part;
171                        cond2obs->set_connection(obs->_rvc(), rvc);
172                        cond2par->set_connection(par->_rvc(), rvc);
173                        xt2obs->set_connection(obs->_rvc(), _rv());
174                        xtm2par->set_connection(par->_rvc(), _rv().copy_t(-1));
175                       
176                        dimc = rvc._dsize();
177                };
178};
179UIREGISTER(BootstrapParticle);
180
181
182/*!
183* \brief Trivial particle filter with proposal density equal to parameter evolution model.
184
185Posterior density is represented by a weighted empirical density (\c eEmp ).
186*/
187
188class PF : public BM {
189        //! \var log_level_enums weights
190        //! all weightes will be logged
191
192        //! \var log_level_enums samples
193        //! all samples will be logged
194        LOG_LEVEL(PF,weights,samples);
195       
196        class pf_mix: public emix_base{
197                Array<BM*> &bms;
198                public:
199                        pf_mix(vec &w0, Array<BM*> &bms0):emix_base(w0),bms(bms0){}
200                        const epdf* component(const int &i)const{return &(bms(i)->posterior());}
201                        int no_coms() const {return bms.length();}
202        };
203protected:
204        //!number of particles;
205        int n;
206        //!posterior density
207        pf_mix est;
208        //! weights;
209        vec w;
210        //! particles
211        Array<BM*> particles;
212        //! internal structure storing loglikelihood of predictions
213        vec lls;
214
215        //! which resampling method will be used
216        RESAMPLING_METHOD resmethod;
217        //! resampling threshold; in this case its meaning is minimum ratio of active particles
218        //! For example, for 0.5 resampling is performed when the numebr of active aprticles drops belo 50%.
219        double res_threshold;
220
221        //! \name Options
222        //!@{
223        //!@}
224
225public:
226        //! \name Constructors
227        //!@{
228        PF ( ) : est(w,particles) { };
229
230        void set_parameters ( int n0, double res_th0 = 0.5, RESAMPLING_METHOD rm = SYSTEMATIC ) {
231                n = n0;
232                res_threshold = res_th0;
233                resmethod = rm;
234        };
235        void set_model ( const BM *particle0, const epdf *prior) {
236                if (n>0){
237                        particles.set_length(n);
238                        for (int i=0; i<n;i++){
239                                particles(i) = particle0->_copy();
240                                particles(i)->set_prior(prior);
241                        }
242                }
243                // set values for posterior
244                est.set_rv ( particle0->posterior()._rv() );
245        };
246        void set_statistics ( const vec w0, const epdf &epdf0 ) {
247                //est.set_statistics ( w0, epdf0 );
248        };
249/*      void set_statistics ( const eEmp &epdf0 ) {
250                bdm_assert_debug ( epdf0._rv().equal ( par->_rv() ), "Incompatible input" );
251                est = epdf0;
252        };*/
253        //!@}
254
255        //! bayes compute weights of the
256        virtual void bayes_weights();
257        //! important part of particle filtering - decide if it is time to perform resampling
258        virtual bool do_resampling() {
259                double eff = 1.0 / ( w * w );
260                return eff < ( res_threshold*n );
261        }
262        void bayes ( const vec &yt, const vec &cond );
263        //!access function
264        vec& _lls() {
265                return lls;
266        }
267        //!access function
268        RESAMPLING_METHOD _resmethod() const {
269                return resmethod;
270        }
271        //! return correctly typed posterior (covariant return)
272        const pf_mix& posterior() const {
273                return est;
274        }
275
276        /*! configuration structure for basic PF
277        \code
278        parameter_pdf   = pdf_class;         // parameter evolution pdf
279        observation_pdf = pdf_class;         // observation pdf
280        prior           = epdf_class;         // prior probability density
281        --- optional ---
282        n               = 10;                 // number of particles
283        resmethod       = 'systematic', or 'multinomial', or 'stratified'
284                                                                                  // resampling method
285        res_threshold   = 0.5;                // resample when active particles drop below 50%
286        \endcode
287        */
288        void from_setting ( const Setting &set ) {
289                BM::from_setting ( set );
290               
291                shared_ptr<BM> bm0 = UI::build<BM>(set, "particle",UI::compulsory);
292               
293                shared_ptr<epdf> pri = UI::build<epdf> ( set, "prior", UI::compulsory );
294                n =0;
295                UI::get(n,set,"n",UI::optional);;
296                if (n>0){
297                        particles.set_length(n);
298                        for(int i=0;i<n;i++){particles(i)=bm0->_copy();}
299                        w = ones(n)/n;
300                }
301                set_prior(pri.get());
302                // set resampling method
303                resmethod_from_set ( set );
304                //set drv
305
306                set_yrv ( bm0->_rv() );
307                rvc = bm0->_rvc();
308                BM::set_rv(bm0->_rv());
309                yrv=bm0->_yrv();
310        }
311       
312        void set_prior(const epdf *pri){
313                const emix_base *emi=dynamic_cast<const emix_base*>(pri);
314                if (emi) {
315                        bdm_assert(particles.length()>0, "initial particle is not assigned");
316                        n = emi->_w().length();
317                        int old_n = particles.length();
318                        if (n!=old_n){
319                                particles.set_length(n,true);
320                        } 
321                        for(int i=old_n;i<n;i++){particles(i)=particles(0)->_copy();}
322                       
323                        for (int i =0; i<n; i++){
324                                particles(i)->set_prior(emi->_com(i));
325                        }
326                } else {
327                        // try to find "n"
328                        bdm_assert(n>0, "Field 'n' must be filled when prior is not of type emix");
329                        for (int i =0; i<n; i++){
330                                particles(i)->set_prior(pri);
331                        }
332                       
333                }
334        }
335        //! auxiliary function reading parameter 'resmethod' from configuration file
336        void resmethod_from_set ( const Setting &set ) {
337                string resmeth;
338                if ( UI::get ( resmeth, set, "resmethod", UI::optional ) ) {
339                        if ( resmeth == "systematic" ) {
340                                resmethod = SYSTEMATIC;
341                        } else  {
342                                if ( resmeth == "multinomial" ) {
343                                        resmethod = MULTINOMIAL;
344                                } else {
345                                        if ( resmeth == "stratified" ) {
346                                                resmethod = STRATIFIED;
347                                        } else {
348                                                bdm_error ( "Unknown resampling method" );
349                                        }
350                                }
351                        }
352                } else {
353                        resmethod = SYSTEMATIC;
354                };
355                if ( !UI::get ( res_threshold, set, "res_threshold", UI::optional ) ) {
356                        res_threshold = 0.9;
357                }
358                //validate();
359        }
360
361        void validate() {
362                BM::validate();
363                est.validate();
364                bdm_assert ( n>0, "empty particle pool" );
365                n = w.length();
366                lls = zeros ( n );
367
368                if ( particles(0)->_rv()._dsize() > 0 ) {
369                        bdm_assert (  particles(0)->_rv()._dsize() == est.dimension(), "Mismatch of RV and dimension of posterior" );
370                }
371        }
372        //! resample posterior density (from outside - see MPF)
373        void resample ( ) {
374                ivec ind = zeros_i ( n );
375                bdm::resample(w,ind,resmethod);
376                // copy the internals according to ind
377                for (int i = 0; i < n; i++ ) {
378                        if ( ind ( i ) != i ) {
379                                particles( i ) = particles( ind ( i ) )->_copy();
380                        }
381                        w ( i ) = 1.0 / n;
382                }
383        }
384        //! access function
385        Array<BM*>& _particles() {
386                return particles;
387        }
388
389};
390UIREGISTER ( PF );
391
392/*!
393\brief Marginalized Particle filter
394
395A composition of particle filter with exact (or almost exact) bayesian models (BMs).
396The Bayesian models provide marginalized predictive density. Internaly this is achieved by virtual class MPFpdf.
397*/
398
399// class MPF : public BM  {
400//      //! Introduces new option
401//      //! \li means - meaning TODO
402//      LOG_LEVEL(MPF,means);
403// protected:
404//      //! particle filter on non-linear variable
405//      shared_ptr<PF> pf;
406//      //! Array of Bayesian models
407//      Array<BM*> BMs;
408//
409//      //! internal class for pdf providing composition of eEmp with external components
410//
411//      class mpfepdf : public epdf  {
412//              //! pointer to particle filter
413//              shared_ptr<PF> &pf;
414//              //! pointer to Array of BMs
415//              Array<BM*> &BMs;
416//      public:
417//              //! constructor
418//              mpfepdf ( shared_ptr<PF> &pf0, Array<BM*> &BMs0 ) : epdf(), pf ( pf0 ), BMs ( BMs0 ) { };
419//              //! a variant of set parameters - this time, parameters are read from BMs and pf
420//              void read_parameters() {
421//                      rv = concat ( pf->posterior()._rv(), BMs ( 0 )->posterior()._rv() );
422//                      dim = pf->posterior().dimension() + BMs ( 0 )->posterior().dimension();
423//                      bdm_assert_debug ( dim == rv._dsize(), "Wrong name " );
424//              }
425//              vec mean() const;
426//
427//              vec variance() const;
428//
429//              void qbounds ( vec &lb, vec &ub, double perc = 0.95 ) const;
430//
431//              vec sample() const NOT_IMPLEMENTED(0);
432//
433//              double evallog ( const vec &val ) const NOT_IMPLEMENTED(0);             
434//      };
435//
436//      //! Density joining PF.est with conditional parts
437//      mpfepdf jest;
438//
439//      //! datalink from global yt and cond (Up) to BMs yt and cond (Down)
440//      datalink_m2m this2bm;
441//      //! datalink from global yt and cond (Up) to PFs yt and cond (Down)
442//      datalink_m2m this2pf;
443//      //!datalink from PF part to BM
444//      datalink_part pf2bm;
445//
446// public:
447//      //! Default constructor.
448//      MPF () :  jest ( pf, BMs ) {};
449//      //! set all parameters at once
450//      void set_pf ( shared_ptr<pdf> par0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
451//              if (!pf) pf=new PF;
452//              pf->set_model ( par0, par0 ); // <=== nasty!!!
453//              pf->set_parameters ( n0, rm );
454//              pf->set_rv(par0->_rv());
455//              BMs.set_length ( n0 );
456//      }
457//      //! set a prototype of BM, copy it to as many times as there is particles in pf
458//      void set_BM ( const BM &BMcond0 ) {
459//
460//              int n = pf->__w().length();
461//              BMs.set_length ( n );
462//              // copy
463//              //BMcond0 .condition ( pf->posterior()._sample ( 0 ) );
464//              for ( int i = 0; i < n; i++ ) {
465//                      BMs ( i ) = (BM*) BMcond0._copy();
466//              }
467//      };
468//
469//      void bayes ( const vec &yt, const vec &cond );
470//
471//      const epdf& posterior() const {
472//              return jest;
473//      }
474//
475//      //!Access function
476//      const BM* _BM ( int i ) {
477//              return BMs ( i );
478//      }
479//      PF& _pf() {return *pf;}
480//
481//
482//      virtual double logpred ( const vec &yt ) const NOT_IMPLEMENTED(0);
483//             
484//      virtual epdf* epredictor() const NOT_IMPLEMENTED(NULL);
485//     
486//      virtual pdf* predictor() const NOT_IMPLEMENTED(NULL);
487//
488//
489//      /*! configuration structure for basic PF
490//      \code
491//      BM              = BM_class;           // Bayesian filtr for analytical part of the model
492//      parameter_pdf   = pdf_class;         // transitional pdf for non-parametric part of the model
493//      prior           = epdf_class;         // prior probability density
494//      --- optional ---
495//      n               = 10;                 // number of particles
496//      resmethod       = 'systematic', or 'multinomial', or 'stratified'
497//                                                                                // resampling method
498//      \endcode
499//      */
500//      void from_setting ( const Setting &set ) {
501//              BM::from_setting( set );
502//
503//              shared_ptr<pdf> par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );
504//
505//              pf = new PF;
506//              // prior must be set before BM
507//              pf->prior_from_set ( set );
508//              pf->resmethod_from_set ( set );
509//              pf->set_model ( par, par ); // too hackish!
510//
511//              shared_ptr<BM> BM0 = UI::build<BM> ( set, "BM", UI::compulsory );
512//              set_BM ( *BM0 );
513//
514//              //set drv
515//              //??set_yrv(concat(BM0->_yrv(),u) );
516//              set_yrv ( BM0->_yrv() );
517//              rvc = BM0->_rvc().subt ( par->_rv() );
518//              //find potential input - what remains in rvc when we subtract rv
519//              RV u = par->_rvc().subt ( par->_rv().copy_t ( -1 ) );
520//              rvc.add ( u );
521//              dimc = rvc._dsize();
522//              validate();
523//      }
524//
525//      void validate() {
526//              BM::validate();
527//              try {
528//                      pf->validate();
529//              } catch ( std::exception ) {
530//                      throw UIException ( "Error in PF part of MPF:" );
531//              }
532//              jest.read_parameters();
533//              this2bm.set_connection ( BMs ( 0 )->_yrv(), BMs ( 0 )->_rvc(), yrv, rvc );
534//              this2pf.set_connection ( pf->_yrv(), pf->_rvc(), yrv, rvc );
535//              pf2bm.set_connection ( BMs ( 0 )->_rvc(), pf->posterior()._rv() );
536//      }
537// };
538// UIREGISTER ( MPF );
539
540/*! ARXg for estimation of state-space variances
541*/
542// class MPF_ARXg :public BM{
543//      protected:
544//      shared_ptr<PF> pf;
545//      //! pointer to Array of BMs
546//      Array<ARX*> BMso;
547//      //! pointer to Array of BMs
548//      Array<ARX*> BMsp;
549//      //!parameter evolution
550//      shared_ptr<fnc> g;
551//      //!observation function
552//      shared_ptr<fnc> h;
553//     
554//      public:
555//              void bayes(const vec &yt, const vec &cond );
556//              void from_setting(const Setting &set) ;
557//              void validate() {
558//                      bdm_assert(g->dimensionc()==g->dimension(),"not supported yet");
559//                      bdm_assert(h->dimensionc()==g->dimension(),"not supported yet");                       
560//              }
561//
562//              double logpred(const vec &cond) const NOT_IMPLEMENTED(0.0);
563//              epdf* epredictor() const NOT_IMPLEMENTED(NULL);
564//              pdf* predictor() const NOT_IMPLEMENTED(NULL);
565//              const epdf& posterior() const {return pf->posterior();};
566//             
567//              void log_register( logger &L, const string &prefix ){
568//                      BM::log_register(L,prefix);
569//                      logrec->ids.set_size ( 3 );
570//                      logrec->ids(1)= L.add_vector(RV("Q",dimension()*dimension()), prefix+L.prefix_sep()+"Q");
571//                      logrec->ids(2)= L.add_vector(RV("R",dimensiony()*dimensiony()), prefix+L.prefix_sep()+"R");
572//                     
573//              };
574//              void log_write() const {
575//                      BM::log_write();
576//                      mat mQ=zeros(dimension(),dimension());
577//                      mat pom=zeros(dimension(),dimension());
578//                      mat mR=zeros(dimensiony(),dimensiony());
579//                      mat pom2=zeros(dimensiony(),dimensiony());
580//                      mat dum;
581//                      const vec w=pf->posterior()._w();
582//                      for (int i=0; i<w.length(); i++){
583//                              BMsp(i)->posterior().mean_mat(dum,pom);
584//                              mQ += w(i) * pom;
585//                              BMso(i)->posterior().mean_mat(dum,pom2);
586//                              mR += w(i) * pom2;
587//                             
588//                      }
589//                      logrec->L.log_vector ( logrec->ids ( 1 ), cvectorize(mQ) );
590//                      logrec->L.log_vector ( logrec->ids ( 2 ), cvectorize(mR) );
591//                     
592//              }
593// };
594// UIREGISTER(MPF_ARXg);
595
596
597}
598#endif // KF_H
599
Note: See TracBrowser for help on using the browser.