root/library/bdm/estim/particles.cpp @ 811

#include "particles.h"

namespace bdm {

using std::endl;

void PF::bayes_gensmp ( const vec &ut ) {
        if ( ut.length() > 0 ) {
                vec cond ( par->dimensionc() );
                cond.set_subvector ( par->dimension(), ut );
#pragma parallel for
                for ( int i = 0; i < n; i++ ) {
                        cond.set_subvector ( 0, _samples ( i ) );
                        _samples ( i ) = par->samplecond ( cond );
                        lls ( i ) = 0;
                }
        } else {
#pragma parallel for
                for ( int i = 0; i < n; i++ ) {
                        _samples ( i ) = par->samplecond ( _samples ( i ) );
                        lls ( i ) = 0;
                }
        }
}

void PF::bayes_weights() {
//
        double mlls = max ( lls );
        // compute weights
        for ( int  i = 0; i < n; i++ ) {
                _w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood
        }

        //renormalize
        double sw = sum ( _w );
        if ( !std::isfinite ( sw ) ) {
                for ( int i = 0; i < n; i++ ) {
                        if ( !std::isfinite ( _w ( i ) ) ) {
                                _w ( i ) = 0;
                        }
                }
                sw = sum ( _w );
                if ( !std::isfinite ( sw ) || sw == 0.0 ) {
                        bdm_error ( "Particle filter is lost; no particle is good enough." );
                }
        }
        _w /= sw;
}

void PF::bayes ( const vec &yt, const vec &cond ) {
        const vec &ut = cond; //todo

        int i;
        // generate samples - time step
        bayes_gensmp ( ut );
        // weight them - data step
        for ( i = 0; i < n; i++ ) {
                lls ( i ) += obs->evallogcond ( yt, _samples ( i ) ); //+= because lls may have something from gensmp!
        }

        bayes_weights();

        if ( do_resampling() ) {
                est.resample ( resmethod );
        }

}

// void PF::set_est ( const epdf &epdf0 ) {
//      int i;
//
//      for ( i=0;i<n;i++ ) {
//              _samples ( i ) = epdf0.sample();
//      }
// }

vec MPF::mpfepdf::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 MPF::mpfepdf::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 MPF::mpfepdf::qbounds ( vec &lb, vec &ub, double perc ) 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 );
}



void MPF::bayes ( const vec &yt, const vec &cond ) {
        // follows PF::bayes in most places!!!
        int i;
        int n = pf->__w().length();
        vec &lls = pf->_lls();
        Array<vec> &samples = pf->__samples();

        // generate samples - time step
        pf->bayes_gensmp ( vec ( 0 ) );
        // weight them - data step
#pragma parallel for
        for ( i = 0; i < n; i++ ) {
                vec bm_cond ( BMs ( i )->dimensionc() );
                this2bm.fill_cond ( yt, cond, bm_cond );
                pf2bm.filldown ( samples ( i ), bm_cond );
                BMs ( i ) -> bayes ( this2bm.pushdown ( yt ), bm_cond );
                lls ( i ) += BMs ( i )->_ll();
        }

        pf->bayes_weights();

        ivec ind;
        if ( pf->do_resampling() ) {
                pf->resample ( ind );

#pragma omp parallel for
                for ( i = 0; i < n; i++ ) {
                        if ( ind ( i ) != i ) {//replace the current Bm by a new one
                                delete BMs ( i );
                                BMs ( i ) = (BM*) BMs ( ind ( i ) )->_copy(); //copy constructor
                        }
                };
        }
};


void MPF_ARXg::bayes ( const vec &yt, const vec &cond ) {
        // follows PF::bayes in most places!!!
        int i;
        int n = pf->__w().length();
        vec &lls = pf->_lls();
        Array<vec> &samples = pf->__samples();
        
        // generate samples - time step
        for (int i =0;i<n; i++){
                mat M; chmat R;
                BMsp(i)->posterior().sample_mat(M,R);
                vec diff=R._Ch().T()*randn(samples(i).length());
                samples(i) = g->eval(samples(i)) + diff;
                ////////////
//              samples(i) = cond;
                /////////
                BMsp(i)->bayes(diff);
        }
        // weight them - data step
        enorm<ldmat> ooo;
        ooo.set_parameters(zeros(2),0.1*eye(2));
        ooo.validate();
        
        #pragma parallel for
        for ( i = 0; i < n; i++ ) {
                vec tmp=yt - h->eval(samples(i));
                BMso ( i ) -> bayes ( tmp );
                lls ( i ) += BMso ( i )->_ll();
                /////
                ooo._mu()=h->eval(samples(i));
                lls(i) = ooo.evallog_nn(yt);
        }
        
        pf->bayes_weights();
        
        ivec ind;
        if ( pf->do_resampling() ) {
                pf->resample ( ind );
                
                #pragma omp parallel for
                for ( i = 0; i < n; i++ ) {
                        if ( ind ( i ) != i ) {//replace the current Bm by a new one
                                delete BMso ( i );
                                BMso ( i ) = (ARX*) BMso ( ind ( i ) )->_copy(); //copy constructor
                                delete BMsp ( i );
                                BMsp ( i ) = (ARX*) BMsp ( ind ( i ) )->_copy(); //copy constructor
                        }
                };
        }
};

void MPF_ARXg::from_setting(const libconfig::Setting& set) {
        BM::from_setting(set);
        
        pf = new PF;
        // prior must be set before BM
        pf->prior_from_set ( set );
        pf->resmethod_from_set ( set );
        
        string opt;
        if ( UI::get ( opt, set, "options", UI::optional ) ) {
                set_options ( opt );
        }
        
        // read functions g and h
        g=UI::build<fnc>(set,"g");
        h=UI::build<fnc>(set,"h");
        
        set_dim( g->dimension());
        dimy = h->dimension();
        
        shared_ptr<ARX> arxo=UI::build<ARX>(set,"arxo");
        shared_ptr<ARX> arxp=UI::build<ARX>(set,"arxp");
        int n = pf->__samples().length();
        BMso.set_length(n);
        BMsp.set_length(n);
        for(int i=0; i<n; i++){
                BMso(i)=arxo->_copy();
                BMsp(i)=arxp->_copy();
        }
        
        yrv = arxo->_yrv();
        //rvc = arxo->_rvc();
        
        validate();
        
}


}
//MPF::MPF:{}