1 #ifndef PARTICLEFILTER_H 2 #define PARTICLEFILTER_H 24 template <
class OutputIterator,
class Iterator>
25 double toNormalForm(OutputIterator& out,
const Iterator & begin,
const Iterator & end){
28 for (Iterator it=begin; it!=end; it++){
29 lmax=lmax>((double)(*it))? lmax: (
double)(*it);
32 for (Iterator it=begin; it!=end; it++){
33 *out=exp((
double)(*it)-lmax);
39 template <
class OutputIterator,
class Iterator,
class Numeric>
40 void toLogForm(OutputIterator& out,
const Iterator & begin,
const Iterator & end, Numeric lmax){
42 for (Iterator it=begin; it!=end; it++){
43 *out=log((Numeric)(*it))-lmax;
49 template <
class WeightVector>
50 void resample(std::vector<int>& indexes,
const WeightVector& weights,
unsigned int nparticles=0){
55 for (
typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
64 double interval=cweight/
n;
67 double target=interval*::drand48();
75 for (
typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
76 cweight+=(double)* it;
77 while(cweight>target){
84 template <
typename Vector>
85 void repeatIndexes(Vector& dest,
const std::vector<int>& indexes,
const Vector& particles){
86 assert(indexes.size()==particles.size());
87 dest.resize(particles.size());
89 for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
90 dest[i]=particles[*it];
96 template <
class Iterator>
97 double neff(
const Iterator& begin,
const Iterator& end){
99 for (Iterator it=begin; it!=end; ++it){
103 for (Iterator it=begin; it!=end; ++it){
110 template <
class Iterator>
111 void normalize(
const Iterator& begin,
const Iterator& end){
113 for (Iterator it=begin; it!=end; ++it){
116 for (Iterator it=begin; it!=end; ++it){
121 template <
class OutputIterator,
class Iterator>
122 void rle(OutputIterator& out,
const Iterator & begin,
const Iterator & end){
123 unsigned int current=0;
124 unsigned int count=0;
125 for (Iterator it=begin; it!=end; it++){
131 if (((uint)*it) ==current)
133 if (((uint)*it)!=current){
134 *out=std::make_pair(current,count);
141 *out=std::make_pair(current,count);
146 template <
class Particle,
class Numeric>
148 std::vector<unsigned int>
resampleIndexes(
const std::vector<Particle> & particles,
int nparticles=0)
const;
149 std::vector<Particle>
resample(
const std::vector<Particle> & particles,
int nparticles=0)
const;
150 Numeric
neff(
const std::vector<Particle> & particles)
const;
154 template <
class Particle,
class Numeric>
160 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
161 cweight+=(Numeric)*it;
169 Numeric interval=cweight/
n;
172 Numeric target=interval*::drand48();
176 std::vector<unsigned int> indexes(n);
179 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
180 cweight+=(Numeric)* it;
181 while(cweight>target){
189 template <
class Particle,
class Numeric>
191 (
const typename std::vector<Particle>& particles,
int nparticles)
const{
196 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
197 cweight+=(Numeric)*it;
208 Numeric interval=cweight/
n;
211 Numeric target=interval*::drand48();
215 std::vector<Particle> resampled;
218 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
219 cweight+=(Numeric)*it;
220 while(cweight>target){
221 resampled.push_back(*it);
222 resampled.back().setWeight(uw);
229 template <
class Particle,
class Numeric>
233 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
234 Numeric w=(Numeric)*it;
261 template <
class Particle,
class EvolutionModel>
264 void evolve(std::vector<Particle>& particles);
265 void evolve(std::vector<Particle>& dest,
const std::vector<Particle>& src);
268 template <
class Particle,
class EvolutionModel>
270 for (
typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
271 *it=evolutionModel.evolve(*it);
275 template <
class Particle,
class EvolutionModel>
278 for (
typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
279 dest.push_back(evolutionModel.evolve(*it));
283 template <
class Particle,
class Numeric,
class QualificationModel,
class EvolutionModel,
class LikelyhoodModel>
288 void evolve(std::vector<Particle>& particles);
289 void evolve(std::vector<Particle>& dest,
const std::vector<Particle>& src);
292 template <
class Particle,
class Numeric,
class QualificationModel,
class EvolutionModel,
class LikelyhoodModel>
294 (std::vector<Particle>&particles){
295 std::vector<Numeric> observationWeights(particles.size());
297 for (
typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
298 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
301 std::vector<unsigned int> indexes(resampler.
resampleIndexes(observationWeights));
302 for (
typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
304 particle=evolutionModel.evolve(particle);
305 particle.
setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
309 template <
class Particle,
class Numeric,
class QualificationModel,
class EvolutionModel,
class LikelyhoodModel>
311 (std::vector<Particle>& dest,
const std::vector<Particle>& src){
313 std::vector<Numeric> observationWeights(src.size());
315 for (
typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
316 observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
319 std::vector<unsigned int> indexes(resampler.
resampleIndexes(observationWeights));
320 for (
typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
322 dest.push_back(evolutionModel.evolve(particle));
323 dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it];
void normalize(const Iterator &begin, const Iterator &end)
QualificationModel qualificationModel
void repeatIndexes(Vector &dest, const std::vector< int > &indexes, const Vector &particles)
void evolve(std::vector< Particle > &particles)
double toNormalForm(OutputIterator &out, const Iterator &begin, const Iterator &end)
EvolutionModel evolutionModel
std::pair< uint, uint > UIntPair
void setWeight(double _w)
void rle(OutputIterator &out, const Iterator &begin, const Iterator &end)
EvolutionModel evolutionModel
double neff(const Iterator &begin, const Iterator &end)
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
LikelyhoodModel likelyhoodModel
void toLogForm(OutputIterator &out, const Iterator &begin, const Iterator &end, Numeric lmax)
void evolve(std::vector< Particle > &particles)
double max(double a, double b)