root/library/bdm/estim/particles.h

/*!
  \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"
#include "../stat/emix.h"

namespace bdm {

//! \brief Abstract class for Marginalized Particles
class MarginalizedParticleBase : public BM {
protected:
    //! discrte particle
    dirac est_emp;
    //! internal Bayes Model
    shared_ptr<BM> bm;

    //! \brief Internal class for custom posterior - product of empirical and exact part
    class eprod_2:public eprod_base {
    protected:
        MarginalizedParticleBase &mp;
    public:
                eprod_2(MarginalizedParticleBase &m):mp(m) {}
                const epdf* factor(int i) const {
            return (i==0) ? &mp.bm->posterior() : &mp.est_emp;
        }
        const int no_factors() const {
            return 2;
        }
    } est;

public:
    MarginalizedParticleBase():est(*this) {};
    MarginalizedParticleBase(const MarginalizedParticleBase &m2):BM(m2),est(*this) {
        bm = m2.bm->_copy();
        est_emp = m2.est_emp;
        est.validate();
        validate();
    };
    void bayes(const vec &dt, const vec &cond) NOT_IMPLEMENTED_VOID;

    const eprod_2& posterior() const {
        return est;
    }

    void set_prior(const epdf *pdf0) {
        const eprod *ep=dynamic_cast<const eprod*>(pdf0);
        if (ep) { // full prior
            bdm_assert(ep->no_factors()==2,"Incompatible prod");
            bm->set_prior(ep->factor(0));
            est_emp.set_point(ep->factor(1)->sample());
        } else {
            // assume prior is only for emp;
            est_emp.set_point(pdf0->sample());
        }
    }


    /*! Create object from the following structure

    \code
    class = "MarginalizedParticleBase";
    bm = configuration of bdm::BM;          % any offspring of BM, bdm::BM::from_setting
    --- inherited fields ---
    bdm::BM::from_setting
    \endcode
    */
    void from_setting(const Setting &set) {
        BM::from_setting ( set );
        bm = UI::build<BM> ( set, "bm", UI::compulsory );
    }
    void validate() {
        BM::validate();
        //est.validate(); --pdfs not known
        bdm_assert(bm,"Internal BM is not given");
    }
};

//! \brief Particle with marginalized subspace, used in PF 
class MarginalizedParticle : public MarginalizedParticleBase {
protected:
    //! pdf with for transitional par
    shared_ptr<pdf> par; // pdf for non-linear part
    //! link from this to bm
    shared_ptr<datalink_part> cond2bm;
    //! link from cond to par
    shared_ptr<datalink_part> cond2par;
    //! link from emp 2 par
    shared_ptr<datalink_part> emp2bm;
    //! link from emp 2 par
    shared_ptr<datalink_part> emp2par;

public:
    BM* _copy() const {
        return new MarginalizedParticle(*this);
    };
    void bayes(const vec &dt, const vec &cond) {
        vec par_cond(par->dimensionc());
        cond2par->filldown(cond,par_cond); // copy ut
        emp2par->filldown(est_emp._point(),par_cond); // copy xt-1

        //sample new particle
        est_emp.set_point(par->samplecond(par_cond));
        //if (evalll)
        vec bm_cond(bm->dimensionc());
        cond2bm->filldown(cond, bm_cond);// set e.g. ut
        emp2bm->filldown(est_emp._point(), bm_cond);// set e.g. ut
        bm->bayes(dt, bm_cond);
        ll=bm->_ll();
    }

    /*! Create object from the following structure

    \code
    class = "MarginalizedParticle";
    parameter_pdf = configuration of bdm::epdf;          % any offspring of epdf, bdm::epdf::from_setting
    --- inherited fields ---
    bdm::MarginalizedParticleBase::from_setting
    \endcode
    */    
    void from_setting(const Setting &set) {
        MarginalizedParticleBase::from_setting ( set );
        par = UI::build<pdf> ( set, "parameter_pdf", UI::compulsory );
    }

