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

/*!
  \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 "../stat/exp_family.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 {
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<mpdf> par;
        //! Observation model
        shared_ptr<mpdf> 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<mpdf> par0, shared_ptr<mpdf> 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()),"Incompatibel input");
                est=epdf0;
        };
        //!@}
        //! Set posterior density by sampling from epdf0
        //! Extends original BM::set_options by two more options:
        //! \li logweights - meaning that all weightes will be logged
        //! \li logsamples - all samples will be also logged
        void set_options ( const string &opt ) {
                BM::set_options ( opt );
        }
        //! 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 &dt );
        //!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   = mpdf_class;         // parameter evolution pdf
        observation_pdf = mpdf_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<mpdf>(set,"parameter_pdf",UI::compulsory);
                obs = UI::build<mpdf>(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_drv(concat(obs->_rv(),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();
                //validate();
        }
        
        void validate(){
                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;}
};
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 MPFmpdf.
*/

class MPF : public BM  {
        protected:
                //! particle filter on non-linear variable
        shared_ptr<PF> pf;
        //! Array of Bayesian models
        Array<BM*> BMs;

        //! internal class for MPDF 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 {
                        const vec &w = pf->posterior()._w();
                        vec pom = zeros ( BMs(0)->posterior ().dimension() );
                        //compute mean of BMs
                        for ( int i = 0; i < w.length(); i++ ) {
                                pom += BMs ( i )->posterior().mean() * w ( i );
                        }
                        return concat ( pf->posterior().mean(), pom );
                }
                vec variance() const {
                        const vec &w = pf->posterior()._w();
                        
                        vec pom = zeros ( BMs(0)->posterior ().dimension() );
                        vec pom2 = zeros ( BMs(0)->posterior ().dimension() );
                        vec mea;
                        
                        for ( int i = 0; i < w.length(); i++ ) {
                                // save current mean 
                                mea = BMs ( i )->posterior().mean();
                                pom += mea * w ( i );
                                //compute variance
                                pom2 += ( BMs ( i )->posterior().variance() + pow ( mea, 2 ) ) * w ( i );
                        }
                        return concat ( pf->posterior().variance(), pom2 - pow ( pom, 2 ) );
                }
                
                void qbounds ( vec &lb, vec &ub, double perc = 0.95 ) const {
                        //bounds on particles
                        vec lbp;
                        vec ubp;
                        pf->posterior().qbounds ( lbp, ubp );

                        //bounds on Components
                        int dimC = BMs ( 0 )->posterior().dimension();
                        int j;
                        // temporary
                        vec lbc ( dimC );
                        vec ubc ( dimC );
                        // minima and maxima
                        vec Lbc ( dimC );
                        vec Ubc ( dimC );
                        Lbc = std::numeric_limits<double>::infinity();
                        Ubc = -std::numeric_limits<double>::infinity();

                        for ( int i = 0; i < BMs.length(); i++ ) {
                                // check Coms
                                BMs ( i )->posterior().qbounds ( lbc, ubc );
                                //save either minima or maxima
                                for ( j = 0; j < dimC; j++ ) {
                                        if ( lbc ( j ) < Lbc ( j ) ) {
                                                Lbc ( j ) = lbc ( j );
                                        }
                                        if ( ubc ( j ) > Ubc ( j ) ) {
                                                Ubc ( j ) = ubc ( j );
                                        }
                                }
                        }
                        lb = concat ( lbp, Lbc );
                        ub = concat ( ubp, Ubc );
                }

                vec sample() const {
                        bdm_error ( "Not implemented" );
                        return vec();
                }

                double evallog ( const vec &val ) const {
                        bdm_error ( "not implemented" );
                        return 0.0;
                }
        };

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

        //! Log means of BMs
        bool opt_L_mea;

public:
        //! Default constructor.
        MPF () :  jest (pf,BMs) {};
        //! set all parameters at once
        void set_parameters ( shared_ptr<mpdf> par0, shared_ptr<mpdf> obs0, int n0, RESAMPLING_METHOD rm = SYSTEMATIC ) {
                pf->set_model ( par0, obs0); 
                pf->set_parameters(n0, rm );
                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 ) = BMcond0._copy_();
                }
        };

        void bayes ( const vec &dt );
        const epdf& posterior() const {
                return jest;
        }
        //! Extends options understood by BM::set_options by option 
        //! \li logmeans - meaning 
        void set_options ( const string &opt ) {
                BM::set_options(opt);
                opt_L_mea = ( opt.find ( "logmeans" ) != string::npos );
        }

        //!Access function
        const BM* _BM ( int i ) {
                return BMs ( i );
        }
        
        /*! configuration structure for basic PF
        \code
        BM              = BM_class;           // Bayesian filtr for analytical part of the model
        parameter_pdf   = mpdf_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){
                shared_ptr<mpdf> par = UI::build<mpdf>(set,"parameter_pdf",UI::compulsory);
                shared_ptr<mpdf> obs= new mpdf(); // not used!!

                pf = new PF;
                // rpior must be set before BM
                pf->prior_from_set(set);
                pf->resmethod_from_set(set);
                pf->set_model(par,obs);
                
                shared_ptr<BM> BM0 =UI::build<BM>(set,"BM",UI::compulsory);
                set_BM(*BM0);
                
                string opt;
                if (UI::get(opt,set,"options",UI::optional)){
                        set_options(opt);
                }
                //set drv
                //find potential input - what remains in rvc when we subtract rv
                RV u = par->_rvc().remove_time().subt( par->_rv() );            
                set_drv(concat(BM0->_drv(),u) );
                validate();
        }
        void validate(){
                try{
                pf->validate();
                } catch (std::exception &e){
                        throw UIException("Error in PF part of MPF:");
                }
                jest.read_parameters();
                for ( int i = 0; i < pf->__w().length(); i++ ) {
                        BMs ( i )->condition ( pf->posterior()._sample ( i ) );
                }
        }

};
UIREGISTER(MPF);

}
#endif // KF_H