Changeset 638 for library/bdm/estim/particles.cpp

Timestamp:

09/27/09 00:58:04 (15 years ago)

Author:

smidl

Message:

redesign of PF and MPF to be more flexible and share more code

Files:

• library/bdm/estim/particles.cpp (modified) (2 diffs)

library/bdm/estim/particles.cpp

@@ -5 +5 @@
 using std::endl;
 
-void PF::bayes ( const vec &dt ) {
-        int i;
-        vec lls ( n );
-        ivec ind;
-        double mlls = -std::numeric_limits<double>::infinity(), sum = 0.0;
+void PF::bayes_gensmp(){
+        #pragma parallel for
+        for (int i = 0; i < n; i++ ) {
+                _samples ( i ) = par->samplecond ( _samples ( i ) );
+                lls(i) = 0;
+        }
+}
 
-        for ( i = 0; i < n; i++ ) {
-                //generate new samples from paramater evolution model;
-                vec old_smp=_samples ( i );
-                _samples ( i ) = par->samplecond ( old_smp );
-                lls ( i ) = par->evallogcond ( _samples ( i ), old_smp );
-                lls ( i ) *= obs->evallogcond ( dt, _samples ( i ) );
-
-                if ( lls ( i ) > mlls ) mlls = lls ( i ); //find maximum
-        }
-
+void PF::bayes_weights(){
+// 
+        double mlls=max(lls);
         // compute weights
-        for ( i = 0; i < n; i++ ) {
+        for (int  i = 0; i < n; i++ ) {
                 _w ( i ) *= exp ( lls ( i ) - mlls ); // multiply w by likelihood
         }
 
         //renormalize
+        double sw=sum(_w);
+        if (!isfinite(sw)) {
+                for (int i=0;i<n;i++){
+                        if (!isfinite(_w(i))) {_w(i)=0;}
+                }
+                sw = sum(_w);
+                if (!isfinite(sw)) {
+                        bdm_error("Particle filter is lost; no particle is good enough.");
+                }
+        }
+        _w /= sw;
+}
+
+void PF::bayes ( const vec &dt ) {
+        int i;
+        // generate samples - time step
+        bayes_gensmp();
+        // weight them - data step
         for ( i = 0; i < n; i++ ) {
-                sum += _w ( i );
-        };
+                lls ( i ) += obs->evallogcond ( dt, _samples ( i ) ); //+= because lls may have something from gensmp!
+        }
 
-        _w ( i ) /= sum; //?
+        bayes_weights();
 
-        ind = est.resample ( resmethod );
+        if (do_resampling()) {
+                est.resample ( resmethod);
+        }
 
 }
@@ -45 +60 @@
 // }
 
+void MPF::bayes ( const vec &dt ) {     
+        // follows PF::bayes in most places!!!
+        
+        int i;
+        int n=pf->__w().length();
+        vec &lls = pf->_lls();
+        
+        // generate samples - time step
+        pf->bayes_gensmp();
+        // weight them - data step
+        #pragma parallel for
+        for ( i = 0; i < n; i++ ) {
+                BMs(i) -> condition(pf->posterior()._sample(i));
+                BMs(i) -> bayes(dt);
+                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 ) = BMs ( ind ( i ) )->_copy_(); //copy constructor
+                        }
+                };
+        }
+};
+
 
 }