    void to_setting(Setting &set)const {
        MarginalizedParticleBase::to_setting(set);
        UI::save(par,set,"parameter_pdf");
                UI::save(bm,set,"bm");
    }
    void validate() {
        MarginalizedParticleBase::validate();
        est_emp.set_rv(par->_rv());
        if (est_emp.point.length()!=par->dimension())
            est_emp.set_point(zeros(par->dimension()));
        est.validate();

        yrv = bm->_yrv();
        dimy = bm->dimensiony();
        set_rv( concat(bm->_rv(), par->_rv()));
        set_dim( par->dimension()+bm->dimension());

        rvc = par->_rvc();
        rvc.add(bm->_rvc());
        rvc=rvc.subt(par->_rv());
        rvc=rvc.subt(par->_rv().copy_t(-1));
        rvc=rvc.subt(bm->_rv().copy_t(-1)); //

        cond2bm=new datalink_part;
        cond2par=new datalink_part;
        emp2bm  =new datalink_part;
        emp2par =new datalink_part;
        cond2bm->set_connection(bm->_rvc(), rvc);
        cond2par->set_connection(par->_rvc(), rvc);
        emp2bm->set_connection(bm->_rvc(), par->_rv());
        emp2par->set_connection(par->_rvc(), par->_rv().copy_t(-1));

        dimc = rvc._dsize();
    };
};
UIREGISTER(MarginalizedParticle);

//! Internal class which is used in PF
class BootstrapParticle : public BM {
    dirac est;
    shared_ptr<pdf> par;
    shared_ptr<pdf> obs;
    shared_ptr<datalink_part> cond2par;
    shared_ptr<datalink_part> cond2obs;
    shared_ptr<datalink_part> xt2obs;
    shared_ptr<datalink_part> xtm2par;
public:
    BM* _copy() const {
        return new BootstrapParticle(*this);
    };
    void bayes(const vec &dt, const vec &cond) {
        vec par_cond(par->dimensionc());
        cond2par->filldown(cond,par_cond); // copy ut
        xtm2par->filldown(est._point(),par_cond); // copy xt-1

        //sample new particle
        est.set_point(par->samplecond(par_cond));
        //if (evalll)
        vec obs_cond(obs->dimensionc());
        cond2obs->filldown(cond, obs_cond);// set e.g. ut
        xt2obs->filldown(est._point(), obs_cond);// set e.g. ut
        ll=obs->evallogcond(dt,obs_cond);
    }
    const dirac& posterior() const {
        return est;
    }

    void set_prior(const epdf *pdf0) {
        est.set_point(pdf0->sample());
    }

    /*! Create object from the following structure
    \code
    class = "BootstrapParticle";
    parameter_pdf = configuration of bdm::epdf;      % any offspring of epdf, bdm::epdf::from_setting
    observation_pdf = configuration of bdm::epdf;    % any offspring of epdf, bdm::epdf::from_setting
    --- inherited fields ---
    bdm::BM::from_setting
    \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 );
    }

    void validate() {
        yrv = obs->_rv();
        dimy = obs->dimension();
        set_rv( par->_rv());
        set_dim( par->dimension());

        rvc = par->_rvc().subt(par->_rv().copy_t(-1));
                rvc.add(obs->_rvc().subt(par->_rv())); //

        cond2obs=new datalink_part;
        cond2par=new datalink_part;
        xt2obs  =new datalink_part;
        xtm2par =new datalink_part;
        cond2obs->set_connection(obs->_rvc(), rvc);
        cond2par->set_connection(par->_rvc(), rvc);
        xt2obs->set_connection(obs->_rvc(), _rv());
        xtm2par->set_connection(par->_rvc(), _rv().copy_t(-1));

        dimc = rvc._dsize();
    };
};
UIREGISTER(BootstrapParticle);


/*!
* \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 {
    //! \var log_level_enums logweights
    //! all weightes will be logged

    //! \var log_level_enums logmeans
    //! means of particles will be logged
    LOG_LEVEL(PF,logweights,logmeans,logvars,logNeff);

    class pf_mix: public emix_base {
        Array<BM*> &bms;
    public:
        pf_mix(vec &w0, Array<BM*> &bms0):emix_base(w0),bms(bms0) {}
        const epdf* component(const int &i)const {
            return &(bms(i)->posterior());
        }
        int no_coms() const {
            return bms.length();
        }
    };
protected:
    //!number of particles;
    int n;
    //!posterior density
    pf_mix est;
    //! weights;
    vec w;
    //! particles
    Array<BM*> particles;
    //! 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,particles) { };

    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 ( const BM *particle0, const epdf *prior) {
        if (n>0) {
            particles.set_length(n);
            for (int i=0; i<n; i++) {
                particles(i) = particle0->_copy();
                particles(i)->set_prior(prior);
            }
        }
        // set values for posterior
        est.set_rv ( particle0->posterior()._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;
        };*/
    //!@}

