vector<State*> Belief::Resample(int num, const vector<State*>& belief, const DSPOMDP* model, History history, int hstart) { double unit = 1.0 / num; double mass = Random::RANDOM.NextDouble(0, unit); int pos = 0; double cur = belief[0]->weight; double reward; OBS_TYPE obs; vector<State*> sample; int count = 0; double max_wgt = Globals::NEG_INFTY; int trial = 0; while (count < num && trial < 200 * num) { // Pick next particle while (mass > cur) { pos++; if (pos == belief.size()) pos = 0; cur += belief[pos]->weight; } trial++; mass += unit; State* particle = model->Copy(belief[pos]); // Step through history double log_wgt = 0; for (int i = hstart; i < history.Size(); i++) { model->Step(*particle, Random::RANDOM.NextDouble(), history.Action(i), reward, obs); double prob = model->ObsProb(history.Observation(i), *particle, history.Action(i)); if (prob > 0) { log_wgt += log(prob); } else { model->Free(particle); break; } } // Add to sample if survived if (particle->IsAllocated()) { count++; particle->weight = log_wgt; sample.push_back(particle); max_wgt = max(log_wgt, max_wgt); } // Remove particles with very small weights if (count == num) { for (int i = sample.size() - 1; i >= 0; i--) if (sample[i]->weight - max_wgt < log(1.0 / num)) { model->Free(sample[i]); sample.erase(sample.begin() + i); count--; } } } double total_weight = 0; for (int i = 0; i < sample.size(); i++) { sample[i]->weight = exp(sample[i]->weight - max_wgt); total_weight += sample[i]->weight; } for (int i = 0; i < sample.size(); i++) { sample[i]->weight = sample[i]->weight / total_weight; } logd << "[Belief::Resample] Resampled " << sample.size() << " particles" << endl; for (int i = 0; i < sample.size(); i++) { logv << " " << i << " = " << *sample[i] << endl; } return sample; }
vector<State*> Belief::Resample(int num, const Belief& belief, History history, int hstart) { double reward; OBS_TYPE obs; vector<State*> sample; int count = 0; int pos = 0; double max_wgt = Globals::NEG_INFTY; vector<State*> particles; int trial = 0; while (count < num || trial < 200 * num) { // Pick next particle if (pos == particles.size()) { particles = belief.Sample(num); pos = 0; } State* particle = particles[pos]; trial++; // Step through history double log_wgt = 0; for (int i = hstart; i < history.Size(); i++) { belief.model_->Step(*particle, Random::RANDOM.NextDouble(), history.Action(i), reward, obs); double prob = belief.model_->ObsProb(history.Observation(i), *particle, history.Action(i)); if (prob > 0) { log_wgt += log(prob); } else { belief.model_->Free(particle); break; } } // Add to sample if survived if (particle->IsAllocated()) { particle->weight = log_wgt; sample.push_back(particle); max_wgt = max(log_wgt, max_wgt); count++; } // Remove particles with very small weights if (count == num) { for (int i = sample.size() - 1; i >= 0; i--) { if (sample[i]->weight - max_wgt < log(1.0 / num)) { belief.model_->Free(sample[i]); sample.erase(sample.begin() + i); count--; } } } pos++; } // Free unused particles for (int i = pos; i < particles.size(); i++) belief.model_->Free(particles[i]); double total_weight = 0; for (int i = 0; i < sample.size(); i++) { sample[i]->weight = exp(sample[i]->weight - max_wgt); total_weight += sample[i]->weight; } for (int i = 0; i < sample.size(); i++) { sample[i]->weight = sample[i]->weight / total_weight; } logd << "[Belief::Resample] Resampled " << sample.size() << " particles" << endl; for (int i = 0; i < sample.size(); i++) { logv << " " << i << " = " << *sample[i] << endl; } return sample; }