root/bdm/estim/libPF.cpp @ 14

#include "libPF.h"

using namespace itpp;

using std::endl;

PF::PF(vec w0){w=w0/sum(w0); n=w.length();}

ivec PF::resample( RESAMPLING_METHOD method ) {
        ivec ind=zeros_i( n );
        ivec N_babies = zeros_i( n );
        vec cumDist = cumsum( w );
        vec u( n );
        int i,j,parent;
        double u0;

        switch ( method ) {
        case MULTINOMIAL:
                u( n - 1 ) = pow( URNG.sample(), 1.0 / n );
                for ( i = n - 2;i >= 0;i-- ) {
                        u( i ) = u( i + 1 ) * pow( URNG.sample(), 1.0 / ( i + 1 ) );
                }
                break;
        case STRATIFIED:
                for ( i = 0;i < n;i++ ) {
                        u( i ) = ( i + URNG.sample() ) / n;
                }
                break;
        case SYSTEMATIC:
                u0 = URNG.sample();
                for ( i = 0;i < n;i++ ) {
                        u( i ) = ( i + u0 ) / n;
                }
                break;
        default:
                it_error( "PF::resample(): Unknown resampling method" );
        }
        
//      // <<<<<<<<<<<
//      using std::cout;
//      cout << "resampling:" << endl;
//      cout << w <<endl;
//      cout << cumDist <<endl;
//      cout << u <<endl;

        // U is now full
        j = 0;
        for ( i = 0;i < n;i++ ) {
                while ( u( i ) > cumDist( j ) ) j++;
                N_babies( j )++;
        }

//      // <<<<<<<<<<<
//      cout << N_babies <<endl;

        // We have assigned new babies for each Particle
        // Now, we fill the resulting index such that:
        // * particles with at least one baby should not move *
        // This assures that reassignment can be done inplace;

        // find the first parent;
        parent=0; while(N_babies(parent)==0) parent++;
        // Build index
        for ( i = 0;i < n;i++ ) {
                if ( N_babies( i ) > 0 ){
                        ind( i ) = i;
                        N_babies( i ) --; //this index was now replicated;
                        }
                else {
                        // test if the parent has been fully replicated 
                        // if yes, find the next one
                        while((N_babies(parent)==0) || (N_babies(parent)==1 && parent>i) ) parent++;
                        // Replicate parent
                        ind (i) = parent;
                        N_babies( parent ) --; //this index was now replicated;
                }
/*                      // <<<<<<<<<<<
                        cout <<ind << endl;*/
        }
        return ind;
}

TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, mpdf &prop0, int n0 ) : PF(1/n*ones(n)) {
        is_proposal = true;
        prop = &prop0;
        par = &par0;
        obs = &obs0;
}
TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0,  int n0 ) : PF(1/n*ones(n)) {
        is_proposal = false;
        par = &par0;
        obs = &obs0;
}

void TrivialPF::bayes( const vec &dt , bool evalll ) {
        int i;
        vec oldp;
        double ll, gl, sum = 0.0;
        Sort<double> S;
        ivec ind, iw;
        /*
        //generate new samples
        for ( i=0;i<n;i++ ) {
                prop->evalcond( ptcls( i ), &prop_cond );
                ptcls( i ) = prop_cond.sample();
                gl = prop_cond.eval( ptcls( i ) );

                obs.evalcond( ptcls( i ), &obs_cond );
                ll = obs_cond.eval( dt );
                w( i ) *= ll/gl;
        }
        //renormalize
        for ( i=0;i<n;i++ ){sum+=w( i );};
        w( i ) /=sum; //?
        //
        ind = resample();
        iw = S.sort_index( 0,n-1,w ); // the first one in iw is the strongest

        for ( i=0;i<n;i++ ) {
                ptcls( i ) = ptcls( i ); //potentionally dangerous!
        }
        */
}