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

/*!
  \file
  \brief Bayesian Filtering using stochastic sampling (Particle Filters)
  \author Vaclav Smidl.

  -----------------------------------
  BDM++ - C++ library for Bayesian Decision Making under Uncertainty

  Using IT++ for numerical operations
  -----------------------------------
*/

#ifndef PARTICLES_H
#define PARTICLES_H


#include "../estim/arx_ext.h"

namespace bdm {

/*!
* \brief Trivial particle filter with proposal density equal to parameter evolution model.

Posterior density is represented by a weighted empirical density (\c eEmp ).
*/

class PF : public BM {
        //! Introduces two new options:
        //! \li weights - meaning that all weightes will be logged
        //! \li samples - all samples will be also logged
        LOG_LEVEL(PF,weights,samples);

protected:
        //!number of particles;
        int n;
        //!posterior density
        eEmp est;
        //! pointer into \c eEmp
        vec &_w;
        //! pointer into \c eEmp
        Array<vec> &_samples;
        //! Parameter evolution model
        shared_ptr<pdf> par;
        //! Observation model
        shared_ptr<pdf> obs;
        //! internal structure storing loglikelihood of predictions
        vec lls;

        //! which resampling method will be used
        RESAMPLING_METHOD resmethod;
        //! resampling threshold; in this case its meaning is minimum ratio of active particles
        //! For example, for 0.5 resampling is performed when the numebr of active aprticles drops belo 50%.
        double res_threshold;

        //! \name Options
        //!@{
        //!@}

public:
        //! \name Constructors
        //!@{
        PF ( ) : est(), _w ( est._w() ), _samples ( est._samples() ) {
        };

        void set_parameters ( int n0, double res_th0 = 0.5, RESAMPLING_METHOD rm = SYSTEMATIC ) {
                n = n0;
                res_threshold = res_th0;
                resmethod = rm;
        };
        void set_model ( shared_ptr<pdf> par0, shared_ptr<pdf> obs0 ) {
                par = par0;
                obs = obs0;
                // set values for posterior
                est.set_rv ( par->_rv() );
        };
        void set_statistics ( const vec w0, const epdf &epdf0 ) {
                est.set_statistics ( w0, epdf0 );
        };
        void set_statistics ( const eEmp &epdf0 ) {
                bdm_assert_debug ( epdf0._rv().equal ( par->_rv() ), "Incompatible input" );
                est = epdf0;
        };
        //!@}

        //! Set posterior density by sampling from epdf0
        //! bayes I - generate samples and add their weights to lls
        virtual void bayes_gensmp ( const vec &ut );
        //! bayes II - compute weights of the
        virtual void bayes_weights();
        //! important part of particle filtering - decide if it is time to perform resampling
        virtual bool do_resampling() {
                double eff = 1.0 / ( _w * _w );
                return eff < ( res_threshold*n );
        }
        void bayes ( const vec &yt, const vec &cond );
        //!access function
        vec& __w() {
                return _w;
        }
        //!access function
        vec& _lls() {
                return lls;
        }
        //!access function
        RESAMPLING_METHOD _resmethod() const {
                return resmethod;
        }
        //! return correctly typed posterior (covariant return)
        const eEmp& posterior() const {
                return est;
        }

        /*! configuration structure for basic PF
        \code
        parameter_pdf   = pdf_class;         // parameter evolution pdf
        observation_pdf = pdf_class;         // observation pdf
        prior           = epdf_class;         // prior probability density
        --- optional ---
        n               = 10;                 // number of particles
        resmethod       = 'systematic', or 'multinomial', or 'stratified'
                                                                                  // resampling method
        res_threshold   = 0.5;                // resample when active particles drop below 50%
        \endcode
        */
        void from_setting ( const Setting &set ) {
                BM::from_setting ( set );
                par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );
                obs = UI::build<pdf> ( set, "observation_pdf", UI::compulsory );

                prior_from_set ( set );
                resmethod_from_set ( set );
                // set resampling method
                //set drv
                //find potential input - what remains in rvc when we subtract rv
                RV u = par->_rvc().remove_time().subt ( par->_rv() );
                //find potential input - what remains in rvc when we subtract x_t
                RV obs_u = obs->_rvc().remove_time().subt ( par->_rv() );

                u.add ( obs_u ); // join both u, and check if they do not overlap

