/**
* normalize the weights for all particles
*/
virtual void normalizeParticles() {
double sum = 0.0;
double neff = 0.0;
base::Position mean_pos = base::Position::Zero();
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
mean_pos += position(*it);
sum += confidence(*it);
}
if(sum == 0.0 && particles.size() != 0){
sum = particles.size();
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
setConfidence(*it, 0.0);
neff += confidence(*it) * confidence(*it);
}
}
else{
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
setConfidence(*it, confidence(*it) / sum);
neff += confidence(*it) * confidence(*it);
}
}
mean_position = mean_pos / particles.size();
effective_sample_size = (1.0 / neff) / particles.size();
changed_particles = true;
}
double observe(const Z& z, M& m, double ratio = 1.0)
{
Perception<P, Z, M>* model = dynamic_cast<Perception<P, Z, M>*>(this);
std::vector<double> perception_weights;
perception_weights.reserve(particles.size());
double sum_perception_weight = 0.0;
double sum_main_confidence = 0.0;
double Neff = 0.0;
unsigned i;
changed_particles = true;
// calculate all perceptions
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
perception_weights.push_back(model->perception(*it, z, m));
sum_perception_weight += perception_weights.back();
}
if(sum_perception_weight <= 0.0)
return 0.0;
i = 0;
// normalize perception weights and form mixed weight based on current weight.
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double uniform = 1.0 / particles.size();
double w = 1.0 - ratio;
double main_confidence = confidence(*it) * ((ratio * (perception_weights[i] / sum_perception_weight))
+ w * uniform);
setConfidence(*it, main_confidence);
sum_main_confidence += main_confidence;
i++;
}
// normalize overall confidence
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double weight;
if(sum_main_confidence == 0.0)
weight = 0.0;
else
weight = confidence(*it) / sum_main_confidence;
setConfidence(*it, weight);
Neff += weight * weight;
}
if(Neff == 0.0)
effective_sample_size = 1.0;
else
effective_sample_size = (1.0 / Neff) / particles.size();
return effective_sample_size;
}
void teleportParticles(const base::samples::RigidBodyState& pose)
{
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
position(*it) = pose.position;
setConfidence(*it, 1.0 / particles.size() );
}
}
QCharsetMatch::QCharsetMatch(const QString name, const QString language, const qint32 confidence)
: d_ptr(new QCharsetMatchPrivate)
{
Q_D(QCharsetMatch);
d->q_ptr = this;
setName(name);
setLanguage(language);
setConfidence(confidence);
}
double observe_markov(const Z& z, M& m, double ratio = 1.0)
{
Perception<P, Z, M>* model = dynamic_cast<Perception<P, Z, M>*>(this);
double Neff = 0.0;
double weight;
double sum = 0.0;
// calculate all perceptions
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double conf = model->perception(*it, z, m);
setConfidence(*it, conf);
sum += conf;
}
// normalize overall confidence
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
if(sum == 0.0)
weight = 1 / particles.size();
else
weight = confidence(*it) / sum;
setConfidence(*it, weight);
Neff += weight * weight;
}
if(Neff == 0.0)
effective_sample_size = 1.0;
else
effective_sample_size = (1.0 / Neff) / particles.size();
changed_particles = true;
return effective_sample_size;
}