    //! bayes 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& _lls() {
        return lls;
    }
    //!access function
    RESAMPLING_METHOD _resmethod() const {
        return resmethod;
    }
    //! return correctly typed posterior (covariant return)
    const pf_mix& posterior() const {
        return est;
    }

    /*! configuration structure for basic PF
    \code
    particle        = bdm::BootstrapParticle;       % one bayes rule for each point in the empirical support
      - or -        = bdm::MarginalizedParticle;    % (in case of Marginalized Particle filtering
    prior           = epdf_class;                   % prior probability density on the empirical variable
    --- 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 );
        UI::get ( log_level, set, "log_level", UI::optional );

        shared_ptr<BM> bm0 = UI::build<BM>(set, "particle",UI::compulsory);

        n =0;
        UI::get(n,set,"n",UI::optional);;
        if (n>0) {
            particles.set_length(n);
            for(int i=0; i<n; i++) {
                particles(i)=bm0->_copy();
            }
            w = ones(n)/n;
        }
        shared_ptr<epdf> pri = UI::build<epdf>(set,"prior");
        set_prior(pri.get());
        // set resampling method
        resmethod_from_set ( set );
        //set drv

        rvc = bm0->_rvc();
        dimc = bm0->dimensionc();
        BM::set_rv(bm0->_rv());
        yrv=bm0->_yrv();
        dimy = bm0->dimensiony();
    }
    
    void to_setting(Setting &set) const{
                BM::to_setting(set);
                UI::save(particles, set,"particles");
                UI::save(w,set,"w");
        }

    void log_register ( bdm::logger& L, const string& prefix ) {
        BM::log_register(L,prefix);
                if (log_level[logweights]) {
                        L.add_vector( log_level, logweights, RV ( particles.length()), prefix);
                }
                if (log_level[logNeff]) {
                        L.add_vector( log_level, logNeff, RV ( 1), prefix);
                }
                if (log_level[logmeans]) {
            for (int i=0; i<particles.length(); i++) {
                L.add_vector( log_level, logmeans, RV ( particles(i)->dimension() ), prefix , i);
            }
        }
        if (log_level[logvars]) {
            for (int i=0; i<particles.length(); i++) {
                L.add_vector( log_level, logvars, RV ( particles(i)->dimension() ), prefix , i);
            }
        }
    };
    void log_write ( ) const {
        BM::log_write();
                // weigths are before resamplign -- bayes
        if (log_level[logmeans]) {
            for (int i=0; i<particles.length(); i++) {
                log_level.store( logmeans, particles(i)->posterior().mean(), i);
            }
        }
        if (log_level[logvars]) {
            for (int i=0; i<particles.length(); i++) {
                log_level.store( logvars, particles(i)->posterior().variance(), i);
            }
        }

    }

    void set_prior(const epdf *pri) {
        const emix_base *emi=dynamic_cast<const emix_base*>(pri);
        if (emi) {
            bdm_assert(particles.length()>0, "initial particle is not assigned");
            n = emi->_w().length();
            int old_n = particles.length();
            if (n!=old_n) {
                particles.set_length(n,true);
            }
            for(int i=old_n; i<n; i++) {
                particles(i)=particles(0)->_copy();
            }

            for (int i =0; i<n; i++) {
                particles(i)->set_prior(emi->_com(i));
            }
        } else {
            // try to find "n"
            bdm_assert(n>0, "Field 'n' must be filled when prior is not of type emix");
            for (int i =0; i<n; i++) {
                particles(i)->set_prior(pri);
            }

        }
    }
    //! 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.9;
        }
        //validate();
    }

    void validate() {
        BM::validate();
        est.validate();
        bdm_assert ( n>0, "empty particle pool" );
        n = w.length();
        lls = zeros ( n );

        if ( particles(0)->_rv()._dsize() > 0 ) {
            bdm_assert (  particles(0)->_rv()._dsize() == est.dimension(), "MPF:: Mismatch of RV " +particles(0)->_rv().to_string() +
                          " of size (" +num2str(particles(0)->_rv()._dsize())+") and dimension of posterior ("+num2str(est.dimension()) + ")" );
        }
    }
    //! resample posterior density (from outside - see MPF)
    void resample ( ) {
        ivec ind = zeros_i ( n );
        bdm::resample(w,ind,resmethod);
        // copy the internals according to ind
        for (int i = 0; i < n; i++ ) {
            if ( ind ( i ) != i ) {
                delete particles(i);
                particles( i ) = particles( ind ( i ) )->_copy();
            }
            w ( i ) = 1.0 / n;
        }
    }
    //! access function
    Array<BM*>& _particles() {
        return particles;
    }
    ~PF() {
        for (int i=0; i<particles.length(); i++) {
            delete particles(i);
        }
    }

};
UIREGISTER ( PF );

/*! Marginalized particle for state-space models with unknown parameters of distribuution of residues on \f$v_t\f$.

\f{eqnarray*}{
    x_t &=& g(x_{t-1}) + v_t,\\
    y_t &\sim &fy(x_t),
    \f}

    This particle is a only a shell creating the residues calling internal estimator of their parameters. The internal estimator can be of any compatible type, e.g. ARX for Gaussian residues with unknown mean and variance.

    */