                set_yrv ( obs->_rv() );
                rvc = u;
        }
        //! auxiliary function reading parameter 'resmethod' from configuration file
        void resmethod_from_set ( const Setting &set ) {
                string resmeth;
                if ( UI::get ( resmeth, set, "resmethod", UI::optional ) ) {
                        if ( resmeth == "systematic" ) {
                                resmethod = SYSTEMATIC;
                        } else  {
                                if ( resmeth == "multinomial" ) {
                                        resmethod = MULTINOMIAL;
                                } else {
                                        if ( resmeth == "stratified" ) {
                                                resmethod = STRATIFIED;
                                        } else {
                                                bdm_error ( "Unknown resampling method" );
                                        }
                                }
                        }
                } else {
                        resmethod = SYSTEMATIC;
                };
                if ( !UI::get ( res_threshold, set, "res_threshold", UI::optional ) ) {
                        res_threshold = 0.5;
                }
                //validate();
        }
        //! load prior information from set and set internal structures accordingly
        void prior_from_set ( const Setting & set ) {
                shared_ptr<epdf> pri = UI::build<epdf> ( set, "prior", UI::compulsory );

                eEmp *test_emp = dynamic_cast<eEmp*> ( & ( *pri ) );
                if ( test_emp ) { // given pdf is sampled
                        est = *test_emp;
                } else {
                        //int n;
                        if ( !UI::get ( n, set, "n", UI::optional ) ) {
                                n = 10;
                        }
                        // sample from prior
                        set_statistics ( ones ( n ) / n, *pri );
                }
                n = est._w().length();
                lls=zeros(n);
        }

        void validate() {
                BM::validate();
                bdm_assert ( par, "PF::parameter_pdf not specified." );
                n = _w.length();
                lls = zeros ( n );

                if ( par->_rv()._dsize() > 0 ) {
                        bdm_assert ( par->_rv()._dsize() == est.dimension(), "Mismatch of RV and dimension of posterior" );
                }
        }
        //! resample posterior density (from outside - see MPF)
        void resample ( ivec &ind ) {
                est.resample ( ind, resmethod );
        }
        //! access function
        Array<vec>& __samples() {
                return _samples;
        }

        virtual double logpred ( const vec &yt ) const NOT_IMPLEMENTED(0);

        virtual epdf* epredictor() const NOT_IMPLEMENTED(NULL);
        
        virtual pdf* predictor() const NOT_IMPLEMENTED(NULL);
};
UIREGISTER ( PF );

/*!
\brief Marginalized Particle filter

A composition of particle filter with exact (or almost exact) bayesian models (BMs).
The Bayesian models provide marginalized predictive density. Internaly this is achieved by virtual class MPFpdf.
*/

class MPF : public BM  {
        //! Introduces new option 
        //! \li means - meaning TODO
        LOG_LEVEL(MPF,means);
protected:
        //! particle filter on non-linear variable
        shared_ptr<PF> pf;
        //! Array of Bayesian models
        Array<BM*> BMs;

        //! internal class for pdf providing composition of eEmp with external components

        class mpfepdf : public epdf  {
                //! pointer to particle filter
                shared_ptr<PF> &pf;
                //! pointer to Array of BMs
                Array<BM*> &BMs;
        public:
                //! constructor
                mpfepdf ( shared_ptr<PF> &pf0, Array<BM*> &BMs0 ) : epdf(), pf ( pf0 ), BMs ( BMs0 ) { };
                //! a variant of set parameters - this time, parameters are read from BMs and pf
                void read_parameters() {
                        rv = concat ( pf->posterior()._rv(), BMs ( 0 )->posterior()._rv() );
                        dim = pf->posterior().dimension() + BMs ( 0 )->posterior().dimension();
                        bdm_assert_debug ( dim == rv._dsize(), "Wrong name " );
                }
                vec mean() const;

                vec variance() const;

                void qbounds ( vec &lb, vec &ub, double perc = 0.95 ) const;

                vec sample() const NOT_IMPLEMENTED(0);

                double evallog ( const vec &val ) const NOT_IMPLEMENTED(0);             
        };

        //! Density joining PF.est with conditional parts
        mpfepdf jest;

        //! datalink from global yt and cond (Up) to BMs yt and cond (Down)
        datalink_m2m this2bm;
        //! datalink from global yt and cond (Up) to PFs yt and cond (Down)
        datalink_m2m this2pf;
        //!datalink from PF part to BM
        datalink_part pf2bm;

public:
        //! Default constructor.
        MPF () :  jest ( pf, BMs ) {};
        //! set all parameters at once
        void set_pf ( shared_ptr<pdf> par0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
                if (!pf) pf=new PF;
                pf->set_model ( par0, par0 ); // <=== nasty!!!
                pf->set_parameters ( n0, rm );
                pf->set_rv(par0->_rv());
                BMs.set_length ( n0 );
        }
        //! set a prototype of BM, copy it to as many times as there is particles in pf
        void set_BM ( const BM &BMcond0 ) {

                int n = pf->__w().length();
                BMs.set_length ( n );
                // copy
                //BMcond0 .condition ( pf->posterior()._sample ( 0 ) );
                for ( int i = 0; i < n; i++ ) {
                        BMs ( i ) = (BM*) BMcond0._copy();
                }
        };

