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