class NoiseParticleX : public MarginalizedParticleBase {
protected:
    //! function transforming xt, ut -> x_t+1
    shared_ptr<fnc> g; // pdf for non-linear part
    //! function transforming xt,ut -> yt
    shared_ptr<pdf> fy; // pdf for non-linear part

    RV rvx;
    RV rvxc;
    RV rvyc;

    //!link from condition to f
    datalink_part cond2g;
    //!link from condition to h
    datalink_part cond2fy;
    //!link from xt to f
    datalink_part x2g;
    //!link from xt to h
    datalink_part x2fy;

public:
    BM* _copy() const {
        return new NoiseParticleX(*this);
    };
    void bayes(const vec &dt, const vec &cond) {
        //shared_ptr<epdf> pred_v=bm->epredictor();

vec xt;
vec xthat;
//vec vt=pred_v->sample();
//              vec vt = bm->samplepred();
if (1){
                //vec vt=pred_v->sample();
                int dim_x = bm->dimensiony();
                int dim_y = dimensiony();
                
                vec post_mean =bm->posterior().mean();
                mat SigV;
                if (post_mean.length()==dim_x){
                        SigV=diag(vec(post_mean._data(), dim_x));
                } else {
                        SigV=mat(post_mean._data(), dim_x, dim_x);
                }

                vec &xtm=est_emp.point;
                vec g_args(g->dimensionc());
                x2g.filldown(xtm,g_args);
                cond2g.filldown(cond,g_args);
                xthat = g->eval(g_args);

                mat IC = eye(dim_x+dim_y);
                IC.set_submatrix(0,0,eye(dim_x));
                mat SigJo=eye(dim_x + dim_y);
                SigJo.set_submatrix(0,0,SigV);
                mlnorm<ldmat>* mlfy=dynamic_cast<mlnorm<ldmat>* >(fy.get());
                
                SigJo.set_submatrix(dim_x,dim_x, mlfy->_R());
                IC.set_submatrix(dim_x,0,-mlfy->_A());

                mat iIC=inv(IC);
                enorm<chmat> Jo;
                Jo._mu()=iIC*concat(xthat, zeros(dim_y));
                Jo._R()=iIC*SigJo*iIC.T();
                Jo.set_rv(concat(bm->_yrv(),yrv));
                Jo.validate();
                shared_ptr<pdf> Cond=Jo.condition(bm->_yrv());
                //new sample

                xt = Cond->samplecond(dt);//  + vt;
} else{


        //new sample
        vec &xtm=est_emp.point;
        vec g_args(g->dimensionc());
        x2g.filldown(xtm,g_args);
        cond2g.filldown(cond,g_args);
                xthat = g->eval(g_args);
        
                xt = xthat + bm->samplepred();
                
}
        est_emp.point=xt;

        // the vector [v_t] updates bm,
        bm->bayes(xt-xthat);

        // residue of observation
        vec fy_args(fy->dimensionc());
        x2fy.filldown(xt,fy_args);
        cond2fy.filldown(cond,fy_args);

        ll= fy->evallogcond(dt,fy_args);
    }
    void from_setting(const Setting &set) {
        MarginalizedParticleBase::from_setting(set); //reads bm, yrv,rvc, bm_rv, etc...

        g=UI::build<fnc>(set,"g",UI::compulsory);
        fy=UI::build<pdf>(set,"fy",UI::compulsory);
        UI::get(rvx,set,"rvx",UI::compulsory);
        est_emp.set_rv(rvx);

        UI::get(rvxc,set,"rvxc",UI::compulsory);
        UI::get(rvyc,set,"rvyc",UI::compulsory);

    }
    void to_setting (Setting &set) const {
                MarginalizedParticleBase::to_setting(set); //reads bm, yrv,rvc, bm_rv, etc...
                UI::save(g,set,"g");
                UI::save(fy,set,"fy");
                UI::save(bm,set,"bm");
        }
    void validate() {
        MarginalizedParticleBase::validate();

        dimy = fy->dimension();
        bm->set_yrv(rvx);

        est_emp.set_rv(rvx);
        est_emp.set_dim(rvx._dsize());
        est.validate();
        //
        //check dimensions
        rvc = rvxc.subt(rvx.copy_t(-1));
        rvc.add( rvyc);
        rvc=rvc.subt(rvx);

        bdm_assert(g->dimension()==rvx._dsize(),"rvx is not described");
        bdm_assert(g->dimensionc()==rvxc._dsize(),"rvxc is not described");
        bdm_assert(fy->dimensionc()==rvyc._dsize(),"rvyc is not described");

        bdm_assert(bm->dimensiony()==g->dimension(),
                   "Incompatible noise estimator of dimension " +
                   num2str(bm->dimensiony()) + " does not match dimension of g , " +
                   num2str(g->dimension()));

        dimc = rvc._dsize();

        //establish datalinks
        x2g.set_connection(rvxc, rvx.copy_t(-1));
        cond2g.set_connection(rvxc, rvc);

        x2fy.set_connection(rvyc, rvx);
        cond2fy.set_connection(rvyc, rvc);
    }
};
UIREGISTER(NoiseParticleX);


