1 #ifndef PARTICLEFILTER_H
2 #define PARTICLEFILTER_H
22 template <
class OutputIterator,
class Iterator>
23 double toNormalForm(OutputIterator& out,
const Iterator & begin,
const Iterator & end){
25 double lmax=-MAXDOUBLE;
26 for (Iterator it=begin; it!=end; it++){
27 lmax=lmax>((double)(*it))? lmax: (
double)(*it);
30 for (Iterator it=begin; it!=end; it++){
31 *out=exp((
double)(*it)-lmax);
37 template <
class OutputIterator,
class Iterator,
class Numeric>
38 void toLogForm(OutputIterator& out,
const Iterator & begin,
const Iterator & end, Numeric lmax){
40 for (Iterator it=begin; it!=end; it++){
41 *out=log((Numeric)(*it))-lmax;
47 template <
class WeightVector>
48 void resample(std::vector<int>& indexes,
const WeightVector& weights,
unsigned int nparticles=0){
53 for (
typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
62 double interval=cweight/
n;
65 double target=interval*::drand48();
73 for (
typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
74 cweight+=(double)* it;
75 while(cweight>target){
82 template <
typename WeightVector>
84 double wmin=MAXDOUBLE;
85 double wmax=-MAXDOUBLE;
86 for (uint i=0; i<size; i++){
87 wmin=wmin<weights[i]?wmin:weights[i];
88 wmax=wmax>weights[i]?wmax:weights[i];
90 double min_normalized_value=log(minWeight);
91 double max_normalized_value=log(1.);
92 double dn=max_normalized_value-min_normalized_value;
96 double offset=-wmax*scale;
97 for (uint i=0; i<size; i++){
104 template <
typename Vector>
105 void repeatIndexes(Vector& dest,
const std::vector<int>& indexes,
const Vector& particles){
113 dest.resize(indexes.size());
116 for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
117 dest[i]=particles[*it];
122 template <
typename Vector>
123 void repeatIndexes(Vector& dest,
const std::vector<int>& indexes2,
const Vector& particles,
const std::vector<int>& indexes){
127 for (std::vector<int>::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){
128 dest[indexes[i]]=particles[*it];
134 template <
class Iterator>
135 double neff(
const Iterator& begin,
const Iterator& end){
137 for (Iterator it=begin; it!=end; ++it){
141 for (Iterator it=begin; it!=end; ++it){
150 template <
class OutputIterator,
class Iterator>
151 void rle(OutputIterator& out,
const Iterator & begin,
const Iterator & end){
152 unsigned int current=0;
153 unsigned int count=0;
154 for (Iterator it=begin; it!=end; it++){
160 if (((uint)*it) ==current)
162 if (((uint)*it)!=current){
163 *out=std::make_pair(current,count);
170 *out=std::make_pair(current,count);