        void bayes ( const vec &yt, const vec &cond );

        const epdf& posterior() const {
                return jest;
        }

        //!Access function
        const BM* _BM ( int i ) {
                return BMs ( i );
        }
        PF& _pf() {return *pf;}


        virtual double logpred ( const vec &yt ) const NOT_IMPLEMENTED(0); 
                
        virtual epdf* epredictor() const NOT_IMPLEMENTED(NULL); 
        
        virtual pdf* predictor() const NOT_IMPLEMENTED(NULL); 


        /*! configuration structure for basic PF
        \code
        BM              = BM_class;           // Bayesian filtr for analytical part of the model
        parameter_pdf   = pdf_class;         // transitional pdf for non-parametric part of the model
        prior           = epdf_class;         // prior probability density
        --- optional ---
        n               = 10;                 // number of particles
        resmethod       = 'systematic', or 'multinomial', or 'stratified'
                                                                                  // resampling method
        \endcode
        */
        void from_setting ( const Setting &set ) {
                BM::from_setting( set );

                shared_ptr<pdf> par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );

                pf = new PF;
                // prior must be set before BM
                pf->prior_from_set ( set );
                pf->resmethod_from_set ( set );
                pf->set_model ( par, par ); // too hackish!

                shared_ptr<BM> BM0 = UI::build<BM> ( set, "BM", UI::compulsory );
                set_BM ( *BM0 );

                //set drv
                //??set_yrv(concat(BM0->_yrv(),u) );
                set_yrv ( BM0->_yrv() );
                rvc = BM0->_rvc().subt ( par->_rv() );
                //find potential input - what remains in rvc when we subtract rv
                RV u = par->_rvc().subt ( par->_rv().copy_t ( -1 ) );
                rvc.add ( u );
                dimc = rvc._dsize();
                validate();
        }

        void validate() {
                BM::validate();
                try {
                        pf->validate();
                } catch ( std::exception ) {
                        throw UIException ( "Error in PF part of MPF:" );
                }
                jest.read_parameters();
                this2bm.set_connection ( BMs ( 0 )->_yrv(), BMs ( 0 )->_rvc(), yrv, rvc );
                this2pf.set_connection ( pf->_yrv(), pf->_rvc(), yrv, rvc );
                pf2bm.set_connection ( BMs ( 0 )->_rvc(), pf->posterior()._rv() );
        }
};
UIREGISTER ( MPF );

/*! ARXg for estimation of state-space variances
*/
class MPF_ARXg :public BM{
        protected:
        shared_ptr<PF> pf;
        //! pointer to Array of BMs
        Array<ARX*> BMso;
        //! pointer to Array of BMs
        Array<ARX*> BMsp;
        //!parameter evolution
        shared_ptr<fnc> g;
        //!observation function
        shared_ptr<fnc> h;
        
        public:
                void bayes(const vec &yt, const vec &cond );
                void from_setting(const Setting &set) ;
                void validate() {
                        bdm_assert(g->dimensionc()==g->dimension(),"not supported yet");
                        bdm_assert(h->dimensionc()==g->dimension(),"not supported yet");                        
                }

                double logpred(const vec &cond) const NOT_IMPLEMENTED(0.0);
                epdf* epredictor() const NOT_IMPLEMENTED(NULL);
                pdf* predictor() const NOT_IMPLEMENTED(NULL);
                const epdf& posterior() const {return pf->posterior();};
                
                void log_register( logger &L, const string &prefix ){
                        BM::log_register(L,prefix);
                        logrec->ids.set_size ( 3 );
                        logrec->ids(1)= L.add_vector(RV("Q",dimension()*dimension()), prefix+L.prefix_sep()+"Q");
                        logrec->ids(2)= L.add_vector(RV("R",dimensiony()*dimensiony()), prefix+L.prefix_sep()+"R");
                        
                };
                void log_write() const {
                        BM::log_write();
                        mat mQ=zeros(dimension(),dimension());
                        mat pom=zeros(dimension(),dimension());
                        mat mR=zeros(dimensiony(),dimensiony());
                        mat pom2=zeros(dimensiony(),dimensiony());
                        mat dum;
                        const vec w=pf->posterior()._w();
                        for (int i=0; i<w.length(); i++){
                                BMsp(i)->posterior().mean_mat(dum,pom);
                                mQ += w(i) * pom;
                                BMso(i)->posterior().mean_mat(dum,pom2);
                                mR += w(i) * pom2;
                                
                        }
                        logrec->L.log_vector ( logrec->ids ( 1 ), cvectorize(mQ) );
                        logrec->L.log_vector ( logrec->ids ( 2 ), cvectorize(mR) );
                        
                }
};
UIREGISTER(MPF_ARXg);


}
#endif // KF_H