/*! Marginalized particle for state-space models with unknown parameters of distribuution of residues on \f$v_t\f$ and \f$ w_t \f$.

\f{eqnarray*}{
        x_t &=& g(x_{t-1}) + v_t,\\
        y_t &= &h(x_t)+w_t,
        \f}
        
        This particle is a only a shell creating the residues calling internal estimator of their parameters. The internal estimator can be of any compatible type, e.g. ARX for Gaussian residues with unknown mean and variance.
        
        */
class NoiseParticleXY : public BM {
protected:
        //! discrte particle
        dirac est_emp;
        //! internal Bayes Model
        shared_ptr<BM> bmx;
        shared_ptr<BM> bmy;
        
        //! \brief Internal class for custom posterior - product of empirical and exact part
        class eprod_3:public eprod_base {
                protected:
                        NoiseParticleXY &mp;
                public:
                        eprod_3(NoiseParticleXY &m):mp(m) {}
                        const epdf* factor(int i) const {
                                if (i==0) return &mp.bmx->posterior() ;
                                if(i==1) return &mp.bmy->posterior();
                                return &mp.est_emp;
                        }
                        const int no_factors() const {
                                return 3;
                        }
        } est;

        protected:
                //! function transforming xt, ut -> x_t+1
                shared_ptr<fnc> g; // pdf for non-linear part
                //! function transforming xt,ut -> yt
                shared_ptr<fnc> h; // pdf for non-linear part
                
