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;
}
