From 95a5c8f34fd0af09e7f5ac4ed50b75100d53d4da Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 15 Nov 2017 17:46:06 +0100 Subject: [PATCH] #39 #40 git add for last commit --- math/random/DrawList.h | 169 +++++++++++ math/random/DrawWheel.h | 174 +++++++++++ math/random/Normal.h | 27 ++ math/random/RandomGenerator.h | 29 ++ math/random/RandomIterator.h | 118 ++++++++ math/random/Uniform.h | 43 +++ math/random/Unique.h | 26 ++ misc/Binning.h | 114 +++++++ smc/Particle.h | 54 ++++ smc/ParticleAssertions.h | 69 +++++ smc/filtering/ParticleFilter.h | 256 ++++++++++++++++ smc/filtering/ParticleFilterEvaluation.h | 29 ++ smc/filtering/ParticleFilterHistory.h | 283 ++++++++++++++++++ smc/filtering/ParticleFilterInitializer.h | 25 ++ smc/filtering/ParticleFilterMixing.h | 241 +++++++++++++++ smc/filtering/ParticleFilterTransition.h | 26 ++ .../estimation/ParticleFilterEstimation.h | 24 ++ .../ParticleFilterEstimationKernelDensity.h | 160 ++++++++++ .../estimation/ParticleFilterEstimationMax.h | 37 +++ ...leFilterEstimationOrderedWeightedAverage.h | 78 +++++ ...eFilterEstimationRegionalWeightedAverage.h | 60 ++++ .../ParticleFilterEstimationWeightedAverage.h | 52 ++++ ...FilterEstimationWeightedAverageWithAngle.h | 66 ++++ .../resampling/ParticleFilterResampling.h | 43 +++ .../ParticleFilterResamplingDivergence.h | 157 ++++++++++ .../resampling/ParticleFilterResamplingKLD.h | 130 ++++++++ .../resampling/ParticleFilterResamplingLog.h | 31 ++ .../resampling/ParticleFilterResamplingNEff.h | 163 ++++++++++ .../resampling/ParticleFilterResamplingNone.h | 34 +++ .../ParticleFilterResamplingPercent.h | 94 ++++++ .../ParticleFilterResamplingSimple.h | 102 +++++++ .../ParticleFilterResamplingWheel.h | 95 ++++++ .../InteractingMultipleModelParticleFilter.h | 135 +++++++++ smc/merging/MarkovTransitionProbability.h | 30 ++ smc/merging/estimation/JointEstimation.h | 25 ++ .../estimation/JointEstimationPosteriorOnly.h | 28 ++ smc/merging/mixing/MixingSampler.h | 29 ++ smc/merging/mixing/MixingSamplerDivergency.h | 157 ++++++++++ smc/sampling/CumulativeSampler.h | 100 +++++++ smc/sampling/ParticleTrajectorieSampler.h | 31 ++ smc/smoothing/ArtificialDistribution.h | 24 ++ smc/smoothing/BackwardFilter.h | 57 ++++ smc/smoothing/BackwardFilterTransition.h | 33 ++ smc/smoothing/BackwardSimulation.h | 258 ++++++++++++++++ smc/smoothing/CondensationBackwardFilter.h | 226 ++++++++++++++ smc/smoothing/ForwardFilterHistory.h | 142 +++++++++ smc/smoothing/TwoFilterSmoothing.h | 187 ++++++++++++ tests/smc/filtering/TestParticles.cpp | 44 +++ .../mixing/TestMixingSamplerDivergency.cpp | 146 +++++++++ 49 files changed, 4661 insertions(+) create mode 100644 math/random/DrawList.h create mode 100644 math/random/DrawWheel.h create mode 100644 math/random/Normal.h create mode 100644 math/random/RandomGenerator.h create mode 100644 math/random/RandomIterator.h create mode 100644 math/random/Uniform.h create mode 100644 math/random/Unique.h create mode 100644 misc/Binning.h create mode 100644 smc/Particle.h create mode 100644 smc/ParticleAssertions.h create mode 100644 smc/filtering/ParticleFilter.h create mode 100644 smc/filtering/ParticleFilterEvaluation.h create mode 100644 smc/filtering/ParticleFilterHistory.h create mode 100644 smc/filtering/ParticleFilterInitializer.h create mode 100644 smc/filtering/ParticleFilterMixing.h create mode 100644 smc/filtering/ParticleFilterTransition.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimation.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationMax.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h create mode 100644 smc/filtering/resampling/ParticleFilterResampling.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingDivergence.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingKLD.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingLog.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingNEff.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingNone.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingPercent.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingSimple.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingWheel.h create mode 100644 smc/merging/InteractingMultipleModelParticleFilter.h create mode 100644 smc/merging/MarkovTransitionProbability.h create mode 100644 smc/merging/estimation/JointEstimation.h create mode 100644 smc/merging/estimation/JointEstimationPosteriorOnly.h create mode 100644 smc/merging/mixing/MixingSampler.h create mode 100644 smc/merging/mixing/MixingSamplerDivergency.h create mode 100644 smc/sampling/CumulativeSampler.h create mode 100644 smc/sampling/ParticleTrajectorieSampler.h create mode 100644 smc/smoothing/ArtificialDistribution.h create mode 100644 smc/smoothing/BackwardFilter.h create mode 100644 smc/smoothing/BackwardFilterTransition.h create mode 100644 smc/smoothing/BackwardSimulation.h create mode 100644 smc/smoothing/CondensationBackwardFilter.h create mode 100644 smc/smoothing/ForwardFilterHistory.h create mode 100644 smc/smoothing/TwoFilterSmoothing.h create mode 100644 tests/smc/filtering/TestParticles.cpp create mode 100644 tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp diff --git a/math/random/DrawList.h b/math/random/DrawList.h new file mode 100644 index 0000000..4d252c1 --- /dev/null +++ b/math/random/DrawList.h @@ -0,0 +1,169 @@ +#ifndef K_MATH_RANDOM_DRAWLIST_H +#define K_MATH_RANDOM_DRAWLIST_H + +#include + +/** + * add entries to a list and be able to draw from them depending oh their probability + */ +namespace Random { + + /** + * this class represents one entry within a DrawList. + * such an entry consists of userData denoted by the template argument + * and a probability. + */ + template struct DrawListEntry { + + template friend class DrawList; + friend class DrawList_Cumulative_Test; + + public: + + /** the user data behind this entry */ + Entry entry; + + /** this entry's probability */ + double probability; + + private: + + /** the cumulative probability, tracked among all entries within the DrawList */ + double cumulativeProbability; + + public: + + /** empty ctor */ + DrawListEntry() : + entry(), probability(0), cumulativeProbability(0) {;} + + /** ctor */ + DrawListEntry(const Entry& entry, double probability) : + entry(entry), probability(probability), cumulativeProbability(0) {;} + + /** compare this entrie's summed probability with the given probability */ + bool operator < (double probability) const {return cumulativeProbability < probability;} + + private: + + /** ctor */ + DrawListEntry(Entry& e, double probability, double summedProbability) : + entry(entry), probability(probability), cumulativeProbability(summedProbability) {;} + + + }; + + + + /** + * a DrawList is a data-structure containing entries that have a + * probability assigned to them. + * using the draw() function one may draw from these entries according + * to their assigned probability in O(log(n)) + */ + template class DrawList { + + friend class DrawList_Cumulative_Test; + + public: + + /** ctor */ + DrawList() : sumValid(false) {;} + + + /** append a new entry to the end of the list */ + void push_back(const Entry& entry, const double probability) { + const DrawListEntry dle(entry, probability); + entries.push_back(dle); + sumValid = false; + } + + /** change the entry at the given position. ensure the vector is resized()! */ + void set(const uint32_t idx, const Entry& entry, const double probability) { + entries[idx].entry = entry; + entries[idx].probability = probability; + sumValid = false; + } + + /** resize the underlying vector to hold the given number of entries */ + void resize(const uint32_t numEntries) {entries.resize(numEntries);} + + /** clear all currently inserted entries */ + void clear() {entries.clear();} + + /** does the underlying vector contain any entries? */ + bool empty() const {return entries.empty();} + + /** the number of entries */ + uint32_t size() const {return entries.size();} + + /** draw a random entry from the draw list */ + Entry& draw() { + + // random value between [0, 1] + double rand01 = double(rand()) / double(RAND_MAX); + + return draw(rand01); + + } + + /** draw an entry according to the given probability [0,1] */ + Entry& draw(double rand01) { + + // sanity check + assert(!entries.empty()); + + ensureCumulativeProbability(); + + // random value between [0, summedProbability] + // (this prevents us from norming the list to [0, 1]) + double rand = rand01 * entries[entries.size()-1].cumulativeProbability; + + // binary search for the matching entry O(log(n)) + auto tmp = std::lower_bound(entries.begin(), entries.end(), rand); + return (*tmp).entry; + +// // O(n) +// for (DrawListEntry& dle : entries) { +// if (dle.cumulativeProbability > rand) {return dle.entry;} +// } +// return entries[this->size()-1].entry; + + } + + private: + + /** ensure the cumulative probability is valid. if not -> calculate it */ + void ensureCumulativeProbability() { + + // already valid? + if (sumValid) {return;} + + // calculate the cumulative probability + double sum = 0; + for (DrawListEntry& dle : entries) { + sum += dle.probability; + dle.cumulativeProbability = sum; + } + + // the sum is now valid + sumValid = true; + + } + + private: + + /** all entries within the DrawList */ + std::vector> entries; + + /** track wether the summedProbability is valid or not */ + bool sumValid; + + }; + + + + +} + +#endif // K_MATH_RANDOM_DRAWLIST_H diff --git a/math/random/DrawWheel.h b/math/random/DrawWheel.h new file mode 100644 index 0000000..6e9bbf5 --- /dev/null +++ b/math/random/DrawWheel.h @@ -0,0 +1,174 @@ +#ifndef K_MATH_RANDOM_DRAWLIST_H +#define K_MATH_RANDOM_DRAWLIST_H + +#include +#include "../distribution/Uniform.h" + +/** + * add entries to a list and be able to draw from them depending oh their probability + * + * souces: + * https://www.udacity.com/course/viewer#!/c-cs373/l-48704330/e-48748082/m-48740082 + * https://www.youtube.com/watch?list=PLpUPoM7Rgzi_7YWn14Va2FODh7LzADBSm&feature=player_detailpage&v=kZhOJgooMxI#t=567 + */ +namespace Random { + + /** + * this class represents one entry within a DrawWheel. + * such an entry consists of userData denoted by the template argument + * and a probability. + */ + template struct DrawWheelEntry { + + template friend class DrawWheel; + + public: + + /** the user data behind this entry */ + Entry entry; + + /** this entry's probability */ + double probability; + + public: + + /** empty ctor */ + DrawWheelEntry() : entry(), probability(0) {;} + + /** ctor */ + DrawWheelEntry(const Entry& entry, double probability) : entry(entry), probability(probability) {;} + + }; + + /** + * a DrawWheel is a data-structure containing entries that have a + * probability assigned to them. + * using the draw() function one may draw from these entries according + * to their assigned probability in ~O(n) + */ + template class DrawWheel { + + private: + + /** all entries within the DrawWheel */ + std::vector> entries; + + /** is the current maximum valid? */ + bool maxValid = true; + + /** the maximum weight among all entries */ + double curMax = 0; + + /** the current index within the wheel */ + int curIdx = 0; + + /** the current offset at the wheel's index */ + double curOffset = 0; + + /** draw random numbers for the offset */ + K::UniformDistribution dist; + + + public: + + /** ctor */ + DrawWheel() {;} + + + /** append a new entry to the end of the list */ + void push_back(const Entry& entry, const double probability) { + entries.push_back( DrawWheelEntry(entry, probability) ); + if (curMax < probability) {curMax = probability;} + } + + /** change the entry at the given position. ensure the vector is resized()! */ + void set(const uint32_t idx, const Entry& entry, const double probability) { + entries[idx].entry = entry; + entries[idx].probability = probability; + maxValid = false; + } + + /** resize the underlying vector to hold the given number of entries */ + void resize(const uint32_t numEntries) {entries.resize(numEntries);} + + /** clear all currently inserted entries */ + void clear() {entries.clear();} + + /** does the underlying vector contain any entries? */ + bool empty() const {return entries.empty();} + + /** the number of entries */ + uint32_t size() const {return entries.size();} + + + /** call this once before drawing anything */ + void init() { + + // ensure the maximum number is correct + ensureMaxProbability(); + + // setup the distribution to draw a new offset + dist.reset(0, 2 * curMax); + + // draw starting values + curIdx = K::UniformDistribution::draw( (int)0, (int)entries.size() - 1); + curOffset = dist.draw(); + + } + + /** draw a random entry from the wheel */ + Entry& draw() { + + while(true) { + + // found a suitable particle? use it and draw the next random number + if (entries[curIdx].probability >= curOffset) { + + // next offset + curOffset += dist.draw(); + + // return + return entries[curIdx].entry; + + // weight to small, subtract the elements weight and move on to the next element + } else { + + // remove the current entries probability + curOffset -= entries[curIdx].probability; + + // resume with the next one along the wheel + curIdx = (curIdx + 1) % ((int)entries.size()); + + } + + } + + } + + + + private: + + /** ensure the max probability is valid. if not -> calculate it */ + void ensureMaxProbability() { + + // valid? + if (maxValid) {return;} + + // comparisen + const auto lambda = [] (const DrawWheelEntry& e1, const DrawWheelEntry& e2) {return e1.probability < e2.probability;}; + + // find the largest entry + const DrawWheelEntry max = *std::max_element(entries.begin(), entries.end(), lambda); + this->curMax = max.probability; + + // the max is now valid + maxValid = true; + + } + + }; + +} + +#endif // K_MATH_RANDOM_DRAWLIST_H diff --git a/math/random/Normal.h b/math/random/Normal.h new file mode 100644 index 0000000..f5e09bd --- /dev/null +++ b/math/random/Normal.h @@ -0,0 +1,27 @@ +#ifndef K_MATH_RND_NORMAL_H +#define K_MATH_RND_NORMAL_H + +namespace Random { + + + /** + * provides some common functions + * for handling normal-distributed random numbers + */ + class Normal { + + public: + + /** get normal-distributed random number for given mu/sigma */ + static double get(double mu, double sigma) { + static std::random_device rd; + static std::mt19937 gen(rd()); + std::normal_distribution<> d(mu, sigma); + return d(gen); + } + + }; + +} + +#endif // K_MATH_RND_NORMAL_H diff --git a/math/random/RandomGenerator.h b/math/random/RandomGenerator.h new file mode 100644 index 0000000..4c41f36 --- /dev/null +++ b/math/random/RandomGenerator.h @@ -0,0 +1,29 @@ +#ifndef RANDOM_Random::RandomGenerator_H +#define RANDOM_Random::RandomGenerator_H + +#include +#include + +#ifdef USE_FIXED_SEED + #define RANDOM_SEED 1234 +#else + #define RANDOM_SEED ( std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1) ) +#endif + +namespace Random { + + class RandomGenerator : public std::minstd_rand { + + public: + + /** ctor with default seed */ + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + + /** ctor with custom seed */ + RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;} + + }; + +} + +#endif // K_MATH_RANDOM_Random::RandomGenerator_H diff --git a/math/random/RandomIterator.h b/math/random/RandomIterator.h new file mode 100644 index 0000000..b37b3e3 --- /dev/null +++ b/math/random/RandomIterator.h @@ -0,0 +1,118 @@ +#ifndef RANDOMITERATOR_H +#define RANDOMITERATOR_H + +#include +#include +#include "../../Assertions.h" + +namespace Random { + + template class RandomIterator { + + private: + + /** the user's data-vector to randomly iterate */ + const std::vector& vec; + + /** the number of random indices */ + int cnt; + + /** the random number generator */ + std::minstd_rand gen; + bool isRandomized = false; + + /** X random indices */ + std::vector indices; + + + public: + + /** ctor */ + RandomIterator(const std::vector& vec, const int cnt) : vec(vec), cnt(cnt) { + + //const uint64_t ts = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + static int seed = 0; ++seed; + gen.seed(seed); + + // sanity check + if ((int)vec.size() < cnt) {throw Exception("number of elements in list is smaller than the requested number to draw");} + if (cnt == 0) {throw Exception("number of elements in list must be at least 1");} + if (vec.empty()) {throw Exception("empty input vector given");} + + indices.resize(cnt); + + } + + /** create random samples (vector-indicies) that are hereafter available for iteration */ + void randomize() { + + // random-number generator between [0:size-1] + std::uniform_int_distribution dist(0, (int) vec.size()-1); + + // ensure we use each vector-index only ONCE + bool used[vec.size()] = {false}; + + // draw X random samples + for (int i = 0; i < cnt; ) { + const int rnd = dist(gen); + if (used[rnd]) {continue;} // already used? try again! + used[rnd] = true; // mark as used + indices[i] = rnd; // add to the index + ++i; + } + + // the vector is setup correctly + isRandomized = true; + + } + + /** the iterator state */ + struct Iterator { + + /** the current position within "indicies" */ + int pos; + + /** the vector with the user-data to randomly iterate */ + const std::vector& vec; + + /** the vector containing the random indices */ + const std::vector& indices; + + /** ctor */ + Iterator(const int pos, const std::vector& vec, const std::vector& indices) : pos(pos), vec(vec), indices(indices) {;} + + /** end-of-iteration? */ + bool operator != (const Iterator& o) const {return pos != o.pos;} + + /** next value */ + Iterator& operator ++ () {++pos; return *this;} + + /** get the user-data */ + Element operator * () {return vec[indices[pos]];} + + }; + + //const Element& operator [] (const int idx) const {return vec[indices[idx]]; } + + /** number of available random entries */ + size_t size() const {return cnt;} + + + /** for-each access */ + Iterator begin() const { ensureRandomized(); return Iterator(0, vec, indices); } + + /** for-each access */ + Iterator end() const { ensureRandomized(); return Iterator(cnt, vec, indices); } + +private: + + /** ensure the coder called randomize() before using the iterator */ + void ensureRandomized() const { + Assert::isTrue(isRandomized, "call randomize() before using the iterator!"); + } + + }; + +} + +#endif // RANDOMITERATOR_H diff --git a/math/random/Uniform.h b/math/random/Uniform.h new file mode 100644 index 0000000..dadfe0e --- /dev/null +++ b/math/random/Uniform.h @@ -0,0 +1,43 @@ +#ifndef K_MATH_RANDOM_UNIFORM_H +#define K_MATH_RANDOM_UNIFORM_H + +#include "RandomGenerator.h" + +namespace Random { + + /** + * provides some common functions + * for handling normal-distributed random numbers + */ + template class Uniform { + + private: + + Random::RandomGenerator gen; + + /** depending on T, Dist is either a uniform_real or uniform_int distribution */ + typedef typename std::conditional< std::is_floating_point::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; + Dist dist; + + public: + + /** ctor */ + Uniform(const T min, const T max) : gen(RANDOM_SEED), dist(min, max) { + + } + + /** get a uniformaly distributed random number */ + T draw() { + return dist(gen); + } + + /** set the seed to use */ + void setSeed(const long seed) { + gen.seed(seed); + } + + }; + +} + +#endif // K_MATH_RANDOM_UNIFORM_H diff --git a/math/random/Unique.h b/math/random/Unique.h new file mode 100644 index 0000000..f7f829b --- /dev/null +++ b/math/random/Unique.h @@ -0,0 +1,26 @@ +#ifndef K_MATH_RND_UNIQUE_H +#define K_MATH_RND_UNIQUE_H + +namespace Random { + + /** + * provides some common functions + * for handling uniquely distributed random numbers + */ + class Unique { + + public: + + /** get uniquely distributed random number between min and max */ + static double getBetween(double min, double max) { + double rnd = (double) rand() / (double) RAND_MAX; + rnd *= (max-min); + rnd += min; + return rnd; + } + + }; + +} + +#endif // K_MATH_RND_UNIQUE_H diff --git a/misc/Binning.h b/misc/Binning.h new file mode 100644 index 0000000..7a65eca --- /dev/null +++ b/misc/Binning.h @@ -0,0 +1,114 @@ +#ifndef BINNING_H +#define BINNING_H + +#include +#include + +struct BinningRange { + float min; + float max; + BinningRange(float min, float max) : min(min), max(max) {;} + float getWidth() const {return max-min;} +}; + +template class Binning { + +private: + + /** size to use for each dimension's bin */ + std::vector binSizes; + + std::vector binRanges; + + std::unordered_set used; + +public: + + /** add a new dimension for binning with its corresponding size */ + void setBinSizes(const std::vector binSizes) { + this->binSizes = binSizes; + } + + /** manually set min/max range for each dimension */ + void setRanges(const std::vector ranges) { + this->binRanges = ranges; + } + + /** estimate each dimension's min/max from the given entry set */ + void setRanges(const std::vector& entries) { + + clearUsed(); + binRanges.clear(); + + // process each to-be-binned dimension + const int numDimensions = binSizes.size(); + for (int dim = 0; dim < numDimensions; ++dim) { + + // determin min and max value for the current dimension + BinningRange r(+1e30, -1e30); + for (const Binable& b : entries) { + const float val = b.getBinValue(dim); + if(val < r.min) {r.min = val;} + if(val > r.max) {r.max = val;} + } + + // remember + binRanges.push_back(r); + + } + + } + + /** remove all tracked usages */ + void clearUsed() { + used.clear(); + } + + /** does the given element relate to an used or unsed bin? */ + bool isFree(const Binable& b) const { + const uint64_t hash = getHash(b); + return used.find(hash) == used.end(); + } + + /** mark the bin the given element belongs to as used */ + void markUsed(const Binable& b) { + const uint64_t hash = getHash(b); + used.insert(hash); + } + +private: + + /** calculate unique-bin-hash for the given element */ + uint64_t getHash(const Binable& b) const { + + uint64_t hash = 0; + + const int numDimensions = binSizes.size(); + for (int dim = 0; dim < numDimensions; ++dim) { + + // get binable's value for the current dimension + const float val = b.getBinValue(dim); + + // snap value to bin-number + const int binNr = std::round((val-binRanges[dim].min) / binSizes[dim]); + + // maximum binNr + const int binNrMax = binRanges[dim].getWidth() / binSizes[dim]; + + // sanity check + //if (binNr < 0 || binNr > 255) {throw "bin index out of range!!";} + + // update hash + hash *= (binNrMax+1); + hash += binNr; + + } + + // done + return hash; + + } + +}; + +#endif // BINNING_H diff --git a/smc/Particle.h b/smc/Particle.h new file mode 100644 index 0000000..f9023d3 --- /dev/null +++ b/smc/Particle.h @@ -0,0 +1,54 @@ +/* + * Particle.h + * + * Created on: Sep 17, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLE_H_ +#define PARTICLE_H_ + +/** + * a particle consists of a (user-defined) state + * assigned with a weight (importance). + * + * depending on the particle filter's configuration, + * the (user-defined) state must provide several methods + * like: + * assigning values from another state + * multiplication + * etc.. + * + */ + +namespace SMC { + + template struct Particle { + + /** the particles state */ + State state; + + /** the (current) probability for this state */ + double weight; + + + /** empty ctor */ + Particle() : state(), weight(0) {;} + + /** ctor */ + Particle(const State& state, double weight) : state(state), weight(weight) {;} + + + /** assignment operator */ + Particle& operator = (const Particle& other) { + this->state = other.state; + this->weight = other.weight; + return *this; + } + + + }; + +} + +#endif /* PARTICLE_H_ */ diff --git a/smc/ParticleAssertions.h b/smc/ParticleAssertions.h new file mode 100644 index 0000000..5c65ae6 --- /dev/null +++ b/smc/ParticleAssertions.h @@ -0,0 +1,69 @@ +#ifndef PARTICLEASSERTIONS_H +#define PARTICLEASSERTIONS_H + +namespace SMC { + + /** check whether T provides a += operator */ + template + class HasOperatorPlusEq { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator+=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + /** check whether T provides a /= operator */ + template + class HasOperatorDivEq { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator/=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + + /** check whether T provides a * operator */ + template + class HasOperatorMul { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator*) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + /** check whether T provides an assignment operator */ + template + class HasOperatorAssign{ + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + +} + +#endif // PARTICLEASSERTIONS_H diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h new file mode 100644 index 0000000..48d3aa2 --- /dev/null +++ b/smc/filtering/ParticleFilter.h @@ -0,0 +1,256 @@ +/* + * ParticleFilter.h + * + * Created on: Sep 17, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLEFILTER_H_ +#define PARTICLEFILTER_H_ + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + + class ParticleFilter { + + private: + + /** all used particles */ + std::vector> particles; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the evaluation function to use */ + std::unique_ptr> evaluation; + + /** the initialization function to use */ + std::unique_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + + public: + + /** ctor */ + ParticleFilter(const uint32_t numParticles, std::unique_ptr> initializer) { + particles.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilter() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() { + return particles; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::unique_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::unique_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + double lastNEff = 9999999999999; + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // if the number of efficient particles is too low, perform resampling + if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); } + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + lastNEff = normalize(); + + //std::cout << "normalized. n_eff is " << lastNEff << std::endl; + + // estimate the current state + const State est = estimation->estimate(particles); + + // done + return est; + + } + + /** perform only the transition step */ + void updateTransitionOnly(const Control* control) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + + // perform the transition step + transition->transition(particles, control); + + } + + /** perform only the evaluation step */ + State updateEvaluationOnly(const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the evaluation step and calculate the sum of all particle weights + evaluation->evaluation(particles, observation); + + // not needed anymore.. was to tricky to forget etc... + // sanity check + //Assert::isNotNaN(weightSum, "sum of all particle weights (returned from eval) is NAN!"); + //Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(); + + // estimate the current state + const State est = estimation->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** estimate the current state without update or transition just based on the current weights */ + State estimate() { + + // sanity checks (if enabled) + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + return estimation->estimate(particles); + + } + + private: + + /** normalize the weight of all particles to 1.0 and perform some sanity checks */ + double normalize() { + + // calculate sum(weights) + //double min1 = 9999999; + double weightSum = 0.0; + for (const auto& p : particles) { + weightSum += p.weight; + //if (p.weight < min1) {min1 = p.weight;} + } + + // sanity check. always! + if (weightSum != weightSum) { + throw Exception("sum of paticle-weights is NaN"); + } + if (weightSum == 0) { + throw Exception("sum of paticle-weights is 0.0"); + } + + // normalize and calculate N_eff + double sum2 = 0.0; + //double min2 = 9999999; + for (auto& p : particles) { + p.weight /= weightSum; + //if (p.weight < min2) {min2 = p.weight;} + sum2 += (p.weight * p.weight); + } + + // N_eff + return 1.0 / sum2; + + } + +// /** calculate the number of efficient particles (N_eff) */ +// double getNeff() const { +// double sum = 0.0; +// for (auto& p : particles) {sum += (p.weight * p.weight);} +// return 1.0 / sum; +// } + + }; + +} + +#endif /* PARTICLEFILTER_H_ */ diff --git a/smc/filtering/ParticleFilterEvaluation.h b/smc/filtering/ParticleFilterEvaluation.h new file mode 100644 index 0000000..7283767 --- /dev/null +++ b/smc/filtering/ParticleFilterEvaluation.h @@ -0,0 +1,29 @@ +#ifndef K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H +#define K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H + +#include + +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined particle-evaluation. + * the evaluation weighs the particle by comparing its state with the observation p(o_t | q_t) + */ + template + class ParticleFilterEvaluation { + + public: + + /** + * evaluate all particles (update p.weight) depending on their state and the current observation. + * this method MUST return the sum of all weights (used for normalization) + */ + virtual double evaluation(std::vector>& particles, const Observation& observation) = 0; + + }; + +} + +#endif // K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H diff --git a/smc/filtering/ParticleFilterHistory.h b/smc/filtering/ParticleFilterHistory.h new file mode 100644 index 0000000..15cacb2 --- /dev/null +++ b/smc/filtering/ParticleFilterHistory.h @@ -0,0 +1,283 @@ +/* + * ParticleFilterHistory.h + * + * Created on: Jul 13, 2015 + * Author: Toni Fetzer + */ + +#ifndef PARTICLEFILTERHISTORY_H_ +#define PARTICLEFILTERHISTORY_H_ + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + + class ParticleFilterHistory { + + private: + + /** all used particles */ + std::vector> particles; + + /** all non resampled particles */ + std::vector> particlesNoResampling; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the evaluation function to use */ + std::unique_ptr> evaluation; + + /** the initialization function to use */ + std::unique_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + + public: + + /** ctor */ + ParticleFilterHistory(const uint32_t numParticles, std::unique_ptr> initializer) { + particles.resize(numParticles); + particlesNoResampling.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilterHistory() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() { + return particles; + } + + /** access to all non resample particles */ + const std::vector>& getNonResamplingParticles() { + return particlesNoResampling; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + void setAngle(const double angle){ + for(SMC::Particle& p : particles){ + p.state.walkState.heading = angle; + } + } + + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::unique_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::unique_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + //save particels before resampling to save the weight at this timestep + particlesNoResampling = particles; + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation, std::vector>& particlesWifi, + std::function>&, State, std::vector>&)> calcKLD, double& kld) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + //Assert::isNotNull(kld, "kld MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + //save particels before resampling to save the weight at this timestep + particlesNoResampling = particles; + + //calc the current divergence between wifi and particle propability + kld = calcKLD(particles, est, particlesWifi); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles, kld, particlesWifi); } + + // done + return est; + + } + + + /** perform only the transition step */ + void updateTransitionOnly(const Control* control) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + + // perform the transition step + transition->transition(particles, control); + + } + + /** perform only the evaluation step */ + State updateEvaluationOnly(const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // sanity check + Assert::isNotNaN(weightSum, "sum of all particle weights (returned from eval) is NAN!"); + Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** estimate the current state without update or transition just based on the current weights */ + State estimate() { + + // sanity checks (if enabled) + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + return estimation->estimate(particles); + + } + + private: + + /** normalize the weight of all particles to one */ + double normalize(const double weightSum) { + double sum = 0.0; + for (auto& p : particles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + return 1.0 / sum; + } + + /** calculate the number of efficient particles (N_eff) */ + double getNeff() const { + double sum = 0.0; + for (auto& p : particles) {sum += (p.weight * p.weight);} + return 1.0 / sum; + } + }; + +} + +#endif /* PARTICLEFILTERHISTORY_H_ */ diff --git a/smc/filtering/ParticleFilterInitializer.h b/smc/filtering/ParticleFilterInitializer.h new file mode 100644 index 0000000..17e9087 --- /dev/null +++ b/smc/filtering/ParticleFilterInitializer.h @@ -0,0 +1,25 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for particle filter initializers. + * an initializer "configures" all particles before the filter starts: p(q_0) + */ + template + class ParticleFilterInitializer { + + public: + + /** the initializer must setup each particle within the given vector */ + virtual void initialize(std::vector>& particles) = 0; + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H diff --git a/smc/filtering/ParticleFilterMixing.h b/smc/filtering/ParticleFilterMixing.h new file mode 100644 index 0000000..fa3085c --- /dev/null +++ b/smc/filtering/ParticleFilterMixing.h @@ -0,0 +1,241 @@ +#ifndef PARTICLEFILTERMIXING_H +#define PARTICLEFILTERMIXING_H + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + class ParticleFilterMixing { + + private: + + /** all used particles */ + std::vector> particles; + + /** the current calculated estimation */ + State estimation; + + /** the resampler to use */ + std::shared_ptr> resampler; + + /** the estimation function to use */ + std::shared_ptr> estimator; + + /** the transition function to use */ + std::shared_ptr> transition; + + /** the evaluation function to use */ + std::shared_ptr> evaluation; + + /** the initialization function to use */ + std::shared_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** the current sum of all weights NOT normalized*/ + double weightSum = 1.0; + + /** the predicted mode probability P(m_t|Z_t-1) */ + double predictedModeProbability = 1.0; + + /** the posterior probability of the mode p(m_t | Z_t)*/ + double modePosteriorProbability = 1.0; + + /** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/ + double transitionModeProbability = 1.0; + + public: + + /** ctor + * NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!! + */ + ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr> initializer, double modePosteriorProbability) { + this->modePosteriorProbability = modePosteriorProbability; + + particles.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilterMixing() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() const { + return this->particles; + } + + void setParticles(const std::vector>& newParticles){ + this->particles = newParticles; + } + + /** get the current estimation */ + const State getEstimation() const { + return estimation; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + + /** set the resampling method to use */ + void setResampling(std::shared_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimator(std::shared_ptr> estimator) { + Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!"); + this->estimator = std::move(estimator); + } + + /** set the transition method to use */ + void setTransition(std::shared_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::shared_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::shared_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the unormalized weight sum of all particles */ + double getWeightSum() const + { + return this->weightSum; + } + + /** get the predicted mode probability P(m_t|Z_t-1)*/ + double getPredictedModeProbability() const + { + return this->predictedModeProbability; + } + + /** set the predicted mode probability P(m_t|Z_t-1)*/ + void setPredictedModeProbability(const double val) { + this->predictedModeProbability = val; + } + + /** get the posterior mode probability P(m_t|Z_t)*/ + double getModePosteriorProbability() const + { + return this->modePosteriorProbability; + } + + /** set the posterior mode probability P(m_t|Z_t)*/ + void setModePosteriorProbability(const double likelihoodSum) { + + Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible"); + Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible"); + //Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible"); + + this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum; + + //Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible"); + } + + /** get the transition mode probability P(m_t|Z_t) + * NOTE: Dont use this value! It is only needed for more beatiful mixed sampling! + */ + double getTransitionModeProbability() const + { + return this->transitionModeProbability; + } + + /** set the transition mode probability P(m_t|Z_t)*/ + void setTransitionModeProbability(const double val) { + this->transitionModeProbability = val; + } + + /** perform resampling -> transition -> evaluation -> estimation */ + void update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + this->weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + this->estimation = estimator->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + } + + private: + + /** normalize the weight of all particles to one */ + double normalize(const double weightSum) { + double sum = 0.0; + for (auto& p : particles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + return 1.0 / sum; + } + + /** calculate the number of efficient particles (N_eff) */ + double getNeff() const { + double sum = 0.0; + for (auto& p : particles) {sum += (p.weight * p.weight);} + return 1.0 / sum; + } + }; + +} + +#endif // PARTICLEFILTERMIXING_H diff --git a/smc/filtering/ParticleFilterTransition.h b/smc/filtering/ParticleFilterTransition.h new file mode 100644 index 0000000..6e0d220 --- /dev/null +++ b/smc/filtering/ParticleFilterTransition.h @@ -0,0 +1,26 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined particle transition. + * the transition describes the particles change during the transition phase p(q_t | q_t-1) + * depending on the control data (if any) + */ + template + class ParticleFilterTransition { + + public: + + /** perform the transition p(q_t | q_t-1) for all particles based on the given control data */ + virtual void transition(std::vector>& particles, const Control* control) = 0; + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H diff --git a/smc/filtering/estimation/ParticleFilterEstimation.h b/smc/filtering/estimation/ParticleFilterEstimation.h new file mode 100644 index 0000000..932bc0e --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimation.h @@ -0,0 +1,24 @@ +#ifndef PARTICLEFILTERESTIMATION_H +#define PARTICLEFILTERESTIMATION_H + +#include "../../Particle.h" +#include + +namespace SMC { + + template + class ParticleFilterEstimation { + + public: + + // dtor + virtual ~ParticleFilterEstimation() {;} + + // get the current state estimation for the given particle set + virtual State estimate(const std::vector>& particles) = 0; + + }; + +} + +#endif // PARTICLEFILTERESTIMATION_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h b/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h new file mode 100644 index 0000000..ba37b59 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h @@ -0,0 +1,160 @@ +#if FIXME + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H + + +#include "ParticleFilterEstimation.h" +#include +#include +#include "../../Particle.h" + +#include "../../../../misc/gnuplot/Gnuplot.h" +#include "../../../optimization/NumOptVector.h" +#include "../../../optimization/NumOptAlgoDownhillSimplex.h" + + + +namespace SMC { + + template + class ParticleFilterEstimationKernelDensity : public ParticleFilterEstimation { + + Gnuplot gp; + + K::NumOptAlgoDownhillSimplex simplex; + + private: + + class OptFunc { + + private: + + /** the particles to work on */ + const std::vector>& particles; + + public: + + /** ctor */ + OptFunc(const std::vector>& particles) : particles(particles) {;} + + float operator () (const float* params) const { + + double prob = 0; + const int size = particles.size(); + + //#pragma omp parallel for + for (int i = 0; i < size; i+=10) { + const Particle& p = particles[i]; + prob += p.state.getKernelDensityProbability(params) * p.weight; + } + + // convert probability to "error" + return -prob; + + } + + }; + + + public: + + State estimate(std::vector>& particles) override { + + + // comparator + auto comp = [] (const Particle& p1, const Particle& p2) { return p1.weight < p2.weight; }; + +// // find max state +// auto el = std::max_element(particles.begin(), particles.end(), comp); +// State max = el->state; + + +// // region to check +// BBox2 bbox; +// bbox.add(Point2f(-50,-50)); +// bbox.add(Point2f(100,100)); + +// const float stepSize = 1.0f; + +// const int pxX = (bbox.getMax().x - bbox.getMin().x) / stepSize; +// const int pxY = (bbox.getMax().y - bbox.getMin().y) / stepSize; + +// // optimize using simplex + OptFunc func(particles); + +// // calculate the optimum +// simplex.calculateOptimum(func, params); +// std::cout << params << std::endl; + + + float params[numParams]; + getGlobalMax(particles, params); + + // create output state from optimized params + State res(params); + + + gp << "splot '-' with lines\n"; + + int x1 = 0;//params[0]-2500; + int x2 = 100*100;//params[0]+2500; + int y1 = 0;//params[1]-2500; + int y2 = 60*100;//params[1]+2500; + + for (int x = x1; x < x2; x += 400) { + for (int y = y1; y < y2; y += 400) { + params[0] = x; + params[1] = y; + params[2] = 0; + gp << x << " " << y << " " << -func(params) << "\n"; + } + gp << "\n"; + } + gp << "e\n"; + gp.flush(); + + return res; + + } + + void getGlobalMax(const std::vector>& particles, float* startParams) { + + OptFunc func(particles); + double bestP = 0; + float bestParams[numParams]; + + simplex.setMaxIterations(10); + simplex.setNumRestarts(0); + + + for (int i = 0; i < 15; ++i) { + + // start at a random particle + const int idx = rand() % particles.size(); + + // start optimization at this particle's paramters + particles[idx].state.fillKernelDenstityParameters(startParams); + + simplex.calculateOptimum(func, startParams); + + const float prob = -func(startParams); + if (prob > bestP) { + bestP = prob; + memcpy(bestParams, startParams, numParams*sizeof(float)); + //std::cout << bestParams << std::endl; + } + + } + + memcpy(startParams, bestParams, numParams*sizeof(float)); + + } + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H + +#endif diff --git a/smc/filtering/estimation/ParticleFilterEstimationMax.h b/smc/filtering/estimation/ParticleFilterEstimationMax.h new file mode 100644 index 0000000..b07718e --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationMax.h @@ -0,0 +1,37 @@ +#ifndef PARTICLEFILTERESTIMATIONMAX_H +#define PARTICLEFILTERESTIMATIONMAX_H + +#include "ParticleFilterEstimation.h" +#include +#include +#include "../../Particle.h" + +namespace SMC { + + /** + * just use the particle with the maximum weight + * as the currently sestimated state + */ + template + class ParticleFilterEstimationMax : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // comparator + auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight < p2.weight; + }; + + // find max element + auto el = std::max_element(particles.begin(), particles.end(), comp); + return el->state; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONMAX_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h new file mode 100644 index 0000000..61102aa --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h @@ -0,0 +1,78 @@ +#ifndef PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H + + +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * the X% best weighted particles + */ + template + class ParticleFilterEstimationOrderedWeightedAverage : public ParticleFilterEstimation { + + private: + + const float percent; + + public: + + /** ctor */ + ParticleFilterEstimationOrderedWeightedAverage(const float percent) : percent(percent) {;} + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + // comparator (highest first) + auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + // create a copy + std::vector> copy; + copy.insert(copy.begin(), particles.begin(), particles.end()); + + // sort the copy (highest weight first) + std::sort (copy.begin(), copy.end(), comp); + + State tmp; + + // calculate weighted average + const int numBest = copy.size() * percent; + double weightSum = 0; + for (int i = 0; i < numBest; ++i) { + const Particle& p = copy[i]; + tmp += p.state * p.weight; + weightSum += p.weight; + } + + if (weightSum != weightSum) { + int i = 0; (void) i; + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h new file mode 100644 index 0000000..20c3bc2 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h @@ -0,0 +1,60 @@ +#ifndef PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H + +#include "ParticleFilterEstimation.h" +#include "../../Particle.h" +#include "../../ParticleAssertions.h" + +#include + +namespace SMC { + + /** + * this implementation estimates the current state + * by using the most probable particle and some near particles + * combining them by their weight (weighted average) + * + * the checker, whether a particle is near or not, uses a special, + * user-defined metric "belongsToRegion()". This user-method must + * return a boolean, whether to include the provided particle + * within the region around the maximum particle, or not. + */ + template + class ParticleFilterEstimationRegionalWeightedAverage : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + //1) get the most probable particle + const auto comp = [] (const Particle& p1, const Particle& p2) {return p1.weight < p2.weight;}; + const Particle& max = *std::max_element(particles.begin(), particles.end(), comp); + + //2) calculate the regional weighted average + double cumWeight = 0; + State res; + for (const Particle& p : particles) { + if (!p.state.belongsToRegion(max.state)) {continue;} + cumWeight += p.weight; + res += p.state * p.weight; + } + + // 3) normalize + res /= cumWeight; + + // done + return res; + + } + + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h new file mode 100644 index 0000000..f533161 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h @@ -0,0 +1,52 @@ +#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H + +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * all particles and their weight + */ + template + class ParticleFilterEstimationWeightedAverage : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + State tmp; + + // calculate weighted average + double weightSum = 0; + for (const Particle& p : particles) { + tmp += p.state * p.weight; + weightSum += p.weight; + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h new file mode 100644 index 0000000..37e8edc --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h @@ -0,0 +1,66 @@ +#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H +#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H + +#include +#include +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * all particles and their weight + */ + template + class ParticleFilterEstimationWeightedAverageWithAngle : public ParticleFilterEstimation { + + public: + + State estimate(std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + State tmp; + + // calculate weighted average + double weightSum = 0; + + double xAngle = 0; + double yAngle = 0; + for (const Particle& p : particles) { + tmp += p.state * p.weight; + weightSum += p.weight; + + xAngle += std::cos(p.state.walkState.heading.getRAD()); + yAngle += std::sin(p.state.walkState.heading.getRAD()); + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + //calculated avg angle + tmp.avgAngle = std::fmod((std::atan2(yAngle / particles.size(), xAngle / particles.size()) * 180.0/PI) + 720, 360.0); + + if(tmp.avgAngle > 360.0) + std::cout << "fuck" << std::endl; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H diff --git a/smc/filtering/resampling/ParticleFilterResampling.h b/smc/filtering/resampling/ParticleFilterResampling.h new file mode 100644 index 0000000..906e91f --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResampling.h @@ -0,0 +1,43 @@ +/* + * ParticleResampling.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLEFILTERRESAMPLING_H_ +#define PARTICLEFILTERRESAMPLING_H_ + +#include "../../Particle.h" +#include +#include + +/** + * interface for all available resampling methods + * within the particle filter + * @param State the (user-defined) state + */ + +namespace SMC { + + template class ParticleFilterResampling { + + public: + + /** + * perform resampling on the given particle-vector + * @param particles the vector of all particles to resample + */ + virtual void resample(std::vector>& particles) = 0; + + /** + * perform resampling on the given particle-vector + * @param particles the vector of all particles to resample + */ + virtual void resample(std::vector>& particles, double kld, std::vector>& particlesWifi) {} + + }; + +} + +#endif /* PARTICLEFILTERRESAMPLING_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingDivergence.h b/smc/filtering/resampling/ParticleFilterResamplingDivergence.h new file mode 100644 index 0000000..b8b2335 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingDivergence.h @@ -0,0 +1,157 @@ +#ifndef PARTICLEFILTERRESAMPLINGDIVERGENCE_H +#define PARTICLEFILTERRESAMPLINGDIVERGENCE_H + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +int fuck_off = 0; + + +namespace SMC { + + /** + * draws new particle depending on the current divergence between two + * probability distributations based on Jensen–Shannon divergence + */ + template + class ParticleFilterResamplingDivergence : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** this is a copy of the wifi particle-set */ + std::vector> particlesWifiCopy; + + /** random number generator */ + std::minstd_rand gen; + + public: + + /** ctor */ + ParticleFilterResamplingDivergence() { + gen.seed(1234); + } + + + void resample(std::vector>& particles) override{ + + } + + void resample(std::vector>& particles, double kld, std::vector>& particlesWifi) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + //static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + const uint32_t cntWifi = (uint32_t) particlesWifi.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + particlesWifiCopy.resize(cntWifi); + + // swap both vectors + particlesCopy.swap(particles); + particlesWifiCopy = particlesWifi; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + double cumWeightWifi = 0; + for (uint32_t i = 0; i < cntWifi; ++i) { + cumWeightWifi += particlesWifiCopy[i].weight; + particlesWifiCopy[i].weight = cumWeightWifi; + } + + double maxKld = 250; + double minKld = 20; + + double diffKld = kld - minKld; + if(diffKld < 0){ + diffKld = 0; + } + + int numWifiParticel = (diffKld / maxKld) * cnt; + + //skip the first observations + static const int skipStart = 5; + if(fuck_off++ < skipStart){ + numWifiParticel = 0; + } + + + // now draw from the copy vector and fill the original one + // with the resampled particle-set keep the number of particles. + for (uint32_t i = 0; i < cnt; ++i) { + if(i < numWifiParticel){ + + //we draw a particle from posterior and change to position based on the wifi distribution + Particle posteriorParticle = draw(cumWeight); + Particle wifiParticle = drawWifi(cumWeightWifi); + + posteriorParticle.state.position = wifiParticle.state.position; + particles[i] = posteriorParticle; + } else { + particles[i] = draw(cumWeight); + } + particles[i].weight = equalWeight; + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + /** draw one particle according to its weight from the copy vector */ + const Particle& drawWifi(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesWifiCopy.begin(), particlesWifiCopy.end(), rand, comp); + return *it; + + } + + + }; + + +} + + + +#endif // PARTICLEFILTERRESAMPLINGDIVERGENCE_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingKLD.h b/smc/filtering/resampling/ParticleFilterResamplingKLD.h new file mode 100644 index 0000000..5f7e58b --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingKLD.h @@ -0,0 +1,130 @@ +#ifndef PARTICLEFILTERRESAMPLINGKLD_H +#define PARTICLEFILTERRESAMPLINGKLD_H + +#include +#include + +#include +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include +#include + +namespace SMC { + + /** + * "Adapting sample size in particle filters through KLD-resampling" T.Li et al. + * Resample a dynamic size of new particles. for resampling we use the simple (multinomial version) + * We cann guarente with the probability 1 - delt, the KLD between posterior and the true posterior is + * less then epsilon. + * + * + * Note: the State template parameter needs to implement a float getBinValue(const int dim) const {..} function + */ + template + class ParticleFilterResamplingKLD : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + /** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/ + double epsilon; + + /** the upper 1 - delta quantil of the normal distribution. something like 0.01 */ + double delta; + + /** the bins */ + Binning bins; + + /** max particle size */ + uint32_t N_max; + + public: + + /** ctor */ + ParticleFilterResamplingKLD(double delta = 0.01, double epsilon = 0.13, uint32_t N_max = 25000) : delta(delta), epsilon(epsilon), N_max(N_max) { + gen.seed(RANDOM_SEED); + + bins.setBinSizes({0.01, 0.01, 0.2, 0.3}); + bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)}); + } + + void resample(std::vector>& particles) override { + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + //clean the bins and particles + bins.clearUsed(); + particles.clear(); + + // draw a new particle and check if it is within a bin or not + uint32_t k = 1; + double N = 0; + int i = 0; + while(i <= N && i <= N_max){ + particles.push_back(draw(cumWeight)); + particles.back().weight = equalWeight; + + //is bin free? + if(bins.isFree(particles[i].state)){ + k++; + bins.markUsed(particles[i].state); + + //calculate the new N + double z_delta = Distribution::NormalCDF::getProbit(1 - delta); + double front = (k - 1) / (2 * epsilon); + double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta ); + + N = front * std::pow(back, 3.0); + } + ++i; + + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + }; + + +} + +#endif // PARTICLEFILTERRESAMPLINGKLD_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingLog.h b/smc/filtering/resampling/ParticleFilterResamplingLog.h new file mode 100644 index 0000000..500391d --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingLog.h @@ -0,0 +1,31 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H + +#include "ParticleFilterResamplingSimple.h" + +namespace K { + + /** + * draw particles using their log-weight + */ + template + class ParticleFilterResamplingLog : public ParticleFilterResamplingSimple { + + public: + + void resample(std::vector>& particles) override { + + for (Particle& p : particles) { + //p.weight = - 1.0 / std::log(p.weight); + p.weight = std::pow(p.weight, 0.5); + } + + ParticleFilterResamplingSimple::resample(particles); + + } + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingNEff.h b/smc/filtering/resampling/ParticleFilterResamplingNEff.h new file mode 100644 index 0000000..aed655b --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingNEff.h @@ -0,0 +1,163 @@ +#ifndef PARTICLEFILTERRESAMPLINGNEFF_H +#define PARTICLEFILTERRESAMPLINGNEFF_H + + + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../ParticleAssertions.h" + +namespace K { + + /** + * TODO + */ + template class ParticleFilterResamplingNEff : public ParticleFilterResampling { + + public: + + using DrawCallback = std::function& p)>; + + private: + + /** random number generator */ + std::minstd_rand gen; + + const float desiredNEff; + + const float maxAdapt = 0.05; + + DrawCallback drawCallback; + + public: + + /** ctor */ + ParticleFilterResamplingNEff(const float desiredNEff = 0.75, const float maxAdapt = 0.05) : desiredNEff(desiredNEff), maxAdapt(maxAdapt) { + gen.seed(1234); + } + + /** callback to inform about redrawn particles */ + void setDrawCallback(const DrawCallback& callback) {this->drawCallback = callback;} + + void resample(std::vector>& particles) override { + + // comparator (highest first) + static auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + // ensure all particle-weights sum up to 1.0 + normalize(particles); + + // calculate current N_eff + const double curNEff = getNEff(particles); + + // cur N_eff is > desired N_eff -> nothing to do + if (curNEff > desiredNEff) {return;} + + const size_t cnt = particles.size(); + + // how many particles to discard and resample based on N_eff + // example: N_eff = 75% -> 25% to discard/resample + // cur N_eff < desired N_eff -> calculate adaption + float adapt = desiredNEff - curNEff; + if (adapt > maxAdapt) {adapt = maxAdapt;} + const size_t toResample = cnt * adapt; + + + + // sort orig particles by weight (highest first) + std::sort(particles.begin(), particles.end(), comp); + + // to-be-removed region [at the end of the vector] + const size_t start = particles.size() - toResample; + + // zero the weight of the to-be-removed region + for (size_t i = start; i < cnt; ++i) { + particles[i].weight = 0; + } + + // renormalize the remaining set + normalize(particles); + + // replace the to-be-removed region [start:cnt] + for (size_t i = start; i < cnt; ++i) { + + // track cumulative weight for each particle-index + std::vector cumWeights; + + // calculate cumulative weight for the copy [0:start] = the to-be-kept particles + double cumWeight = 0; + for (size_t i = 0; i < start; ++i) { + cumWeight += particles[i].weight; + cumWeights.push_back(cumWeight); + } + + // get a random particle between [0:start] + const size_t idx = draw(cumWeights); + + // split + particles[i] = particles[idx]; + particles[i].weight /= 2; + particles[idx].weight /= 2; + + // callback? + if (drawCallback) {drawCallback(particles[i]);} + + } + + // sanity check. sum of weights must now still be 1.0! + double weightSum = 0; + for (const auto& p : particles) {weightSum += p.weight;} + + if (std::abs(weightSum - 1.0) > 0.01) { + throw Exception("particle weight does not sum up to 1.0"); + } + + } + + private: + + void normalize(std::vector>& particles) const { + double sum = 0.0; + for (const auto& p : particles) {sum += p.weight;} // calculate sum + for (auto& p : particles) {p.weight /= sum;} // normalize + } + + double getNEff(const std::vector>& particles) const { + double sum = 0.0; + for (auto& p : particles) { + sum += (p.weight * p.weight); + } + return 1.0 / sum / particles.size(); + } + + size_t draw(const std::vector& cumWeights) { + + // get the cumulative weight [= last entry] + const double cumWeight = cumWeights.back(); + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto it = std::lower_bound(cumWeights.begin(), cumWeights.end(), rand); + + // convert iterator to index + const int idx = it - cumWeights.begin(); + + return idx; + + } + + }; + +} + + +#endif // PARTICLEFILTERRESAMPLINGNEFF_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingNone.h b/smc/filtering/resampling/ParticleFilterResamplingNone.h new file mode 100644 index 0000000..1c1bb02 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingNone.h @@ -0,0 +1,34 @@ +/* + * ParticleResamplingNone.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ + +#include "ParticleFilterResampling.h" + +namespace K { + + /** + * dummy resampler simply doing nothing + */ + template + class ParticleFilterResamplingNone : public ParticleFilterResampling { + + public: + +// /** dtor */ +// ~ParticleFilterResamplingNone() {;} + + void resample(std::vector>& particles) override { + (void) particles; + } + + }; + +} + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingPercent.h b/smc/filtering/resampling/ParticleFilterResamplingPercent.h new file mode 100644 index 0000000..42ec764 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingPercent.h @@ -0,0 +1,94 @@ +#ifndef PARTICLEFILTERRESAMPLINGPERCENT_H +#define PARTICLEFILTERRESAMPLINGPERCENT_H + + +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +namespace SMC { + + /** + * TODO + */ + template class ParticleFilterResamplingPercent : public ParticleFilterResampling { + + private: + + /** random number generator */ + std::minstd_rand gen; + + float percent; + + public: + + /** ctor */ + ParticleFilterResamplingPercent(const float percent) : percent(percent) { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // comparator (highest first) + static auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + const uint32_t cnt = (uint32_t) particles.size(); + + // sort particles by weight (highest first) + std::sort(particles.begin(), particles.end(), comp); + + + // to-be-removed region + const int start = particles.size() * (1-percent); + const int end = particles.size(); + std::uniform_int_distribution dist(0, start-1); + + // remove by re-drawing + for (uint32_t i = start; i < end; ++i) { + const int rnd = dist(gen); + particles[i] = particles[rnd]; + particles[i].weight /= 2; + particles[rnd].weight /= 2; + } + + // calculate weight-sum + double weightSum = 0; + for (const auto& p : particles) { + weightSum += p.weight; + } + + + int i = 0; + + + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(std::vector>& copy, const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); + return *it; + + } + + + + }; + +} + +#endif // PARTICLEFILTERRESAMPLINGPERCENT_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimple.h b/smc/filtering/resampling/ParticleFilterResamplingSimple.h new file mode 100644 index 0000000..4a2a9bf --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingSimple.h @@ -0,0 +1,102 @@ +/* + * ParticleResamplingSimple.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +namespace SMC { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight. + * O(log(n)) per particle + */ + template + class ParticleFilterResamplingSimple : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + public: + + /** ctor */ + ParticleFilterResamplingSimple() { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // now draw from the copy vector and fill the original one + // with the resampled particle-set + for (uint32_t i = 0; i < cnt; ++i) { + particles[i] = draw(cumWeight); + particles[i].weight = equalWeight; + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + + }; + + +} + + + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingWheel.h b/smc/filtering/resampling/ParticleFilterResamplingWheel.h new file mode 100644 index 0000000..9c1c4d3 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingWheel.h @@ -0,0 +1,95 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../ParticleAssertions.h" +#include "../../../distribution/Uniform.h" +#include "../../../../os/Time.h" + +/** + * sources: + * https://www.udacity.com/course/viewer#!/c-cs373/l-48704330/e-48748082/m-48740082 + * https://www.youtube.com/watch?list=PLpUPoM7Rgzi_7YWn14Va2FODh7LzADBSm&feature=player_detailpage&v=kZhOJgooMxI#t=567 + */ + +namespace K { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight using a "wheel" + * -> O(n) for all particles + */ + template + class ParticleFilterResamplingWheel: public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + public: + + void resample(std::vector>& particles) override { + + uint64_t start = K::Time::getTimeMS(); + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + //static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + + + // get the weight of the "heaviest" particle + const auto lambda = [] (const Particle& p1, const Particle& p2) {return p1.weight < p2.weight;}; + const Particle max = *std::max_element(particles.begin(), particles.end(), lambda); + + // draw form the input particle set by treating it as a wheel + K::UniformDistribution dist(0, 2 * max.weight); + int curIdx = K::UniformDistribution::draw( (int)0, (int)particles.size() - 1); + double curOffset = dist.draw(); + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // draw the new set of particles + for (uint32_t i = 0; i < cnt; ) { + + // found a suitable particle, use it and draw the next random number + if (particles[curIdx].weight >= curOffset) { + particlesCopy[i] = particles[curIdx]; + particlesCopy[i].weight = equalWeight; + curOffset += dist.draw(); + ++i; + + // weight to small, subtract the particles weight and move on to the next particle + } else { + curOffset -= particles[curIdx].weight; + curIdx = (curIdx + 1) % (particles.size()); + } + + } + + // swap both vectors + particlesCopy.swap(particles); + + uint64_t end = K::Time::getTimeMS(); + std::cout << (end-start) << std::endl; + + } + + }; + + +} + + + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ */ diff --git a/smc/merging/InteractingMultipleModelParticleFilter.h b/smc/merging/InteractingMultipleModelParticleFilter.h new file mode 100644 index 0000000..fb96967 --- /dev/null +++ b/smc/merging/InteractingMultipleModelParticleFilter.h @@ -0,0 +1,135 @@ +#ifndef INTERACTINGMULTIPLEMODELPARTICLEFILTER_H +#define INTERACTINGMULTIPLEMODELPARTICLEFILTER_H + +#include + +#include "mixing/MixingSampler.h" +#include "MarkovTransitionProbability.h" +#include "estimation/JointEstimation.h" + +#include "../filtering/ParticleFilterMixing.h" +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for IMMPF based on Driessen and Boers + * @param vector of particle filters as modes + */ + template + class InteractingMultipleModelParticleFilter { + + private: + + /** the used particle filters */ + std::vector> modes; + + /** the mixing function to use */ + std::unique_ptr> mixing; + + /** the function for calculating markov chain transition*/ + std::unique_ptr> transition; + + /** the function for calculating a joint estimation */ + std::unique_ptr> estimation; + + /** the transition probability matrix */ + Eigen::MatrixXd transitionProbabilityMatrix; + + public: + + /** ctor + * NOTE: The single rows of the transition matrix need to sum to 1!!! + */ + InteractingMultipleModelParticleFilter(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) { + + // TODO: this is a deep copy... we could or should change that later.. since slooooooooow =) + this->modes = modes; + + //init transmatrix + this->transitionProbabilityMatrix = transitionProbabilityMatrix; + } + + /** dtor */ + ~InteractingMultipleModelParticleFilter() { + ; + } + + /** set mixing function */ + void setMixingSampler(std::unique_ptr> mixing) { + Assert::isNotNull(mixing, "setMixingSampler() MUST not be called with a nullptr!"); + this->mixing = std::move(mixing); + } + + /** set mode transition function */ + void setMarkovTransitionProbability(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setMarkovTransitionProbability() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set joint estimation function */ + void setJointEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setJointEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + const std::vector>& getModes() const{ + return modes; + } + + /** perform the mixed update -> update particle filters -> estimation -> mixing*/ + State update(const Control* control, const Observation& observation){ + + // sanity checks (if enabled) + Assert::isNotNull(mixing, "mixingsampler MUST not be null! call setResampler() first!"); + //Assert::isNotNull(transition, "transition MUST not be null! call setResampler() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setResampler() first!"); + + // mix both modes depending on the kld divergency and their calculated + // impact (mode posterior prob) to draw new particles + mixing->mixAndSample(modes, this->transitionProbabilityMatrix); + + for(ParticleFilterMixing& filter : modes){ + + //update the particle filter and save estimation + filter.update(control, observation); + } + + // calculate the transition probability matrix for the markov chain based on + // kld divergency. + this->transitionProbabilityMatrix = transition->update(modes, observation); + + // calc posterior probability + this->calcPosteriorProbability(); + + // calculate current estimation + const State jointEst = estimation->estimate(modes); + + //done + return jointEst; + } + + + private: + + void calcPosteriorProbability(){ + + // calculate the likelihood Sum lambda for all modes + // the Posterior is init with P(m_0 | Z_0) + double likelihoodSum; + for(ParticleFilterMixing& filter : modes){ + + likelihoodSum += filter.getWeightSum() * filter.getPredictedModeProbability(); + } + + // set the last posterior probability p(m_t-1 | Z_t-1) for all modes + for(ParticleFilterMixing& filter : modes){ + + filter.setModePosteriorProbability(likelihoodSum); + } + } + }; + +} + +#endif // INTERACTINGMULTIPLEMODELPARTICLEFILTER_H diff --git a/smc/merging/MarkovTransitionProbability.h b/smc/merging/MarkovTransitionProbability.h new file mode 100644 index 0000000..0a43a7e --- /dev/null +++ b/smc/merging/MarkovTransitionProbability.h @@ -0,0 +1,30 @@ +#ifndef MARKOVTRANSITIONPROBABILITY_H +#define MARKOVTRANSITIONPROBABILITY_H + +#include "../filtering/ParticleFilterMixing.h" +#include + +namespace SMC { + + + /** + * interface for all available transition probability calculations + * within the IMMPF + */ + template + class MarkovTransitionProbability { + + public: + + /** + * perform the calculation of the transition matrix + * @param vector of modes / particle filters + */ + virtual Eigen::MatrixXd update(std::vector>& modes, const Observation& obs) = 0; + + }; + +} + + +#endif // MARKOVTRANSITIONPROBABILITY_H diff --git a/smc/merging/estimation/JointEstimation.h b/smc/merging/estimation/JointEstimation.h new file mode 100644 index 0000000..b5c0a6f --- /dev/null +++ b/smc/merging/estimation/JointEstimation.h @@ -0,0 +1,25 @@ +#ifndef JOINTESTIMATION_H +#define JOINTESTIMATION_H + +#include "../../filtering/ParticleFilterMixing.h" + +namespace SMC { + + /** + * interface for all available joint estimations + * within the IMMPF we have multiple particle filters + * the "true" estimation is a joint state of all + */ + template + class JointEstimation { + + public: + + // get the current state estimation for the given particle set + virtual const State estimate(std::vector>& modes) = 0; + + }; + +} + +#endif // JOINTESTIMATION_H diff --git a/smc/merging/estimation/JointEstimationPosteriorOnly.h b/smc/merging/estimation/JointEstimationPosteriorOnly.h new file mode 100644 index 0000000..77271f4 --- /dev/null +++ b/smc/merging/estimation/JointEstimationPosteriorOnly.h @@ -0,0 +1,28 @@ +#ifndef JOINTESTIMATIONPOSTERIORONLY_H +#define JOINTESTIMATIONPOSTERIORONLY_H + +#include "JointEstimation.h" + +#include "../../filtering/ParticleFilterMixing.h" + +namespace SMC { + + /** + * Use only the posterior distribution (first mode entry) for the joint estimaton + */ + template + class JointEstimationPosteriorOnly + : public JointEstimation { + + public: + + // get the current state estimation for the given particle set + const State estimate(std::vector>& modes) override { + return modes[0].getEstimation(); + } + + }; + +} + +#endif // JOINTESTIMATIONPOSTERIORONLY_H diff --git a/smc/merging/mixing/MixingSampler.h b/smc/merging/mixing/MixingSampler.h new file mode 100644 index 0000000..3a8d712 --- /dev/null +++ b/smc/merging/mixing/MixingSampler.h @@ -0,0 +1,29 @@ +#ifndef MIXINGSAMPLER_H +#define MIXINGSAMPLER_H + +#include "../../filtering/ParticleFilterMixing.h" +#include + +namespace SMC { + + + /** + * interface for all available resampling methods + * within the particle filter + */ + template + class MixingSampler { + + public: + + /** + * perform mixing of modes and sample according to the modes probability + * @param particles the vector of all particles to resample + */ + virtual void mixAndSample(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) = 0; + + }; + +} + +#endif // MIXINGSAMPLER_H diff --git a/smc/merging/mixing/MixingSamplerDivergency.h b/smc/merging/mixing/MixingSamplerDivergency.h new file mode 100644 index 0000000..88e4663 --- /dev/null +++ b/smc/merging/mixing/MixingSamplerDivergency.h @@ -0,0 +1,157 @@ +#ifndef MIXINGSAMPLERDIVERGENCY_H +#define MIXINGSAMPLERDIVERGENCY_H + +#include "MixingSampler.h" + +#include +#include +#include + +namespace SMC { + + /** + * Using the direct sampling approach of Driessen and Boers in + * "Efficient particle filter for jump Markov nonlinear systems". + * as transition probability matrix for the markov chain we use + * a divergence based on Jensen–Shannon divergence + */ + template class MixingSamplerDivergency + : public MixingSampler { + + /** random number generator */ + std::minstd_rand genNorm; + std::minstd_rand genPart; + + /** copy of the modes with cumulative probabilities for easy drawing*/ + std::vector> copyModes; + + public: + + void mixAndSample(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) override{ + + genNorm.seed(std::chrono::system_clock::now().time_since_epoch().count()); + genPart.seed(std::chrono::system_clock::now().time_since_epoch().count() + 233); + + // set copyModes for later drawing + copyModes = modes; + + // create cumulative particlesets for the copy + // Note: in most cases, the particles are already resampled within the filtering stage + // but in some cases they are not and therefore we need to draw cumulatively and not equally + for(ParticleFilterMixing& copyFilter : copyModes){ + double cumWeight = 0; + std::vector> copyParticles; + copyParticles = copyFilter.getParticles(); + + for(int i = 0; i < copyParticles.size(); ++i){ + cumWeight += copyParticles[i].weight; + copyParticles[i].weight = cumWeight; + } + + copyFilter.setParticles(copyParticles); + } + + // calculate the new predicted mode prob P(m_t|Z_t-1) and P(m_t-1 | m_t, Z_t-1) + int m = 0; //this is m_t + for(ParticleFilterMixing& focusedFilter : modes){ + + // P(m_t|Z_t-1) = sum(P(m_t | m_t-1) P(m_t-1 | Z_t-1)) + int i = 0; //this are all possible m_t-1 + double predictedModeProbability = 0; + for(ParticleFilterMixing& filter : modes){ + predictedModeProbability += transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability(); + ++i; + } + + //Assert::isNotNull(predictedModeProbability, "predictedModeProbability is zero.. thats not possible!"); + focusedFilter.setPredictedModeProbability(predictedModeProbability); + + // cumulative sum of TransitionModeProbabilities for drawing modes from the perspective of ONE filter! + // this means, the transition mode probabilities are calculated for each filter NEW! + // calculate P(m_t-1 | m_t, Z_t-1) = P(m_t | m_t-1) * p(m_t-1 | Z_t-1) / P(m_t|Z_t-1) + double cumTransitionModeProbability = 0; + i = 0; + for(ParticleFilterMixing& filter : modes){ + + double prob = (transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability()) / focusedFilter.getPredictedModeProbability(); + filter.setTransitionModeProbability(prob); + + cumTransitionModeProbability += prob; + copyModes[i].setTransitionModeProbability(cumTransitionModeProbability); + + //std::cout << "Draw Mode Probability from mode " << m << i << " : " << prob << std::endl; + ++i; + } + + // draw new modes and particles + // Note: in most cases, the particles are already resampled within the filtering stage + // but in some cases they are not and therefore we need to draw cumulatively and not equally + + // todo: make the particle size dynamic depending on the kld or something else + // number of particles for this timestep + int numParticles = focusedFilter.getParticles().size(); + + std::vector> newParticles; + newParticles.resize(numParticles); + double equalWeight = 1.0 / numParticles; + + // draw modes cumulative + for(int k = 0; k < numParticles; ++k){ + + auto mode = drawMode(cumTransitionModeProbability); + newParticles[k] = drawParticle(mode); + newParticles[k].weight = equalWeight; + } + + focusedFilter.setParticles(newParticles); + + //iter + ++m; + + } + } + + private: + + /** draw a mode depending upon the transition mode probabilities */ + ParticleFilterMixing& drawMode(const double cumTransitionModeProbabilities){ + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumTransitionModeProbabilities); + + // draw a random value between [0:cumWeight] + const float rand = dist(genNorm); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (ParticleFilterMixing& filter, const float d) {return filter.getTransitionModeProbability() < d;}; + auto it = std::lower_bound(copyModes.begin(), copyModes.end(), rand, comp); + return *it; + + } + + + /** draw one particle according to its weight from the copy vector of a given mode */ + const Particle& drawParticle(ParticleFilterMixing& filter) { + + double weights = filter.getParticles().back().weight; + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, filter.getParticles().back().weight); + + // draw a random value between [0:cumWeight] + const float rand = dist(genPart); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(filter.getParticles().begin(), filter.getParticles().end(), rand, comp); + return *it; + + } + + }; + + +} + + +#endif // MIXINGSAMPLERDIVERGENCY_H diff --git a/smc/sampling/CumulativeSampler.h b/smc/sampling/CumulativeSampler.h new file mode 100644 index 0000000..677d244 --- /dev/null +++ b/smc/sampling/CumulativeSampler.h @@ -0,0 +1,100 @@ +#ifndef CUMULATIVESAMPLER_H +#define CUMULATIVESAMPLER_H + +#include +#include +#include +#include "../Particle.h" +#include "ParticleTrajectorieSampler.h" + +namespace SMC { + + /** + * drawing trajectories using a cumulative drawer + */ + template + class CumulativeSampler : public ParticleTrajectorieSampler { + + public: + + /** ctor */ + CumulativeSampler(){ + + } + + /** draw a single particle */ + Particle drawSingleParticle(std::vector> const& particles){ + + // ensure the copy vector has the same size as the real particle vector + std::vector> particlesCopy; + particlesCopy.resize(particles.size()); + + // swap both vectors + particlesCopy = particles; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < particles.size(); ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + return draw(cumWeight, particlesCopy, particles); + + } + + /** draw a trajectorie of n particles */ + std::vector> drawTrajectorie(std::vector> const& particles, const int numRealizations){ + + // ensure the copy vector has the same size as the real particle vector + std::vector> particlesCopy; + particlesCopy.reserve(particles.size()); + particlesCopy = particles; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < particles.size(); ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // now draw the initial weights and therefore the corresponding particles + std::vector> trajectorie; + trajectorie.reserve(numRealizations); + for (uint32_t i = 0; i < numRealizations; ++i) { + trajectorie.push_back(draw(cumWeight, particlesCopy, particles)); + } + + return trajectorie; + } + + private: + + /** function for drawing particles */ + Particle draw(const double cumWeight, std::vector> const& cumParticles, std::vector> const& origParticles){ + + // random value between [0;1] + const double rand01 = double(rand()) / double(RAND_MAX); + + // random value between [0; cumulativeWeight] + const double rand = rand01 * cumWeight; + + // search comparator + auto comp = [] (const Particle& s, const double d) {return s.weight < d;}; + auto it = std::lower_bound(cumParticles.begin(), cumParticles.end(), rand, comp); + + //get the idx for the iterator it. this is the same as std::distance(..,..) + int idx = it - cumParticles.begin(); + + //get the original weight instead of the cumulative weight + Particle drawnParticle = *it; + drawnParticle.weight = origParticles[idx].weight; + + return drawnParticle; + + } + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/sampling/ParticleTrajectorieSampler.h b/smc/sampling/ParticleTrajectorieSampler.h new file mode 100644 index 0000000..13501c3 --- /dev/null +++ b/smc/sampling/ParticleTrajectorieSampler.h @@ -0,0 +1,31 @@ +#ifndef PARTICLETRAJECTORIESAMPLER_H +#define PARTICLETRAJECTORIESAMPLER_H + +#include +#include "..//Particle.h" + +namespace SMC { + + /** + * interface for Sampling Trajectories of Particles + */ + template + class ParticleTrajectorieSampler { + + public: + + /** draw a single particle */ + virtual Particle drawSingleParticle(std::vector> const& particles) = 0; + + /** draw a trajectorie of all incoming particles / like resampling*/ + virtual std::vector> drawTrajectorie(std::vector>const& particles, const int num) = 0; + + private: + + /** function for drawing particles */ + virtual Particle draw(const double cumWeight, std::vector> const& cumParticles, std::vector> const& origParticles) = 0; + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/smoothing/ArtificialDistribution.h b/smc/smoothing/ArtificialDistribution.h new file mode 100644 index 0000000..671dca5 --- /dev/null +++ b/smc/smoothing/ArtificialDistribution.h @@ -0,0 +1,24 @@ +#ifndef ARTIFICIALDISTRIBUTION_H +#define ARTIFICIALDISTRIBUTION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for artificial distributions + */ + template + class ArtificialDistribution { + + public: + + /** calculate the probability/density*/ + virtual double calculate(Particle const& particle) = 0; + + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/smoothing/BackwardFilter.h b/smc/smoothing/BackwardFilter.h new file mode 100644 index 0000000..969c360 --- /dev/null +++ b/smc/smoothing/BackwardFilter.h @@ -0,0 +1,57 @@ +#ifndef BACKWARDFILTER_H +#define BACKWARDFILTER_H + + +#include +#include + +#include "BackwardFilterTransition.h" + +#include "../sampling/ParticleTrajectorieSampler.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + + +namespace SMC { + + template + class BackwardFilter { + + public: + virtual State update(std::vector> const& forwardParticles) = 0; + + /** access to all backward / smoothed particles */ + virtual const std::vector>>& getbackwardParticles() = 0; + + /** set the estimation method to use */ + virtual void setEstimation(std::unique_ptr> estimation) = 0; + + /** set the transition method to use */ + virtual void setTransition(std::unique_ptr> transition) = 0; + + /** set the resampling method to use */ + virtual void setResampling(std::unique_ptr> resampler) = 0; + + /** set the resampling threshold as the percentage of efficient particles */ + virtual void setNEffThreshold(const double thresholdPercent) = 0; + + /** set sampler */ + virtual void setSampler(std::unique_ptr> sampler) { (void) sampler; }; + + /** get the used transition method */ + virtual BackwardFilterTransition* getTransition() = 0; + + /** reset */ + virtual void reset() {}; + + }; +} + +#endif // BACKWARDFILTER_H diff --git a/smc/smoothing/BackwardFilterTransition.h b/smc/smoothing/BackwardFilterTransition.h new file mode 100644 index 0000000..474b98b --- /dev/null +++ b/smc/smoothing/BackwardFilterTransition.h @@ -0,0 +1,33 @@ +#ifndef BACKWARDFILTERTRANSITION_H +#define BACKWARDFILTERTRANSITION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined backward filter transition. + * the transition describes the probability of a state change during the transition phase p(q_t+1 | q_t) + */ + template + class BackwardFilterTransition { + + public: + + /** + * @brief perform the transition p(q_t+1 | q_t) for all particles and possibilities + * if you do not use this abstract function, do not forget to throw an error if the user does + */ + virtual std::vector> transition(std::vector> const& toBeSmoothedParticles_qt, std::vector>const& alreadySmoothedParticles_qt1) = 0; + + /** + * @brief perform a forward transition based on the to be smoothed particles at time q_t and sample particles at time q_t+1, also gets an vector with controls c_1:T + * if you do not use this abstract function, do not forget to throw an error if the user does + */ + virtual std::vector> transition(std::vector> const& toBeSmoothedParticles_qt, std::vector const& controls_1T) = 0; + }; + +} + +#endif // BACKWARDFILTERTRANSITION_H diff --git a/smc/smoothing/BackwardSimulation.h b/smc/smoothing/BackwardSimulation.h new file mode 100644 index 0000000..5f3729b --- /dev/null +++ b/smc/smoothing/BackwardSimulation.h @@ -0,0 +1,258 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef BACKWARDSIMULATION_H_ +#define BACKWARDSIMULATION_H_ + + +#include +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../sampling/ParticleTrajectorieSampler.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the Backward Simulation Filter + * running "backwards" in time, generates multiple backwards trajectories + * (Realizations) by repeating the backward simulation M time. + * it can be started at a random time T of any forward particle filter + * [Monte Carlo smoothing for non-linear time series Godsill et al. '03] + * @param State the (user-defined) state for each particle + * @param numRealizations is the number of backward trajectories starting + */ + template + class BackwardSimulation : public BackwardFilter{ + + private: + + /** all smoothed particles T -> 1*/ + std::vector>> backwardParticles; + + /** container for particles */ + std::vector> smoothedParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the sampler for drawing trajectories */ + std::unique_ptr> sampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** number of realizations to be calculated */ + int numRealizations; + + /** is update called the first time? */ + bool firstFunctionCall; + + + public: + + /** ctor */ + BackwardSimulation(int numRealizations) { + this->numRealizations = numRealizations; + backwardParticles.reserve(numRealizations); + smoothedParticles.reserve(numRealizations); + firstFunctionCall = true; + } + + /** dtor */ + ~BackwardSimulation() { + ; + } + + /** reset **/ + void reset(){ + this->numRealizations = numRealizations; + + backwardParticles.clear(); + backwardParticles.reserve(numRealizations); + + smoothedParticles.clear(); + smoothedParticles.reserve(numRealizations); + + firstFunctionCall = true; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the sampler method to use */ + void setSampler(std::unique_ptr> sampler){ + Assert::isNotNull(sampler, "setSampler() MUST not be called with a nullptr!"); + this->sampler = std::move(sampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * gets the weighted sample set of a standard condensation + * particle filter in REVERSED order! + */ + State update(std::vector> const& forwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + //storage for single trajectories / smoothed particles + smoothedParticles.clear(); + + // Choose \tilde x_T = x^(i)_T with probability w^(i)_T + // Therefore sample independently from the categorical distribution of weights. + if(firstFunctionCall){ + + smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); + + firstFunctionCall = false; + backwardParticles.push_back(smoothedParticles); + + const State es = estimation->estimate(smoothedParticles); + return es; + } + + // compute weights using the transition model + // transitionWeigths[numRealizations][numParticles] + std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); + + //get the next trajectorie for a realisation + for(int j = 0; j < numRealizations; ++j){ + + //vector for the current smoothedWeights at time t + std::vector> smoothedWeights; + smoothedWeights.resize(forwardParticles.size()); + smoothedWeights = forwardParticles; + + //check if all transitionWeights are zero + double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0); + Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero"); + + int i = 0; + for (auto& w : transitionWeights.at(j)) { + + // multiply the weight of the particles at time t and normalize + smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w); + if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";} + + // iter + ++i; + } + + //get the sum of all weights + auto lambda = [](double current, const Particle& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + + //normalize the weights + if(weightSumSmoothed != 0.0){ + for (int i = 0; i < smoothedWeights.size(); ++i){ + smoothedWeights.at(i).weight /= weightSumSmoothed; + } + + //check if normalization worked + double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1"); + } + + + //draw the next trajectorie at time t for a realization and save them + smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights)); + + //throw if weight of smoothedParticle is zero + //in practice this is possible, if a particle is completely separated from the rest and is therefore + //weighted zero or very very low. + Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight"); + } + + + if(resampler) + { + + //TODO - does this even make sense? + std::cout << "Warning - Resampling is not yet implemented!" << std::endl; +// //resampling if necessery +// double sum = 0.0; +// double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0); +// for (auto& p : smoothedParticles) { +// p.weight /= weightSum; +// sum += (p.weight * p.weight); +// } + +// const double neff = 1.0/sum; +// if (neff != neff) {throw "detected NaN";} + +// // if the number of efficient particles is too low, perform resampling +// if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } + } + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // estimate the current state + const State est = estimation->estimate(smoothedParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* BACKWARDSIMULATION_H_ */ diff --git a/smc/smoothing/CondensationBackwardFilter.h b/smc/smoothing/CondensationBackwardFilter.h new file mode 100644 index 0000000..d970755 --- /dev/null +++ b/smc/smoothing/CondensationBackwardFilter.h @@ -0,0 +1,226 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef CONDENSATIONBACKWARDFILTER_H_ +#define CONDENSATIONBACKWARDFILTER_H_ + + +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +long long count = 0.0; + +namespace SMC { + + /** + * the main-class for the Condensation Backward Filter + * running "backwards" in time, updating every timestep, no resampling + * it can be started at a random time T of an forward particle filter + * [Smoothing filter for condensation by Isard and Blake '98] + * @param State the (user-defined) state for each particle + */ + template + class CondensationBackwardFilter : public BackwardFilter { + + private: + + /** all smoothed particles 1 -> T*/ + std::vector>> backwardParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + public: + + /** ctor */ + CondensationBackwardFilter() { + } + + /** dtor */ + ~CondensationBackwardFilter() { + ; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * gets the weighted sample set of a standard condensation + * particle filter in REVERSED order! + */ + State update(std::vector> const& forwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // since the algorithm starts at T-1 we need to initialize with the first set of forwardParticels + // psi_T = pi_T + static bool firstFunctionCall = true; + if(firstFunctionCall){ + backwardParticles.push_back(forwardParticles); + firstFunctionCall = false; + + std::vector> tt = forwardParticles; + const State es = estimation->estimate(tt); + return es; + } + + //weightsume for normalization + double weightSum = 0.0; + + // perform the transition step p(x_t+1|x_t) + std::vector> predictionProbabilities = transition->transition(forwardParticles, backwardParticles.back()); + + // calculate the correction factors + std::vector correctionFactors; + for(int m = 0; m < forwardParticles.size(); ++m){ + + double gamma = 0.0; + for(int k = 0; k < forwardParticles.size(); ++k){ + // gamma(m) = sum(pi(k) * alpha(m,k)) + gamma += forwardParticles[k].weight * predictionProbabilities[m][k]; + + if (gamma != gamma) {throw "detected NaN";} + } + correctionFactors.push_back(gamma); + } + + // approximate backward variables + std::vector> smoothedParticles = forwardParticles; + for(int n = 0; n < forwardParticles.size(); ++n){ + + double delta = 0.0; + for(int m = 0; m < forwardParticles.size(); ++m){ + // delta(n) = sum(psi(m) * alpha(m,n) / gamma(m)) + + //!! THIS IS A HACK !! Gamma is getting zero if the prob is to damn low. This would results in NaN for gamma + //!! Therefore we set alpha(m,n) / gamma(m) = 1.0; + if(correctionFactors[m] == 0.0){ + delta += backwardParticles.back().at(m).weight; + std::cout << "Gamma is Zero" << count ++ << std::endl; + } + else + delta += backwardParticles.back().at(m).weight * (predictionProbabilities[m][n] / correctionFactors[m]); + + if (delta != delta) {throw "detected NaN";} + } + + // Evaluate smoothing weights + // psi(n) = pi(n) * delta(n) + double weight = delta * forwardParticles[n].weight; + smoothedParticles[n].weight = weight; + + // fill weightsum + weightSum += weight; + + if (forwardParticles[n].weight != forwardParticles[n].weight) {throw "detected NaN";} + if (delta != delta) {throw "detected NaN";} + if (weight != weight) {throw "detected NaN";} + if (weightSum != weightSum) {throw "detected NaN";} + } + + // normalize the particle weights and thereby calculate N_eff + double sum = 0.0; + for (auto& p : smoothedParticles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + + // sanity check +// if (p.state.heading != p.state.heading) {throw "detected NaN";} +// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";} +// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";} +// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";} +// if (p.weight != p.weight) {throw "detected NaN";} + } + + const double neff = 1.0/sum; + if (neff != neff) {throw "detected NaN";} + + // estimate the current state + const State est = estimation->estimate(smoothedParticles); + +// if (est.heading != est.heading) {throw "detected NaN";} +// if (est.z_nr != est.z_nr) {throw "detected NaN";} +// if (est.x_cm != est.x_cm) {throw "detected NaN";} +// if (est.y_cm != est.y_cm) {throw "detected NaN";} + + if(resampler) + { + // if the number of efficient particles is too low, perform resampling + if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } + } + + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* CONDENSATIONBACKWARDFILTER_H_ */ diff --git a/smc/smoothing/ForwardFilterHistory.h b/smc/smoothing/ForwardFilterHistory.h new file mode 100644 index 0000000..b8841c7 --- /dev/null +++ b/smc/smoothing/ForwardFilterHistory.h @@ -0,0 +1,142 @@ +#ifndef FORWARDFILTERHISTORY_H +#define FORWARDFILTERHISTORY_H + +#include +#include "../Particle.h" +#include "../../data/Timestamp.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * @brief Provides a data structur for the data available at a specific timestamp of the forward filtering procedure. + * @brief Timestamp; ParticleSet (After Transition and Update); Controls; Observations + */ + template + class ForwardFilterHistory { + + + private: + + // NOTE: it would be possible to make some kind of struct for this, but in many upcoming functions and methods, i am not able + // to use all this informations. sometimes if have something like p(q_t+1| q_t, o_t) or p(o_t | q_t, c_t). So keep it flexible! + std::vector timestamps; + std::vector>> particleSets; + std::vector controls; + std::vector observations; + + public: + + ForwardFilterHistory(){ + //empty ctor + } + + void add(Timestamp time, std::vector>> set, Control control, Observation obs){ + + // Is empty? Null? etc. + Assert::isNotNull(time, "Timestamp is Null"); + Assert::isNotNull(set, "Particle Set is Null"); + Assert::isNotNull(control, "Control is Null"); + Assert::isNotNull(obs, "Observation is Null"); + + timestamps.push_back(time); + particleSets.push_back(set); + controls.push_back(control); + observations.push_back(obs); + } + + void removeLatest(){ + + particleSets.pop_back(); + controls.pop_back(); + observations.pop_back(); + } + + void removeFirst(){ + + particleSets.erase(particleSets.begin()); + controls.erase(controls.begin()); + observations.erase(observations.begin()); + } + + /** + * @brief Return the particles from [latestFilterUpdate - @param idx] + * @return returns vector of particles. note: c11 makes a std::move here + */ + std::vector> getParticleSet(idx = 0){ + return particleSets.at(particleSets.end() - idx); + } + + /** + * @brief getControl from [latestFilterUpdate - @param idx] + * @return const controls object + */ + const Control getControl(idx = 0){ + return controls.at(controls.end() - idx); + } + + /** + * @brief getObservationf rom [latestFilterUpdate - @param idx] + * @return const obervations object + */ + const Observation getObservation (idx = 0){ + return observations.at(observations.end() - idx); + } + + /** + * @brief Return the timestamp from [latestFilterUpdate - @param idx] + * @return returns a Timstampf object + */ + std::vector> getTimestamp(idx = 0){ + return timestamps.at(particleSets.end() - idx); + } + + /** + * @brief getLatestFilterUpdateNum + * @return num of particleSets size + */ + const int getLatestFilterUpdateNum(){ + return particleSets.size(); + } + + /** + * @brief getLatestParticleSet Reference + * @return return particle set Note: c11 std::move by vector + */ + std::vector> getLatestParticleSet(){ + return particleSets.back(); + } + + /** + * @brief getLatestControls + * @return const control object + */ + const Control getLatestControls(){ + return controls.back(); + } + + /** + * @brief getLatestObservation + * @return const observation object + */ + const Observation getLatestObservation(){ + return observations.back(); + } + + + /** + * @brief getLatestTimestamp + * @return const Timestamp object + */ + const Timestamp getLatestTimestamp(){ + return timestamps.back(); + } + + + + }; + +} + +#endif // FORWARDFILTERHISTORY_H diff --git a/smc/smoothing/TwoFilterSmoothing.h b/smc/smoothing/TwoFilterSmoothing.h new file mode 100644 index 0000000..0ae69f3 --- /dev/null +++ b/smc/smoothing/TwoFilterSmoothing.h @@ -0,0 +1,187 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef TWOFILTERSMOOTHING_H_ +#define TWOFILTERSMOOTHING_H_ + + +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" +#include "ArtificialDistribution.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * Smoothing Forward and Backward Filter together. + * Call the Update Function. + * Algorithm taken from [Briers04] Smoothing Algorithms for State-Space Models + */ + template + class TwoFilterSmoothing { + + private: + + /** all smoothed particles 1 -> T*/ + std::vector>> smoothedParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** artificial distribuation */ + std::unique_ptr> artificialDistribution; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + public: + + /** ctor */ + TwoFilterSmoothing() { + } + + /** dtor */ + ~TwoFilterSmoothing() { + ; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getsmoothedParticles() { + return smoothedParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + void setArtificialDistribution(std::unique_ptr> artificialDistribution){ + Assert::isNotNull(artificialDistribution, "setArtificialDistribution() MUST not be called with a nullptr!"); + this->artificialDistribution = std::move(artificialDistribution); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * particles from a forwards filter are used to re-weight those from a backwards filter + * so that they represent the target distribution. + * @param: forwardParticles at t-1 + * @param: backwardparticles at t + */ + State update(std::vector> const& forwardParticles, std::vector> const& backwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step p(backward_x_t|forward_x_t-1) + std::vector> predictionProbabilities = transition->transition(forwardParticles, backwardParticles); + + //we are using the forwardparticles to re-weight the backward filter (other direction also possible?) + std::vector> currentParticles = backwardParticles; + + double weightSum = 0.0; + + // calculate the correction factors + for(int j = 0; j < backwardParticles.size(); ++j){ + + double alpha = 0.0; + for(int i = 0; i < backwardParticles.size(); ++i){ + // alpha(j) = sum(forward_weight_t-1 * prediction) + alpha += forwardParticles[i].weight * predictionProbabilities[j][i]; + + if (alpha != alpha) {throw "detected NaN";} + } + + double gamma = 1.0; + if(artificialDistribution){ + gamma = artificialDistribution->calculate(backwardParticles[j]); + } + + //calc the weight + double weight = (currentParticles[j].weight / gamma) * alpha; + + + currentParticles[j].weight = weight; + weightSum += weight; + } + + // normalize the particle weights and thereby calculate N_eff + double sum = 0.0; + for (auto& p : currentParticles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + + double neff = 1.0/sum; + if (neff != neff) {neff = 1.0;} + + // estimate the current state + const State est = estimation->estimate(currentParticles); + + if(resampler) + { + // if the number of efficient particles is too low, perform resampling + if (neff < currentParticles.size() * nEffThresholdPercent) { resampler->resample(currentParticles); } + } + + + // push_back the smoothedParticles + smoothedParticles.push_back(currentParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* TWOFILTERSMOOTHING_H_ */ diff --git a/tests/smc/filtering/TestParticles.cpp b/tests/smc/filtering/TestParticles.cpp new file mode 100644 index 0000000..c4da265 --- /dev/null +++ b/tests/smc/filtering/TestParticles.cpp @@ -0,0 +1,44 @@ +/* + * TestParticles.cpp + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifdef FIXME + +#ifdef WITH_TESTS + +#include "../../../Tests.h" + +#include "../particles/ParticleFilter.h" +#include "../particles/ParticleModel.h" +#include "../particles/ParticleSensor.h" +#include "../wifi/math/WiFiMath.h" +#include "../lib/gnuplot/Gnuplot.h" +#include +#include "../floorplan/FloorPlan.h" +#include "../floorplan/FloorPlanFactory.h" +#include "../lib/misc/File.h" +#include "../particles/ParticleMath.h" +#include "../particles/resampling/ParticleResamplingSimple.h" +#include "../particles/resampling/ParticleResamplingNone.h" + +#include "../wifi/factory/WiFiHelper.h" + +#include +#include + +typedef Point TestState; + + +TEST(Particles, init) { + + //create filter + + +} + +#endif + +#endif diff --git a/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp new file mode 100644 index 0000000..159161d --- /dev/null +++ b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp @@ -0,0 +1,146 @@ +#ifdef WITH_TESTS + +#include "../../../../smc/merging/mixing/MixingSamplerDivergency.h" +#include "../../../../smc/filtering/ParticleFilterMixing.h" + +#include "../../../Tests.h" + +namespace K { + + struct MyState { + double x; + double y; + MyState() : x(0), y(0) {;} + MyState(double x, double y) : x(x), y(y) {;} + MyState& operator += (const MyState& other) { + x += other.x; + y += other.y; + return *this; + } + MyState& operator /= (double d) { + x /= d; + y /= d; + return *this; + } + MyState operator * (double d) const { + return MyState(x*d, y*d); + } + MyState& operator = (const MyState& other) { + this->x = other.x; + this->y = other.y; + return *this; + } + double distance(const MyState& o) const { + return std::sqrt( (x-o.x)*(x-o.x) + (y-o.y)*(y-o.y) ) / 4.9; + } + bool belongsToRegion(const MyState& o) const { + return distance(o) <= 1.0; + } + + }; + + struct MyControl { + + }; + + struct MyObservation { + double x; + double y; + MyObservation() : x(0), y(0) {;} + void set(double x, double y) {this->x = x; this->y = y;} + }; + + class MyInitializer1 : public SMC::ParticleFilterInitializer { + void initialize(std::vector>& particles) override { + for (SMC::Particle& p : particles) { + p.state.x = 0; + p.state.y = 0; + p.weight = 1.0 / particles.size(); + } + } + }; + + class MyInitializer2 : public SMC::ParticleFilterInitializer { + void initialize(std::vector>& particles) override { + for (SMC::Particle& p : particles) { + p.state.x = 1; + p.state.y = 1; + p.weight = 1.0 / particles.size(); + } + } + }; + + + TEST(Mixing, standard) { + + //init particle filters + int numParticles = 1000; + Eigen::MatrixXd transition(2,2); + transition << 0.8, 0.2, 0.2, 0.8; + + SMC::ParticleFilterMixing mode1(numParticles, std::unique_ptr(new MyInitializer1()), 0.5); + SMC::ParticleFilterMixing mode2(numParticles, std::unique_ptr(new MyInitializer2()), 0.5); + + std::vector> modes; + modes.push_back(mode1); + modes.push_back(mode2); + + + SMC::MixingSamplerDivergency mixer; + + //run the mixing and check results + for(int t = 0; t < 100; ++t){ + mixer.mixAndSample(modes, transition); + + int cnt_zero = 0; + int cnt_ones = 0; + for(int i = 0; i < modes[0].getParticles().size(); ++i){ + + if(modes[0].getParticles()[i].state.x == 0){ + cnt_zero++; + }else{ + cnt_ones++; + } + } + + std::cout << "MODE1 Zeros: " << (double) cnt_zero / numParticles<< std::endl; + std::cout << "MODE1 Ones: " << (double) cnt_ones/ numParticles << std::endl; + + + cnt_zero = 0; + cnt_ones = 0; + for(int i = 0; i < modes[0].getParticles().size(); ++i){ + + if(modes[1].getParticles()[i].state.x == 0){ + cnt_zero++; + }else{ + cnt_ones++; + } + } + + std::cout << "MODE2 Zeros: " << (double) cnt_zero / numParticles<< std::endl; + std::cout << "MODE2 Ones: " << (double) cnt_ones/ numParticles << std::endl; + } + + + + } + + TEST(Mixing, differentParticleSize) { + + } + + + TEST(Mixing, dynamicTransitionMatrix) { + + } + + TEST(Mixing, ThreeFilters) { + + } + +} + + + +#endif