[8] | 1 | #include "libPF.h" |
---|
| 2 | |
---|
| 3 | using namespace itpp; |
---|
| 4 | |
---|
| 5 | using std::endl; |
---|
| 6 | |
---|
[11] | 7 | PF::PF(vec w0){w=w0/sum(w0); n=w.length();} |
---|
| 8 | |
---|
[8] | 9 | ivec PF::resample( RESAMPLING_METHOD method ) { |
---|
[11] | 10 | ivec ind=zeros_i( n ); |
---|
| 11 | ivec N_babies = zeros_i( n ); |
---|
| 12 | vec cumDist = cumsum( w ); |
---|
| 13 | vec u( n ); |
---|
| 14 | int i,j,parent; |
---|
| 15 | double u0; |
---|
| 16 | |
---|
| 17 | switch ( method ) { |
---|
| 18 | case MULTINOMIAL: |
---|
| 19 | u( n - 1 ) = pow( URNG.sample(), 1.0 / n ); |
---|
| 20 | for ( i = n - 2;i >= 0;i-- ) { |
---|
| 21 | u( i ) = u( i + 1 ) * pow( URNG.sample(), 1.0 / ( i + 1 ) ); |
---|
| 22 | } |
---|
| 23 | break; |
---|
| 24 | case STRATIFIED: |
---|
| 25 | for ( i = 0;i < n;i++ ) { |
---|
| 26 | u( i ) = ( i + URNG.sample() ) / n; |
---|
| 27 | } |
---|
| 28 | break; |
---|
| 29 | case SYSTEMATIC: |
---|
| 30 | u0 = URNG.sample(); |
---|
| 31 | for ( i = 0;i < n;i++ ) { |
---|
| 32 | u( i ) = ( i + u0 ) / n; |
---|
| 33 | } |
---|
| 34 | break; |
---|
| 35 | default: |
---|
| 36 | it_error( "PF::resample(): Unknown resampling method" ); |
---|
| 37 | } |
---|
| 38 | |
---|
| 39 | // // <<<<<<<<<<< |
---|
| 40 | // using std::cout; |
---|
| 41 | // cout << "resampling:" << endl; |
---|
| 42 | // cout << w <<endl; |
---|
| 43 | // cout << cumDist <<endl; |
---|
| 44 | // cout << u <<endl; |
---|
| 45 | |
---|
| 46 | // U is now full |
---|
| 47 | j = 0; |
---|
| 48 | for ( i = 0;i < n;i++ ) { |
---|
| 49 | while ( u( i ) > cumDist( j ) ) j++; |
---|
| 50 | N_babies( j )++; |
---|
| 51 | } |
---|
| 52 | |
---|
| 53 | // // <<<<<<<<<<< |
---|
| 54 | // cout << N_babies <<endl; |
---|
| 55 | |
---|
| 56 | // We have assigned new babies for each Particle |
---|
| 57 | // Now, we fill the resulting index such that: |
---|
| 58 | // * particles with at least one baby should not move * |
---|
| 59 | // This assures that reassignment can be done inplace; |
---|
| 60 | |
---|
| 61 | // find the first parent; |
---|
| 62 | parent=0; while(N_babies(parent)==0) parent++; |
---|
| 63 | // Build index |
---|
| 64 | for ( i = 0;i < n;i++ ) { |
---|
| 65 | if ( N_babies( i ) > 0 ){ |
---|
| 66 | ind( i ) = i; |
---|
| 67 | N_babies( i ) --; //this index was now replicated; |
---|
| 68 | } |
---|
| 69 | else { |
---|
| 70 | // test if the parent has been fully replicated |
---|
| 71 | // if yes, find the next one |
---|
| 72 | while((N_babies(parent)==0) || (N_babies(parent)==1 && parent>i) ) parent++; |
---|
| 73 | // Replicate parent |
---|
| 74 | ind (i) = parent; |
---|
| 75 | N_babies( parent ) --; //this index was now replicated; |
---|
| 76 | } |
---|
| 77 | /* // <<<<<<<<<<< |
---|
| 78 | cout <<ind << endl;*/ |
---|
| 79 | } |
---|
[8] | 80 | return ind; |
---|
| 81 | } |
---|
| 82 | |
---|
[28] | 83 | TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, BM &prop0, int n0 ) : PF(1/n*ones(n)) { |
---|
[8] | 84 | is_proposal = true; |
---|
| 85 | prop = &prop0; |
---|
| 86 | par = &par0; |
---|
| 87 | obs = &obs0; |
---|
| 88 | } |
---|
[11] | 89 | TrivialPF::TrivialPF( mpdf &par0, mpdf &obs0, int n0 ) : PF(1/n*ones(n)) { |
---|
[8] | 90 | is_proposal = false; |
---|
| 91 | par = &par0; |
---|
| 92 | obs = &obs0; |
---|
| 93 | } |
---|
| 94 | |
---|
[11] | 95 | void TrivialPF::bayes( const vec &dt , bool evalll ) { |
---|
[8] | 96 | int i; |
---|
| 97 | vec oldp; |
---|
[11] | 98 | double ll, gl, sum = 0.0; |
---|
[8] | 99 | Sort<double> S; |
---|
| 100 | ivec ind, iw; |
---|
[28] | 101 | |
---|
[8] | 102 | //generate new samples |
---|
| 103 | for ( i=0;i<n;i++ ) { |
---|
[28] | 104 | if(is_proposal) { |
---|
| 105 | epdf* prop_epdf; |
---|
| 106 | // prop_epdf = prop._epdf(); |
---|
| 107 | prop->bayes(dt); |
---|
| 108 | ptcls( i ) = prop_epdf->sample(); |
---|
| 109 | } |
---|
| 110 | // gl = prop_cond.eval( ptcls( i ) ); |
---|
[8] | 111 | |
---|
[28] | 112 | // obs.evalcond( ptcls( i ), &obs_cond ); |
---|
| 113 | // ll = obs_cond.eval( dt ); |
---|
[8] | 114 | w( i ) *= ll/gl; |
---|
| 115 | } |
---|
| 116 | //renormalize |
---|
| 117 | for ( i=0;i<n;i++ ){sum+=w( i );}; |
---|
| 118 | w( i ) /=sum; //? |
---|
[28] | 119 | |
---|
[8] | 120 | ind = resample(); |
---|
| 121 | iw = S.sort_index( 0,n-1,w ); // the first one in iw is the strongest |
---|
| 122 | |
---|
| 123 | for ( i=0;i<n;i++ ) { |
---|
| 124 | ptcls( i ) = ptcls( i ); //potentionally dangerous! |
---|
| 125 | } |
---|
[28] | 126 | |
---|
[8] | 127 | } |
---|