                RV rvx;
                RV rvxc;
                RV rvyc;
                
                //!link from condition to f
                datalink_part cond2g;
                //!link from condition to h
                datalink_part cond2h;
                //!link from xt to f
                datalink_part x2g;
                //!link from xt to h
                datalink_part x2h;
                
        public:
                NoiseParticleXY():est(*this) {};
                NoiseParticleXY(const NoiseParticleXY &m2):BM(m2),est(*this),g(m2.g),h(m2.h), rvx(m2.rvx),rvxc(m2.rvxc),rvyc(m2.rvyc) {
                        bmx = m2.bmx->_copy();
                        bmy = m2.bmy->_copy();
                        est_emp = m2.est_emp;
                        //est.validate();
                        validate();
                };
                
                const eprod_3& posterior() const {
                        return est;
                }
                
                void set_prior(const epdf *pdf0) {
                        const eprod *ep=dynamic_cast<const eprod*>(pdf0);
                        if (ep) { // full prior
                                bdm_assert(ep->no_factors()==2,"Incompatible prod");
                                bmx->set_prior(ep->factor(0));
                                bmy->set_prior(ep->factor(1));
                                est_emp.set_point(ep->factor(2)->sample());
                        } else {
                                // assume prior is only for emp;
                                est_emp.set_point(pdf0->sample());
                        }
                }
                                
                BM* _copy() const {
                        return new NoiseParticleXY(*this);
                };
                
                void bayes(const vec &dt, const vec &cond) {
                        //shared_ptr<epdf> pred_v=bm->epredictor();
                        
                        //vec vt=pred_v->sample();
                        vec vt = bmx->samplepred();
                        
                        //new sample
                        vec &xtm=est_emp.point;
                        vec g_args(g->dimensionc());
                        x2g.filldown(xtm,g_args);
                        cond2g.filldown(cond,g_args);
                        vec xt = g->eval(g_args) + vt;
                        est_emp.point=xt;
                        
                        // the vector [v_t] updates bm,
                        bmx->bayes(vt);
                        
                        // residue of observation
                        vec h_args(h->dimensionc());
                        x2h.filldown(xt,h_args);
                        cond2h.filldown(cond,h_args);

                        vec z_y =h->eval(h_args)-dt;
//                      ARX *abm = dynamic_cast<ARX*>(bmy.get());
/*                      double ll2;
                        if (abm){ //ARX
                                shared_ptr<epdf> pr_y(abm->epredictor_student(empty_vec));
                                ll2=pr_y->evallog(z_y);
                        } else{
                                shared_ptr<epdf> pr_y(bmy->epredictor(empty_vec));
                                ll2=pr_y->evallog(z_y);
                        }*/
                        
                        bmy->bayes(z_y);
                        // test _lls
                        ll= bmy->_ll();
                }
                void from_setting(const Setting &set) {
                        BM::from_setting(set); //reads bm, yrv,rvc, bm_rv, etc...
                        bmx = UI::build<BM>(set,"bmx",UI::compulsory);
                        
                        bmy = UI::build<BM>(set,"bmy",UI::compulsory);
                        g=UI::build<fnc>(set,"g",UI::compulsory);
                        h=UI::build<fnc>(set,"h",UI::compulsory);
                        UI::get(rvx,set,"rvx",UI::compulsory);
                        est_emp.set_rv(rvx);
                        
                        UI::get(rvxc,set,"rvxc",UI::compulsory);
                        UI::get(rvyc,set,"rvyc",UI::compulsory);
                        
                }
                void to_setting (Setting &set) const {
                        BM::to_setting(set); //reads bm, yrv,rvc, bm_rv, etc...
                        UI::save(g,set,"g");
                        UI::save(h,set,"h");
                        UI::save(bmx,set,"bmx");
                        UI::save(bmy,set,"bmy");
                }
                void validate() {
                        BM::validate();
                        
                        dimy = h->dimension();
//                      bmx->set_yrv(rvx);
//                      bmy->
                        
                        est_emp.set_rv(rvx);
                        est_emp.set_dim(rvx._dsize());
                        est.validate();
                        //
                        //check dimensions
                        rvc = rvxc.subt(rvx.copy_t(-1));
                        rvc.add( rvyc);
                        rvc=rvc.subt(rvx);
                        
                        bdm_assert(g->dimension()==rvx._dsize(),"rvx is not described");
                        bdm_assert(g->dimensionc()==rvxc._dsize(),"rvxc is not described");
                        bdm_assert(h->dimensionc()==rvyc._dsize(),"rvyc is not described");
                        
                        bdm_assert(bmx->dimensiony()==g->dimension(),
                                           "Incompatible noise estimator of dimension " +
                                           num2str(bmx->dimensiony()) + " does not match dimension of g , " +
                                           num2str(g->dimension())
                                           );
                                           
                        dimc = rvc._dsize();
                        
                        //establish datalinks
                        x2g.set_connection(rvxc, rvx.copy_t(-1));
                        cond2g.set_connection(rvxc, rvc);
                        
                        x2h.set_connection(rvyc, rvx);
                        cond2h.set_connection(rvyc, rvc);
                }               
                
};
UIREGISTER(NoiseParticleXY);

class NoiseParticleXYprop: public NoiseParticleXY{
public:
        BM* _copy() const {
                return new NoiseParticleXYprop(*this);
        };
        void bayes(const vec &dt, const vec &cond) {
                int dim_x = bmx->dimensiony();
                int dim_y = bmy->dimensiony();
                
                //vec vt=pred_v->sample();
                mat SigV=mat(bmx->posterior().mean()._data(), dim_x, dim_x);
                mat SigW=mat(bmy->posterior().mean()._data(), dim_y, dim_y);
                
                vec &xtm=est_emp.point;
                vec g_args(g->dimensionc());
                x2g.filldown(xtm,g_args);
                cond2g.filldown(cond,g_args);
                vec xthat = g->eval(g_args);
                
                mat IC = eye(dim_x+dim_y);
                IC.set_submatrix(0,0,eye(dim_x));
                mat SigJo=eye(dim_x + dim_y);
                SigJo.set_submatrix(0,0,SigV);
                SigJo.set_submatrix(dim_x,dim_x, 10*SigW);
                
                diffbifn* hb=dynamic_cast<bilinfn*>(h.get());
                mat C=zeros(dim_y,dim_x);
                if (hb){
                        hb->dfdx_cond(xtm, cond, C,true);
                        IC.set_submatrix(dim_x,0,-C);
                }
                
                mat iIC=inv(IC);
                enorm<chmat> Jo;
                Jo._mu()=iIC*concat(xthat, zeros(dim_y));
                Jo._R()=iIC*SigJo*iIC.T();
                Jo.set_rv(concat(bmx->_yrv(), bmy->_yrv()));
                Jo.validate();
                shared_ptr<pdf> Cond=Jo.condition(bmx->_yrv());
                //new sample
                
                vec xt = Cond->samplecond(dt);//  + vt;
                
                est_emp.point=xt;
                
                // the vector [v_t] updates bm,
                bmx->bayes(xt-xthat);
                
                // residue of observation
                vec h_args(h->dimensionc());
                x2h.filldown(xt,h_args);
                cond2h.filldown(cond,h_args);
                
                vec z_y =h->eval(h_args)-dt;
                //                      ARX *abm = dynamic_cast<ARX*>(bmy.get());
                /*                      double ll2;
                 i f (abm){ //ARX*
                 shared_ptr<epdf> pr_y(abm->epredictor_student(empty_vec));
                 ll2=pr_y->evallog(z_y);
        } else{
                shared_ptr<epdf> pr_y(bmy->epredictor(empty_vec));
                ll2=pr_y->evallog(z_y);
        }*/
                
                bmy->bayes(z_y);
                // test _lls
                ll= bmy->_ll();
        }
};
UIREGISTER(NoiseParticleXYprop);

/*! Marginalized particle for state-space models with unknown parameters of residues distribution

\f{eqnarray*}{
    x_t &=& g(x_{t-1}) + v_t,\\
    z_t &= &h(x_{t-1}) + w_t,
    \f}

    This particle is a only a shell creating the residues calling internal estimator of their parameters. The internal estimator can be of any compatible type, e.g. ARX for Gaussian residues with unknown mean and variance.

    */
class NoiseParticle : public MarginalizedParticleBase {
protected:
    //! function transforming xt, ut -> x_t+1
    shared_ptr<fnc> g; // pdf for non-linear part
    //! function transforming xt,ut -> yt
    shared_ptr<fnc> h; // pdf for non-linear part

    RV rvx;
    RV rvxc;
    RV rvyc;

    //!link from condition to f
    datalink_part cond2g;
    //!link from condition to h
    datalink_part cond2h;
    //!link from xt to f
    datalink_part x2g;
    //!link from xt to h
    datalink_part x2h;

public:
    BM* _copy() const {
        return new NoiseParticle(*this);
    };
    void bayes(const vec &dt, const vec &cond) {
        shared_ptr<epdf> pred_vw=bm->epredictor();
        shared_ptr<epdf> pred_v = pred_vw->marginal(rvx);

        vec vt=pred_v->sample();

        //new sample
        vec &xtm=est_emp.point;
        vec g_args(g->dimensionc());
        x2g.filldown(xtm,g_args);
        cond2g.filldown(cond,g_args);
        vec xt = g->eval(g_args) + vt;
        est_emp.point=xt;

        // residue of observation
        vec h_args(h->dimensionc());
        x2h.filldown(xt,h_args);
        cond2h.filldown(cond,h_args);
        vec wt = dt-h->eval(h_args);
        // the vector [v_t,w_t] is now complete
        bm->bayes(concat(vt,wt));
        ll=bm->_ll();
    }
    void from_setting(const Setting &set) {
        MarginalizedParticleBase::from_setting(set); //reads bm, yrv,rvc, bm_rv, etc...

        UI::get(g,set,"g",UI::compulsory);
        UI::get(h,set,"h",UI::compulsory);
        UI::get(rvx,set,"rvx",UI::compulsory);
        est_emp.set_rv(rvx);

        UI::get(rvxc,set,"rvxc",UI::compulsory);
        UI::get(rvyc,set,"rvyc",UI::compulsory);

    }
    void validate() {
        MarginalizedParticleBase::validate();

        dimy = h->dimension();
        bm->set_yrv(concat(rvx,yrv));

        est_emp.set_rv(rvx);
        est_emp.set_dim(rvx._dsize());
        est.validate();
        //
        //check dimensions
        rvc = rvxc.subt(rvx.copy_t(-1));
        rvc.add( rvyc);
        rvc=rvc.subt(rvx);

        bdm_assert(g->dimension()==rvx._dsize(),"rvx is not described");
        bdm_assert(g->dimensionc()==rvxc._dsize(),"rvxc is not described");
        bdm_assert(h->dimension()==rvyc._dsize(),"rvyc is not described");

        bdm_assert(bm->dimensiony()==g->dimension()+h->dimension(),
                   "Incompatible noise estimator of dimension " +
                   num2str(bm->dimensiony()) + " does not match dimension of g and h, " +
                   num2str(g->dimension())+" and "+ num2str(h->dimension()) );

        dimc = rvc._dsize();

        //establish datalinks
        x2g.set_connection(rvxc, rvx.copy_t(-1));
        cond2g.set_connection(rvxc, rvc);

        x2h.set_connection(rvyc, rvx);
        cond2h.set_connection(rvyc, rvc);
    }
};
UIREGISTER(NoiseParticle);


}
#endif // KF_H