Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Assertions.h

@@ -51,8 +51,8 @@ namespace Assert {
 		if (v != nullptr) {doThrow(err);}
 	}
 
-	template <typename T, typename STR> static inline void isNotNull(const T v, const STR err) {
-		if (v == nullptr) {doThrow(err);}
+    template <typename T, typename STR> static inline void isNotNull(const T& v, const STR err) {
+        if (v == nullptr) {doThrow(err);}
 	}
 
 	template <typename T, typename STR> static inline void isNotNaN(const T v, const STR err) {

CMakeLists.txt

@@ -30,6 +30,7 @@ FILE(GLOB HEADERS
 	./*/*/*/*.h
 	./*/*/*/*/*.h
 	./*/*/*/*/*/*.h
+        ./*/*/*/*/*/*/*.h
 	./tests/data/*
 	./tests/data/*/*
 	./tests/data/*/*/*
@@ -41,6 +42,8 @@ FILE(GLOB SOURCES
 	./*/*.cpp
 	./*/*/*.cpp
 	./*/*/*/*.cpp
+        ./*/*/*/*/*.cpp
+        ./*/*/*/*/*/*.cpp
 )
 
 FIND_PACKAGE( OpenMP REQUIRED)
@@ -82,6 +85,7 @@ ADD_DEFINITIONS(
 	-DWITH_TESTS
 	-DWITH_ASSERTIONS
 	-DWITH_DEBUG_LOG
+        -D_GLIBCXX_DEBUG
 
 
 )

grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

@@ -35,7 +35,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkPathControl.h

@@ -23,7 +23,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkPushForward.h

@@ -32,7 +32,7 @@ private:
 	static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkRandomHeadingUpdate.h

@@ -35,7 +35,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkRandomHeadingUpdateAdv.h

@@ -35,7 +35,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkShortestPathControl.h

@@ -79,7 +79,7 @@ protected:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkSimpleControl.h

@@ -24,7 +24,7 @@ private:
 	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkWeighted.h

@@ -39,7 +39,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/GridWalkWeighted2.h

@@ -40,7 +40,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/TestWalkWeighted3.h

@@ -40,7 +40,7 @@ private:
 	DrawList<T&> drawer;
 
 	/** fast random-number-generator */
-	RandomGenerator gen;
+	Random::RandomGenerator gen;
 
 	/** 0-mean normal distribution */
 	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

grid/walk/v2/GridWalker.h

@@ -19,7 +19,7 @@ private:
 	/** all modules to evaluate */
 	std::vector<WalkModule<Node, WalkState>*> modules;
 
-	RandomGenerator rnd;
+	Random::RandomGenerator rnd;
 
 public:
 

grid/walk/v2/modules/WalkModuleHeadingVonMises.h

@@ -52,7 +52,7 @@ public:
         state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
 
         //set kappa of mises
-        float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
+        float kappa = 600 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
         dist.setKappa(kappa);
 
     }

grid/walk/v3/Walker.h

@@ -250,7 +250,6 @@ namespace GW3 {
 			const Point3 start = params.start;
 
 
-
 			struct RICond {
 				const GridPoint gpStart;
 				const float maxDist;

math/Distributions.h

@@ -10,5 +10,6 @@
 #include "distribution/Triangle.h"
 #include "distribution/NormalN.h"
 #include "distribution/Rectangular.h"
+#include "distribution/NormalCDF.h"
 
 #endif // DISTRIBUTIONS_H

math/DrawList.h

@@ -3,7 +3,7 @@
 
 #include <vector>
 
-#include "Random.h"
+#include "random/RandomGenerator.h"
 #include "../Assertions.h"
 
 /**
@@ -41,13 +41,13 @@ private:
 	std::vector<Entry> elements;
 
 	/** the used random number generator */
-	RandomGenerator& gen;
+    Random::RandomGenerator& gen;
 
 
 private:
 
 	/** default random generator. fallback */
-	RandomGenerator defRndGen;
+    Random::RandomGenerator defRndGen;
 
 
 public:
@@ -63,7 +63,7 @@ public:
 	}
 
 	/** ctor with custom RandomNumberGenerator */
-	DrawList(RandomGenerator& gen) : cumProbability(0), gen(gen) {
+    DrawList(Random::RandomGenerator& gen) : cumProbability(0), gen(gen) {
 		;
 	}
 

math/Random.h

@@ -1,28 +0,0 @@
-#ifndef RANDOM_H
-#define RANDOM_H
-
-#include <cmath>
-#include <random>
-#include "../misc/Time.h"
-
-#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
-
-//using RandomGenerator = std::minstd_rand;
-
-class RandomGenerator : public std::minstd_rand {
-
-public:
-
-	/** ctor with default seed */
-    RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;}
-
-	/** ctor with custom seed */
-    RandomGenerator(result_type seed) : std::minstd_rand(seed) {;}
-
-};
-
-#endif // RANDOM_H

math/boxkde/BoxGaus.h

@@ -0,0 +1,130 @@
+#pragma once
+
+#include <cmath>
+#include <vector>
+
+#include "Image2D.h"
+#include "BoxSizes.h"
+
+template <class T>
+struct BoxGaus
+{
+    void boxfilter(std::vector<T>& input, size_t w, size_t h, unsigned filterSize)
+    {
+        assertMsg((filterSize % 2) == 1, "filterSize must be odd");
+
+        unsigned radius = filterSize / 2;
+
+        std::vector<T> buffer(input.size());
+        boxBlur(input, buffer, w, h, radius);
+        boxBlur(buffer, input, w, h, radius);
+    }
+    
+    void approxGaus(Image2D<T>& input, T sigmaX, T sigmaY, unsigned nFilt)
+    {
+        approxGaus(input.data(), input.width, input.height, sigmaX, sigmaY, nFilt);
+    }
+
+	void approxGaus(std::vector<T>& input, size_t w, size_t h, T sigmaX, T sigmaY, unsigned nFilt)
+	{
+        BoxSizes<T> bsX(sigmaX, nFilt);
+        BoxSizes<T> bsY(sigmaY, nFilt);
+        std::vector<T> buffer(input.size());
+
+        assertMsg((2 * bsX.wl + 1 < w) && (2 * bsX.wl + 1 < h), "Box-Filter size in X direction is too big");
+        assertMsg((2 * bsX.wu + 1 < w) && (2 * bsX.wu + 1 < h), "Box-Filter size in X direction is too big");
+        assertMsg((2 * bsY.wl + 1 < w) && (2 * bsY.wl + 1 < h), "Box-Filter size in Y direction is too big");
+        assertMsg((2 * bsY.wu + 1 < w) && (2 * bsY.wu + 1 < h), "Box-Filter size in Y direction is too big");
+
+        // if equal, we can save some cond's inside the loop
+        if (bsX.m == bsY.m)
+        {
+            const size_t m = bsX.m;
+
+            for (size_t i = 0; i < m; i++)
+            {
+                boxBlur(input, buffer, w, h, bsY.wl);
+                boxBlur(buffer, input, w, h, bsX.wl);
+            }
+
+            for (size_t i = 0; i < nFilt - m; i++)
+            {
+                boxBlur(input, buffer, w, h, bsY.wu);
+                boxBlur(buffer, input, w, h, bsX.wu);
+            }
+        }
+        else
+        {
+            for (size_t i = 0; i < nFilt; i++)
+            {
+                boxBlur(input, buffer, w, h, (i < bsY.m ? bsY.wl : bsY.wu) );
+                boxBlur(buffer, input, w, h, (i < bsX.m ? bsX.wl : bsX.wu));
+            }
+        }
+
+	}
+
+private:
+    void boxBlur(const std::vector<T> &src, std::vector<T> &dst, size_t w, size_t h, size_t r)
+    {
+        T iarr = (T)1.0 / (r + r + 1);
+
+        for (size_t i = 0; i < w; i++)
+        {
+            // Init indexes
+            size_t ti = i;
+            size_t li = ti;
+            size_t ri = ti + r*w;
+
+            // Init values
+            T fv = src[ti]; 			// first values
+            T lv = src[ti + w*(h - 1)]; // last values
+            T val = fv * (r + 1); // overhang over image border
+
+            for (size_t j = 0; j < r; j++)
+            {
+                val += src[ti + j*w];  // col sum
+            }
+
+            // <20>berhangbereich links vom Bild
+            for (size_t j = 0; j <= r; j++)
+            {
+                val += src[ri] - fv;
+                dst[i + j*w] = val * iarr;
+
+                ri += w;
+                ti += w;
+            }
+
+            // Bildbereich
+            for (size_t j = r + 1; j < h - r; j++)
+            {
+                val += src[ri] - src[li];
+                dst[i + j*w] = val * iarr;
+
+                li += w;
+                ri += w;
+                ti += w;
+            }
+
+            // <20>berhangbereich rechts vom Bild
+            for (size_t j = h - r; j < h; j++)
+            {
+                val += lv - src[li];
+                dst[i + j*w] = val * iarr;
+
+                li += w;
+                ti += w;
+            }
+        }
+
+        int test = 0;
+    }
+};
+
+
+
+
+
+
+

math/boxkde/BoxSizes.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include "DataStructures.h"
+
+template<typename T>
+struct BoxSizes
+{
+    static_assert(std::is_floating_point<T>::value, "This class only works with floats.");
+
+    T sigma, sigmaActual;
+    T wIdeal, mIdeal;
+
+    unsigned n, m, wl, wu;
+
+    BoxSizes(T sigma, unsigned n)
+        : sigma(sigma), n(n)
+    {
+        assertMsg(sigma >= 0.8, "Sigma values below about 0.8 cannot be represented");
+
+        wIdeal = sqrt(12 * sigma*sigma / n + 1); // Ideal averaging filter width
+
+        // wl is first odd valued integer less than wIdeal
+        wl = (unsigned)floor(wIdeal);
+        if (wl % 2 == 0)
+            wl = wl - 1;
+
+        // wu is the next odd value > wl
+        wu = wl + 2;
+
+        // Compute m.Refer to the tech note for derivation of this formula
+        mIdeal = (12 * sigma*sigma - n*wl*wl - 4 * n*wl - 3 * n) / (-4 * wl - 4);
+        m = (unsigned)round(mIdeal);
+
+        assertMsg(!(m > n || m < 0), "calculation of m has failed");
+
+        // Compute actual sigma that will be achieved
+        sigmaActual = sqrt((m*wl*wl + (n - m)*wu*wu - n) / (T)12.0);
+    }
+};
+
+
+template<typename T>
+struct ExBoxSizes
+{
+    static_assert(std::is_floating_point<T>::value, "This class only works with floats.");
+
+    T sigma, sigma_actual;
+    unsigned n, r;
+    T alpha, c, c1, c2;
+
+    T r_f;
+
+    // Special case for h == 1
+    ExBoxSizes(T sigma, unsigned n)
+        : sigma(sigma), n(n)
+    {
+        T var = sigma*sigma;
+        r_f = 0.5*sqrt((12 * var) / n + 1) - T(0.5);
+        r = (unsigned)std::floor(r_f);
+
+        alpha = (2 * r + 1) * ( (r*r + r - 3*var/n) / (6 * (var/n - (r+1)*(r+1))) );
+        
+        c1 = alpha / (2*alpha + 2*r + 1);
+        c2 = (1 - alpha) / (2 * alpha + 2 * r + 1);
+        c = c1 + c2;
+    }
+
+    ExBoxSizes(T sigma, unsigned d, T h)
+        : sigma(sigma), n(d)
+    {
+        T v = sigma*sigma;
+        r_f = sqrt((12 * v) / n + 1)/(2*h) - T(0.5); // (7)
+        r = (unsigned)std::floor(std::max(T(0), r_f));
+
+        alpha = (2 * r + 1) * (((r*r + r) - (v*3*h)/(d*h*h*h)) / (6 * ( v/(d*h*h) - (r+1)*(r+1)) )); // (8) (14)
+        c1 = alpha / (h*(2 * r + 2 * alpha + 1));       // (8) (13)
+        c2 = (1 - alpha) / (h*(2 * r + 2 * alpha + 1)); // (8) (13)
+
+        c = c1 + c2;
+
+        T lambda = h*(2*r + 1 + 2*alpha); // (8)
+        T variance_actual = (d*h*h*h) / (3 * lambda) * (2*r*r*r + 3*r*r + r + 6*alpha*(r+1)*(r+1)); // (14)
+        sigma_actual = sqrt(variance_actual);
+    }
+};

math/boxkde/DataStructures.h

@@ -0,0 +1,115 @@
+#pragma once
+
+#include <cassert>
+#include <limits>
+#include <memory>
+#include <sstream>
+#include <vector>
+
+template <class T>
+struct BoundingBox
+{
+	static_assert(std::is_arithmetic<T>::value, "This class only works with floats or integers.");
+
+	T MinX, MaxX, MinY, MaxY;
+	
+	BoundingBox(T MinX = std::numeric_limits<T>::max(),
+		        T MaxX = std::numeric_limits<T>::lowest(),
+		        T MinY = std::numeric_limits<T>::max(),
+		        T MaxY = std::numeric_limits<T>::lowest())
+		: MinX(MinX), MaxX(MaxX), MinY(MinY), MaxY(MaxY)
+	{ }
+
+	T width () const { return MaxX - MinX; }
+	T heigth() const { return MaxY - MinY; }
+	T area  () const { return width()*heigth(); }
+
+	bool isInside(T x, T y) const { return (x >= MinX && x <= MaxX) && (y >= MinY && y <= MaxY); }
+
+    // Expands the size of the BB if the given values are extreme
+    void expand(T x, T y)
+    {
+        if (x < MinX) MinX = x;
+        if (x > MaxX) MaxX = x;
+        if (y < MinY) MinY = y;
+        if (y > MaxY) MaxY = y;
+    }
+
+    // Enlarges the BB in both direction along an axis.
+    void inflate(T szX, T szY)
+    {
+        MinX -= szX;
+        MinY -= szY;
+
+        MaxX += szX;
+        MaxY += szY;
+    }
+};
+
+
+template <class T>
+struct Point2D
+{
+	static_assert(std::is_arithmetic<T>::value, "This class only works with floats and integers.");
+
+	T X, Y;
+
+	Point2D(T x = 0, T y = 0)
+		: X(x), Y(y)
+	{ }
+};
+
+template <class T>
+struct Size2D
+{
+    static_assert(std::is_arithmetic<T>::value, "This class only works with floats and integers.");
+
+    T sX, sY;
+
+
+    Size2D(T all)
+        : sX(all), sY(all)
+    { }
+
+    Size2D(T x = 0, T y = 0)
+        : sX(x), sY(y)
+    { }
+};
+
+
+#ifdef NDEBUG
+#define assertCond(_EXPR_) (false)
+#define assertThrow(_arg_) ((void)0)
+#define assert_throw(...)  ((void)0)
+#define assertMsg(...)     ((void)0)
+#else
+// Evaluates the expression. Ifdef NDEBUG returns always false
+    #define assertCond(_EXPR_) (!(_EXPR_))
+// Throws a excpetion with the argument as message by prepending the current file name and line number
+    #define assertThrow(_std_string_) assert_throw( (_std_string_), __FILE__, __LINE__)
+    inline void assert_throw(const std::string& message, const char* file, int line)
+    {
+        std::stringstream ss;
+        ss << file << ":" << line << ": " << message;
+        throw std::invalid_argument(ss.str());
+    }
+
+    #define assertMsg(_EXPR_, _MSG_) if (!(_EXPR_)) assertThrow(_MSG_)
+#endif
+
+// ostream overloads
+template<typename T>
+std::ostream& operator<<(std::ostream& os, const Point2D<T>& pt)
+{
+    return os << "(" << pt.X << "; " << pt.Y << ")";
+}
+
+template<typename T>
+std::ostream& operator<<(std::ostream& os, const BoundingBox<T>& bb)
+{
+    return os << "(X: " << bb.MinX << " - " << bb.MaxX << ";" 
+              << " Y: " << bb.MinY << " - " << bb.MaxY << ")";
+}
+
+
+

math/boxkde/GausLib.cpp

@@ -0,0 +1,83 @@
+#include "GausLib.h"
+
+#include "benchmark.h"
+#include "DataStructures.h"
+#include "Image2D.h"
+#include "BoxGaus.h"
+#include "Grid2D.h"
+#include <cstring>
+
+// X sind 2D Samples [x,x,x,.... y,y,y]
+// sizeX = Anzahl Samples
+// hx, hy Bandbreite KDE (default=100)
+// nBins = Anzahl der Bins in die jeweilige Richtung
+// boundingBox ist Größe des Gebäudes (40 m x 60 m)
+// algorithm = define method  
+
+void gausKde2D_simple(float* X, int sizeX, float* weights, float hx, float hy, int nBinsX, int nBinsY, c_bbox* boundingBox, int algorithm, double* out_maxValue, double* out_maxPosX, double* out_maxPosY, double* out_runTimeInNS, float* out_density)
+{
+    BenchResult bench = benchmarkEx("", 1, [&](){
+        // Create bounding box
+        BoundingBox<float> bb(boundingBox->minX, boundingBox->maxX, boundingBox->minY, boundingBox->maxY);
+
+        // Create histogram
+        Grid2D<float> grid(bb, nBinsX, nBinsY);
+        for (size_t i = 0; i < sizeX; i++)
+        {
+            float x = X[i];
+            float y = X[i + sizeX];
+            float w = weights[i];
+
+            grid.add(x, y, w);
+        }
+
+        int nFilt = 3;
+
+        float sigmaX = hx / grid.binSizeX;
+        float sigmaY = hy / grid.binSizeY;
+
+        // Apply gaus filter
+        switch (algorithm)
+        {
+            default:
+            case ALGO_BOX:
+            {
+                BoxGaus<float> boxGaus;
+                boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
+                break;
+            }
+            case ALGO_EXBOX:
+            {
+                //ExtendedBox<float> boxGaus;
+                //boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
+                break;
+            }
+            case ALGO_BOX_SIMD:
+            {
+                //BoxGausSIMD<float> boxGaus;
+                //boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
+                break;
+            }
+            case ALGO_BOX_CL:
+            {
+                //clBox->writeBuffer(grid.image().data().data());
+                //clBox->execute(sigmaX, nFilt);
+                //clBox->readBuffer(grid.image().data().data());
+                break;
+            }
+        }
+
+
+        Point2D<float> maxPos;
+        *out_maxValue = grid.maximum(maxPos);
+        *out_maxPosX = static_cast<double>(maxPos.X);
+        *out_maxPosY = static_cast<double>(maxPos.Y);
+
+        if (out_density)
+        {
+            memcpy(out_density, grid.image().data().data(), sizeof(float)*grid.image().data().size());
+        }
+    });
+
+    *out_runTimeInNS = bench.min;
+}

math/boxkde/GausLib.h

@@ -0,0 +1,34 @@
+// The following ifdef block is the standard way of creating macros which make exporting 
+// from a DLL simpler. All files within this DLL are compiled with the GAUSLIB_EXPORTS
+// symbol defined on the command line. This symbol should not be defined on any project
+// that uses this DLL. This way any other project whose source files include this file see 
+// GAUSLIB_API functions as being imported from a DLL, whereas this DLL sees symbols
+// defined with this macro as being exported.
+
+    typedef struct c_bbox {
+        double minX;
+        double maxX;
+        double minY;
+        double maxY;
+        double minZ;
+        double maxZ;
+    } c_bbox;
+
+    
+#define ALGO_BOX 0
+#define ALGO_EXBOX 1
+#define ALGO_BOX_SIMD 2
+#define ALGO_BOX_CL 3
+    
+    void gausKde2D_simple(float* X, int sizeX,
+                          float* weights,
+                          float hx, float hy,
+                          int nBinsX, int nBinsY,
+                          c_bbox* boundingBox,
+                          int algorithm,
+                          double* out_maxValue,
+                          double* out_maxPosX, double* out_maxPosY,
+                          double* out_runTimeInNS,
+                          float* out_density);
+
+   

math/boxkde/Grid2D.h

@@ -0,0 +1,211 @@
+#pragma once
+
+#include <sstream>
+
+#include "DataStructures.h"
+#include "Image2D.h"
+
+template<class T>
+struct Grid2D
+{
+	static_assert(std::is_floating_point<T>::value, "Grid2D only supports float values");
+
+    BoundingBox<T> bb;
+    size_t numBinsX, numBinsY;
+    T binSizeX, binSizeY;
+private:
+	      Image2D<T> data;
+
+public:
+
+    Grid2D(){   } //TODO: fast hack
+
+    Grid2D(BoundingBox<T> bb, size_t numBins)
+        : Grid2D(bb, numBins, numBins)
+    {
+    }
+
+	Grid2D(BoundingBox<T> bb, size_t numBinsX, size_t numBinsY)
+		: bb(bb),
+          numBinsX(numBinsX),
+          numBinsY(numBinsY),
+          binSizeX((bb.width()) / (numBinsX-1)),
+          binSizeY((bb.heigth()) / (numBinsY-1)),
+          data(numBinsX, numBinsY)
+	{
+	}
+
+          Image2D<T>& image()       { return data; }
+    const Image2D<T>& image() const { return data; }
+
+          T& operator() (size_t x, size_t y)       { return data(x, y); }
+    const T& operator() (size_t x, size_t y) const { return data(x, y); }
+
+    void clear() { data.clear(); }
+
+	void fill(const std::vector<Point2D<T>>& samples)
+	{
+		assertMsg(!samples.empty(), "Samples must be non-empty");
+
+		data.clear();
+
+		const T weight = T(1.0) / samples.size();
+		for (const auto& pt : samples)
+		{
+			add(pt.X, pt.Y, weight);
+		}
+	}
+
+	void fill(const std::vector<Point2D<T>>& samples, const std::vector<T>& weights)
+	{
+        assertMsg(!samples.empty(), "Samples must be non-empty");
+        assertMsg(weights.size() == samples.size(), "Weights must have the same size as samples");
+
+		data.clear();
+
+		for (size_t i = 0; i < samples.size(); i++)
+		{
+			add(samples[i].X, samples[i].Y, weights[i]);
+		}
+	}
+
+	Point2D<size_t> add(T x, T y, T w)
+	{
+        if (assertCond(bb.isInside(x,y)))
+        {
+            std::stringstream ss;
+            ss << "Point " << Point2D<T>(x, y) << " is out of bounds. " << bb;
+            assertThrow(ss.str());
+        }
+
+		return add_simple_bin(x, y, w);
+		//return add_linear_bin(x, y, w);
+	}
+
+    void add_linear(T x, T y, T w)
+    {
+        if (assertCond(bb.isInside(x, y)))
+        {
+            std::stringstream ss;
+            ss << "Point " << Point2D<T>(x, y) << " is out of bounds. " << bb;
+            assertThrow(ss.str());
+        }
+
+        add_linear_bin(x, y, w);
+    }
+
+    T fetch(T x, T y) const
+    {
+        size_t bin_x = (size_t)((x - bb.MinX) / binSizeX);
+        size_t bin_y = (size_t)((y - bb.MinY) / binSizeY);
+
+        //if (bin_x == data.width)  bin_x--;
+        //if (bin_y == data.height) bin_y--;
+
+        return data(bin_x, bin_y);
+    }
+
+    // Returns the summation of all bin values
+    T sum() const
+    {
+        return data.sum();
+    }
+
+    // Takes a point in input space and converts it to grid space coordinates
+    Point2D<size_t> asGridSpace(T x, T y) const
+    {
+        size_t bin_x = (size_t)((x - bb.MinX) / binSizeX);
+        size_t bin_y = (size_t)((y - bb.MinY) / binSizeY);
+
+        if (bin_x == data.width)  bin_x--;
+        if (bin_y == data.height) bin_y--;
+
+        return Point2D<size_t>(bin_x, bin_y);
+    }
+
+    // Takes a Size2D in input space and converts it to grid space coordiantes
+    Size2D<size_t> asGridSpace(Size2D<T> sz) const
+    {
+        return Size2D<size_t>(sz.sX / binSizeX, sz.sY / binSizeY);
+    }
+
+    // Takes a point in grid space and converts it to input space coordinates
+    Point2D<T> asInputSpace(Point2D<size_t> pt) const
+    {
+        return asInputSpace(pt.X, pt.Y);
+    }
+
+    // Takes a point in grid space and converts it to input space coordinates
+    Point2D<T> asInputSpace(size_t x, size_t y) const
+    {
+        return Point2D<T>(x * binSizeX + bb.MinX + T(0.5)*binSizeX,
+                          y * binSizeY + bb.MinY + T(0.5)*binSizeY);
+    }
+
+
+    T maximum(Point2D<T>& pt) const
+    {
+        Point2D<size_t> gridPt;
+
+        T maxValue = image().maximum(gridPt);
+        pt = asInputSpace(gridPt);
+        return maxValue;
+    }
+
+private:
+    Point2D<size_t> add_simple_bin(T x, T y, T w)
+	{
+		size_t bin_x = (size_t)((x - bb.MinX) / binSizeX);
+		size_t bin_y = (size_t)((y - bb.MinY) / binSizeY);
+
+        if (bin_x == data.width)  bin_x--;
+        if (bin_y == data.height) bin_y--;
+
+		data(bin_x, bin_y) += w;
+
+        return Point2D<size_t>(bin_x, bin_y);
+	}
+
+    void add_linear_bin(T x, T y, T w)
+	{
+        T xpos1 = (x - bb.MinX) / binSizeX;
+        T xpos2 = (y - bb.MinY) / binSizeY;
+        size_t ix1 = (size_t)floor(xpos1);
+        size_t ix2 = (size_t)floor(xpos2);
+        T fx1 = xpos1 - ix1;
+        T fx2 = xpos2 - ix2;
+        
+        const size_t ixmin1 = 0;
+        const size_t ixmax1 = numBinsX - 2;
+        const size_t ixmin2 = 0;
+        const size_t ixmax2 = numBinsY - 2;
+
+        if ( ixmin1 <= ix1 && ixmin2 <= ix2 && ix2 <= ixmax2) 
+        {
+            data(ix1, ix2) += w*(1 - fx1)*(1 - fx2);
+            data(ix1+1, ix2) += w*fx1*(1 - fx2);
+            data(ix1, ix2+1) += w*(1 - fx1)*fx2;
+            data(ix1 + 1, ix2 + 1) += w*fx1*fx2;
+        }
+        else if (ix1 == ixmax1 + 1 && ixmin2 <= ix2 && ix2 <= ixmax2) 
+        {
+            // rechts
+            data(ix1, ix2) += w*(1 - fx1)*(1 - fx2);
+            data(ix1, ix2+1) += w*(1 - fx1)*fx2;
+        }
+        else if (ixmin1 <= ix1 && ix1 <= ixmax1 && ix2 == ixmax2 + 1) 
+        {
+            // unten
+            data(ix1, ix2) += w*(1 - fx1)*(1 - fx2);
+            data(ix1+1, ix2) += w*fx1*(1 - fx2);
+        }
+        else if (ix1 == ixmax1 + 1 && ix2 == ixmax2 + 1) 
+        {
+            // rechts-unten
+            data(ix1, ix2) += w*(1 - fx1)*(1 - fx2);
+        }
+	}
+};
+
+template struct Grid2D<float>;
+template struct Grid2D<double>;

math/boxkde/Image2D.h

@@ -0,0 +1,187 @@
+#pragma once
+
+#include <algorithm>
+#include <cassert>
+#include <numeric>
+#include <vector>
+
+#include "DataStructures.h"
+
+enum struct LineDirection { X, Y };
+
+template <typename TValue>
+struct ImageView2D
+{
+    static_assert(std::is_arithmetic<TValue>::value, "Image only supports integers or floats");
+
+    // forward declaration
+    //enum struct LineDirection;
+    template<LineDirection D> struct LineView;
+    template<LineDirection D> struct ConstLineView;
+
+    size_t width, height, size;
+protected:
+    TValue* values; // contains image data row-wise 
+
+public:
+    ImageView2D()
+        : width(0), height(0), size(0), values(nullptr)
+    { }
+
+    ImageView2D(size_t width, size_t height)
+        : width(width), height(height), size(width*height), values(nullptr)
+    { }
+
+    ImageView2D(size_t width, size_t height, TValue* data)
+        : width(width), height(height), size(width*height), values(data)
+    { }
+
+    inline       TValue& operator() (size_t x, size_t y)       { return values[indexFromCoord(x, y)]; }
+    inline const TValue& operator() (size_t x, size_t y) const { return values[indexFromCoord(x, y)]; }
+
+    TValue* val_begin() { return values; }
+    TValue* val_end  () { return values + size; }
+
+    const TValue* val_begin() const { return values; }
+    const TValue* val_end  () const { return values + size; }
+
+    inline size_t indexFromCoord(size_t x, size_t y) const
+    {
+        assertMsg(x < width && y < height, "(x,y) out of bounds");
+        return y * width + x;
+    }
+
+    Point2D<size_t> coordFromIndex(size_t index) const
+    {
+        assertMsg(index < size, "Index out of bounds");
+        return Point2D<size_t>(index % width, index / width);
+    }
+
+    Point2D<size_t> maximum() const
+    {
+        size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end()));
+        return coordFromIndex(maxValueIndex);
+    }
+
+    TValue maximum(Point2D<size_t>& pt) const
+    {
+        size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end()));
+        pt = coordFromIndex(maxValueIndex);
+        return values[maxValueIndex];
+    }
+
+    void clear()
+    {
+        fill(TValue(0));
+    }
+
+    void fill(TValue value)
+    {
+        std::fill(val_begin(), val_end(), value);
+    }
+    
+    void assign(const ImageView2D<TValue>& other)
+    {
+        assertMsg(size == other.size, "Other must be of the same size as this");
+        std::copy(other.val_begin(), other.val_end(), val_begin());
+    }
+
+    TValue sum() const
+    {
+        return std::accumulate(val_begin(), val_end(), TValue(0));
+    }
+
+    // Returns a transposed view of this image without copying any data.
+    ImageView2D<TValue> transpose() const
+    {
+        return ImageView2D<TValue>(height, width, values);
+    }
+
+    // Returns the value at (x,y). Throws if out of bounds.
+    const TValue& at(size_t x, size_t y) const
+    {
+        return values[indexFromCoord(x, y)];
+    }
+
+    // Returns the value at (x,y) but falls back to default if out of bounds.
+    const TValue& get(size_t x, size_t y, TValue dflt = 0) const
+    {
+        if (x >= width || y >= height)
+            return dflt;
+        else
+            return values[indexFromCoord(x, y)];
+    }
+    
+    std::vector<Point2D<size_t>> findLocalMaxima(int radiusX = 1, int radiusY = 1) const
+    {
+        std::vector<Point2D<size_t>> result;
+        findLocalMaxima(result, radiusX, radiusY);
+        return result;
+    }
+
+    void findLocalMaxima(std::vector<Point2D<size_t>>& result, int radiusX = 1, int radiusY = 1) const
+    {
+        assertMsg(radiusX > 0, "Search radius must be greater than 0.");
+
+        for (size_t y = 0; y < height; y++)
+        {
+            for (size_t x = 0; x < width; x++)
+            {
+                TValue val = at(x, y);
+                bool lclMax = true;
+
+                for (int ry = -radiusY; ry <= radiusY; ry++)
+                {
+                    for (int rx = -radiusX; rx <= radiusX; rx++)
+                    {
+                        TValue other = get(x + rx, y + ry);
+                        if (!(rx == 0 && ry == 0) && val <= other)
+                        {
+                            lclMax = false;
+                        }
+                    }
+                }
+
+                if (lclMax)
+                {
+                    result.emplace_back(x, y);
+                }
+            }
+        }
+    }
+};
+
+template <class TValue>
+struct Image2D : public ImageView2D<TValue>
+{
+	static_assert(std::is_arithmetic<TValue>::value, "Image only supports integers or floats");
+
+	std::vector<TValue> values_vec;
+
+	Image2D()
+	{
+	}
+
+	Image2D(size_t width, size_t height)
+        : ImageView2D<TValue>(width, height), values_vec(width * height)
+	{
+        this->values = values_vec.data();
+    }
+
+    Image2D(size_t width, size_t height, std::vector<TValue>& data)
+        : ImageView2D<TValue>(width, height), values_vec(data)
+    {
+        assertMsg(data.size() == width*height, "Sizes must be the same");
+        this->values = values_vec.data();
+    }
+
+          std::vector<TValue>& data()       { return values_vec; }
+    const std::vector<TValue>& data() const { return values_vec; }
+};
+
+
+template struct ImageView2D<float>;
+template struct ImageView2D<double>;
+
+template struct Image2D<float>;
+template struct Image2D<double>;

math/boxkde/benchmark.h

@@ -0,0 +1,108 @@
+#pragma once
+#include <chrono>
+#include <string>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <numeric>
+
+template <typename F>
+static void benchmark(std::string name, size_t count, F&& lambda)
+{
+	auto start = std::chrono::high_resolution_clock::now();
+
+	for (size_t i = 0; i < count; i++)
+	{
+		lambda();
+	}
+
+	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+	long long durationAVG = duration.count() / count;
+
+	std::cout << "Function " << name << " took avg. " << durationAVG << "us (" << durationAVG / 1000.0f << "ms) over " << count << " runs." << std::endl;
+}
+
+
+struct BenchResult
+{
+    const long long min, max;
+    const double mean, median;
+
+    BenchResult()
+        : min(0), max(0), mean(0), median(0)
+    {}
+
+    BenchResult(long long min, long long max, double mean, double median)
+        : min(min), max(max), mean(mean), median(median)
+    {}
+};
+
+template <typename F>
+static BenchResult benchmarkEx(std::string name, size_t count, F&& lambda)
+{
+    std::vector<long long> durations(count);
+    
+    for (size_t i = 0; i < count; i++)
+    {
+        auto start = std::chrono::high_resolution_clock::now();
+        lambda();
+        auto stop = std::chrono::high_resolution_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(stop - start);
+        durations[i] = duration.count();
+    }
+
+    long long min = *std::min_element(durations.begin(), durations.end());
+    long long max = *std::max_element(durations.begin(), durations.end());
+    double average = std::accumulate(durations.begin(), durations.end(), 0.0) / durations.size();
+
+    double median;
+    std::sort(durations.begin(), durations.end());
+
+    if (durations.size() % 2 == 0)
+        median = (double) (durations[durations.size() / 2 - 1] + durations[durations.size() / 2]) / 2;
+    else
+        median = (double) durations[durations.size() / 2];
+
+    std::cout << "Function " << name << " took avg. " << average << "ns (" << average / 1000.0f << "us) over " << count << " runs." << std::endl;
+
+    return BenchResult(min, max, average, median);
+}
+
+struct StopWatch
+{
+	typedef std::chrono::high_resolution_clock clock;
+	typedef clock::time_point time_point;
+
+	time_point startTime; // Point in time when start() was called
+	time_point lapTime;   // Point in time when operator() was called
+
+public:
+	StopWatch()
+	{
+		reset();
+	}
+
+	void reset()
+	{
+		startTime = clock::now();
+		lapTime = startTime;
+	}
+
+    std::chrono::microseconds stop()
+	{
+		auto duration = std::chrono::duration_cast<std::chrono::microseconds>(clock::now() - startTime);
+
+		std::cout << "Total time: " << duration.count() << "us (" << duration.count() / 1000.0f << "ms)" << std::endl;
+
+        return duration;
+	}
+
+	void operator()(const std::string& str = "")
+	{
+		auto duration = std::chrono::duration_cast<std::chrono::microseconds>(clock::now() - lapTime);
+
+		std::cout << str << (str.empty() ? "" : " ") << "took " << duration.count() << "us (" << duration.count() / 1000.0f << "ms)" << std::endl;
+
+		lapTime = clock::now();
+	}
+};

math/distribution/Exponential.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 
 namespace Distribution {
 
@@ -14,7 +14,7 @@ namespace Distribution {
 
 		const T lambda;
 
-		RandomGenerator gen;
+        Random::RandomGenerator gen;
 		std::exponential_distribution<T> dist;
 
 	public:

math/distribution/KernelDensity.h

@@ -8,7 +8,7 @@
 #include <eigen3/Eigen/Dense>
 
 #include "../../Assertions.h"
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 
 
 namespace Distribution {

math/distribution/Normal.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 #include "../../Assertions.h"
 
 namespace Distribution {
@@ -17,7 +17,7 @@ namespace Distribution {
 		const T sigma;
 		const T _a;
 
-		RandomGenerator gen;
+        Random::RandomGenerator gen;
 		std::normal_distribution<T> dist;
 
 	public:

math/distribution/NormalCDF.h

@@ -0,0 +1,65 @@
+#ifndef NORMALCDF_H
+#define NORMALCDF_H
+
+#include <algorithm>
+#include <random>
+#include "../../Assertions.h"
+
+namespace Distribution {
+
+        /** cumulative density version of the normal distribution */
+        template <typename T> class NormalCDF {
+
+        private:
+
+            const T mu;
+            const T sigma;
+
+            static T RationalApproximation(T t)
+            {
+                // Abramowitz and Stegun formula 26.2.23.
+                // The absolute value of the error should be less than 4.5 e-4.
+                T c[] = {2.515517, 0.802853, 0.010328};
+                T d[] = {1.432788, 0.189269, 0.001308};
+                return t - ((c[2]*t + c[1])*t + c[0]) /
+                           (((d[2]*t + d[1])*t + d[0])*t + 1.0);
+            }
+
+        public:
+
+                /** create a new normally distributed CDF */
+                NormalCDF(const T mu, const T sigma) : mu(mu), sigma(sigma) {
+                        ;
+                }
+
+                /** get the probability for val within the underlying CDF */
+                T getProbability(const T val) const {
+                        return getProbability(mu, sigma, val);
+                }
+
+                /** calculate the probability within the underlying CDF */
+                static T getProbability(const T mu, const T sigma, const T val) {
+                    return (1.0 + std::exp( (val - mu) / (sigma * std::sqrt(2)) ) ) / 2.0;
+                }
+
+                /** get the inverse CDF (https://en.wikipedia.org/wiki/Probit)*/
+                static T getProbit(T p){
+
+                    Assert::isBetween(p, 0.0, 1.0, "value not between");
+
+                    // See: https://www.johndcook.com/blog/normal_cdf_inverse/
+                    if (p < 0.5)
+                    {
+                        // F^-1(p) = - G^-1(p)
+                        return -RationalApproximation( sqrt(-2.0*log(p)) );
+                    }
+                    else
+                    {
+                        // F^-1(p) = G^-1(1-p)
+                        return RationalApproximation( sqrt(-2.0*log(1-p)) );
+                    }
+                }
+    };
+}
+
+#endif // NORMALCDF_H

math/distribution/NormalN.h

@@ -7,7 +7,7 @@
 #include <eigen3/Eigen/Dense>
 
 #include "../../Assertions.h"
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 
 namespace Distribution {
 
@@ -24,7 +24,7 @@ namespace Distribution {
         const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
         Eigen::MatrixXd transform; //can i make this const?
 
-        RandomGenerator gen;
+        Random::RandomGenerator gen;
         std::normal_distribution<> dist;
 
     public:

math/distribution/Rectangular.h

@@ -4,7 +4,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 #include "../../Assertions.h"
 #include "Normal.h"
 

math/distribution/Region.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 #include "../../Assertions.h"
 #include "Normal.h"
 

math/distribution/Triangle.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 #include "../../Assertions.h"
 #include "Normal.h"
 

math/distribution/Uniform.h

@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <random>
-#include "../Random.h"
+#include "../random/RandomGenerator.h"
 
 #include <type_traits>
 
@@ -14,7 +14,7 @@ namespace Distribution {
 
 	private:
 
-		RandomGenerator gen;
+        Random::RandomGenerator gen;
 
 		/** depending on T, Dist is either a uniform_real or uniform_int distribution */
 		typedef typename std::conditional< std::is_floating_point<T>::value, std::uniform_real_distribution<T>, std::uniform_int_distribution<T> >::type Dist;

math/random/DrawList.h

@@ -0,0 +1,169 @@
+#ifndef K_MATH_RANDOM_DRAWLIST_H
+#define K_MATH_RANDOM_DRAWLIST_H
+
+#include <vector>
+
+/**
+ * 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 <typename Entry> struct DrawListEntry {
+
+	template <typename> 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 <typename Entry> 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<Entry> 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<Entry>& 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<Entry>& dle : entries) {
+				sum += dle.probability;
+				dle.cumulativeProbability = sum;
+			}
+
+			// the sum is now valid
+			sumValid = true;
+
+		}
+
+	private:
+
+		/** all entries within the DrawList */
+		std::vector<DrawListEntry<Entry>> entries;
+
+		/** track wether the summedProbability is valid or not */
+		bool sumValid;
+
+	};
+
+
+
+
+}
+
+#endif // K_MATH_RANDOM_DRAWLIST_H

math/random/DrawWheel.h

@@ -0,0 +1,174 @@
+#ifndef K_MATH_RANDOM_DRAWLIST_H
+#define K_MATH_RANDOM_DRAWLIST_H
+
+#include <vector>
+#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 <typename Entry> struct DrawWheelEntry {
+
+	template <typename> 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 <typename Entry> class DrawWheel {
+
+	private:
+
+		/** all entries within the DrawWheel */
+		std::vector<DrawWheelEntry<Entry>> 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>(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<Entry>& e1, const DrawWheelEntry<Entry>& e2) {return e1.probability < e2.probability;};
+
+			// find the largest entry
+			const DrawWheelEntry<Entry> 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

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

math/random/RandomGenerator.h

@@ -0,0 +1,29 @@
+#ifndef RANDOM_Random::RandomGenerator_H
+#define RANDOM_Random::RandomGenerator_H
+
+#include <random>
+#include <chrono>
+
+#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

math/random/RandomIterator.h

@@ -0,0 +1,118 @@
+#ifndef RANDOMITERATOR_H
+#define RANDOMITERATOR_H
+
+#include <vector>
+#include <random>
+#include "../../Assertions.h"
+
+namespace Random {
+
+	template <typename Element> class RandomIterator {
+
+	private:
+
+		/** the user's data-vector to randomly iterate */
+		const std::vector<Element>& vec;
+
+		/** the number of random indices */
+		int cnt;
+
+		/** the random number generator */
+		std::minstd_rand gen;
+		bool isRandomized = false;
+
+		/** X random indices */
+		std::vector<int> indices;
+
+
+	public:
+
+		/** ctor */
+		RandomIterator(const std::vector<Element>& vec, const int cnt) : vec(vec), cnt(cnt) {
+
+			//const uint64_t ts = std::chrono::duration_cast<std::chrono::milliseconds>(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<int> 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<Element>& vec;
+
+			/** the vector containing the random indices */
+			const std::vector<int>& indices;
+
+			/** ctor */
+			Iterator(const int pos, const std::vector<Element>& vec, const std::vector<int>& 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

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 <typename T> 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<T>::value, std::uniform_real_distribution<T>, std::uniform_int_distribution<T> >::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

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

math/stats/Statistics.h

@@ -0,0 +1,169 @@
+#ifndef STATISTICS_H
+#define STATISTICS_H
+
+#include <set>
+#include <cstdint>
+#include <sstream>
+#include <cmath>
+
+namespace Stats {
+
+        /**
+         * store values here and get statistics about their
+         * avg, median, std-dev, etc.
+         */
+        template <typename T> class Statistics {
+
+        public:
+
+                /** ctor */
+                Statistics() : sum(), sumSquared(), cnt() {;}
+
+                /** dtor */
+                ~Statistics() {;}
+
+                /** add a new value */
+                void add(T value) {
+                        ++cnt;
+                        sum += value;
+                        sumSquared += (value*value);
+                        values.insert(value);
+                }
+
+                /** add all values from the given statistics instance */
+                void add(const Statistics<T>& other) {
+                        for (const T val : other.values) {
+                                this->add(val);
+                        }
+                }
+
+                /** get the std-dev of all values */
+                T getStdDev() const {
+                        double E1 = sumSquared / (double) cnt;
+                        double E2 = getAvg();
+                        return std::sqrt(E1 - (E2*E2));
+                }
+
+                /** get average value */
+                T getAvg() const {
+                        return sum / (double) cnt;
+                }
+
+                /** get the given quantile (e.g. 0.5 for median) */
+                T getQuantile(const double q) const {
+                        if (q < 0) {return *values.begin();}
+                        if (q >= 1) {return *(--values.end());}
+                        uint32_t pos = cnt * q;
+                        uint32_t curPos = 0;
+                        for (auto val : values) {
+                                if (curPos == pos) {return val;}
+                                ++curPos;
+                        }
+                        return -1;
+                }
+
+                /** get the median value (= Quantile 0.5) */
+                T getMedian() const {
+                        return getQuantile(0.5f);
+                }
+
+                /** get smallest value */
+                T getMin() const {
+                        if (values.empty()) {return -1;}
+                        return *(values.begin());
+                }
+
+                /** get largest value */
+                T getMax() const {
+                        if (values.empty()) {return -1;}
+                        return *(--values.end());
+                }
+
+                /** get the range between min an max */
+                T getRange() const {
+                        return getMax() - getMin();
+                }
+
+                /** get the squared sum */
+                T getSquaredSum() const {
+                        return sumSquared;
+                }
+
+
+                /** get the sum of all values */
+                T getSum() const {
+                        return sum;
+                }
+
+                /** get number of stored values */
+                uint32_t getCount() const {
+                        return cnt;
+                }
+
+                /** get the number of values that are below "val" */
+                T getNumValuesBelow(uint32_t val) const {
+                        uint32_t numFound = 0;
+                        for (auto curVal : values) {
+                                if (curVal > val) {return numFound;}
+                                ++numFound;
+                        }
+                        return numFound;
+                }
+
+                /** get all contained values in ascending order */
+                const std::multiset<T>& getAll() const {
+                        return values;
+                }
+
+                /** get as string */
+                std::string asString() const {
+                        std::stringstream ss;
+                        appendTo(ss);
+                        return ss.str();
+                }
+
+                /** send to the given stream */
+                void appendTo(std::ostream& out) const {
+                        out.precision(6);
+                        out.setf( std::ios::fixed, std:: ios::floatfield );
+                        out << "cnt(" << getCount() << ")\t";
+                        out << "min(" << getMin() << ")\t";
+                        out << "max(" << getMax() << ")\t";
+                        out << "range(" << getRange() << ")\t";
+                        out << "med(" << getMedian() << ")\t";
+                        out << "avg(" << getAvg() << ")\t";
+                        out << "stdDev(" << getStdDev() << ")\t";
+                }
+
+                /** send to stream */
+                inline std::ostream& operator << (std::ostream& out) const {
+                        appendTo(out); return out;
+                }
+
+                /** reset all statistics */
+                void reset() {
+                        sum = T();
+                        sumSquared = T();
+                        cnt = 0;
+                        values.clear();
+                }
+
+        private:
+
+                /** sum of all added values */
+                T sum;
+
+                /** squared sum of all added values (for std-dev) */
+                T sumSquared;
+
+                /** the number of added values */
+                uint32_t cnt;
+
+                /** multiset to sort all values */
+                std::multiset<T> values;
+
+        };
+
+}
+
+#endif // STATISTICS_H

misc/Binning.h

@@ -0,0 +1,114 @@
+#ifndef BINNING_H
+#define BINNING_H
+
+#include <vector>
+#include <unordered_set>
+
+struct BinningRange {
+	float min;
+	float max;
+	BinningRange(float min, float max) : min(min), max(max) {;}
+	float getWidth() const {return max-min;}
+};
+
+template <typename Binable> class Binning {
+
+private:
+
+	/** size to use for each dimension's bin */
+	std::vector<float> binSizes;
+
+	std::vector<BinningRange> binRanges;
+
+	std::unordered_set<uint64_t> used;
+
+public:
+
+	/** add a new dimension for binning with its corresponding size */
+	void setBinSizes(const std::vector<float> binSizes) {
+		this->binSizes = binSizes;
+	}
+
+	/** manually set min/max range for each dimension */
+	void setRanges(const std::vector<BinningRange> ranges) {
+		this->binRanges = ranges;
+	}
+
+	/** estimate each dimension's min/max from the given entry set */
+	void setRanges(const std::vector<Binable>& 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

sensors/activity/ActivityButterPressurePercent.h

@@ -139,17 +139,17 @@ public:
                 float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
                 float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);
 
-                _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
-                _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
-                _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
-                _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
-                _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
+                Assert::isTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
+                Assert::isTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
+                Assert::isTrue( (densityStay == densityStay), "the probability of densityStay is null!");
+                Assert::isTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
+                Assert::isTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
 
-                _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
-                _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
-                _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
-                _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
-                _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");
+                Assert::isTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
+                Assert::isTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
+                Assert::isTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
+                Assert::isTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
+                Assert::isTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");
 
                 //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
                 densities[0] = densityElevatorDown;

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 <typename State> 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_ */

smc/ParticleAssertions.h

@@ -0,0 +1,69 @@
+#ifndef PARTICLEASSERTIONS_H
+#define PARTICLEASSERTIONS_H
+
+namespace SMC {
+
+	/** check whether T provides a += operator */
+	template <typename T>
+	class HasOperatorPlusEq {
+
+		typedef char one;
+		typedef long two;
+
+		template <typename C> static one test( decltype(&C::operator+=) ) ;
+		template <typename C> static two test(...);
+
+	public:
+		enum { value = sizeof(test<T>(0)) == sizeof(one) };
+
+	};
+
+	/** check whether T provides a /= operator */
+	template <typename T>
+	class HasOperatorDivEq {
+
+		typedef char one;
+		typedef long two;
+
+		template <typename C> static one test( decltype(&C::operator/=) ) ;
+		template <typename C> static two test(...);
+
+	public:
+		enum { value = sizeof(test<T>(0)) == sizeof(one) };
+
+	};
+
+
+	/** check whether T provides a * operator */
+	template <typename T>
+	class HasOperatorMul {
+
+		typedef char one;
+		typedef long two;
+
+		template <typename C> static one test( decltype(&C::operator*) ) ;
+		template <typename C> static two test(...);
+
+	public:
+		enum { value = sizeof(test<T>(0)) == sizeof(one) };
+
+	};
+
+	/** check whether T provides an assignment operator */
+	template <typename T>
+	class HasOperatorAssign{
+
+		typedef char one;
+		typedef long two;
+
+		template <typename C> static one test( decltype(&C::operator=) ) ;
+		template <typename C> static two test(...);
+
+	public:
+		enum { value = sizeof(test<T>(0)) == sizeof(one) };
+
+	};
+
+}
+
+#endif // PARTICLEASSERTIONS_H

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 <vector>
+#include <memory>
+
+#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 <typename State, typename Control, typename Observation>
+
+	class ParticleFilter {
+
+	private:
+
+		/** all used particles */
+		std::vector<Particle<State>> particles;
+
+		/** the resampler to use */
+		std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+		/** the estimation function to use */
+		std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+		/** the transition function to use */
+		std::unique_ptr<ParticleFilterTransition<State, Control>> transition;
+
+		/** the evaluation function to use */
+		std::unique_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
+
+		/** the initialization function to use */
+		std::unique_ptr<ParticleFilterInitializer<State>> initializer;
+
+		/** the percentage-of-efficient-particles-threshold for resampling */
+		double nEffThresholdPercent = 0.25;
+
+
+
+	public:
+
+		/** ctor */
+		ParticleFilter(const uint32_t numParticles, std::unique_ptr<ParticleFilterInitializer<State>> initializer) {
+			particles.resize(numParticles);
+			setInitializier(std::move(initializer));
+			init();
+		}
+
+		/** dtor */
+		~ParticleFilter() {
+			;
+		}
+
+		/** access to all particles */
+		const std::vector<Particle<State>>& 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<ParticleFilterResampling<State>> 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<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> transition) {
+			Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+			this->transition = std::move(transition);
+		}
+
+		/** get the used transition method */
+		ParticleFilterTransition<State, Control>* getTransition() {
+			return this->transition.get();
+		}
+
+		/** set the evaluation method to use */
+		void setEvaluation(std::unique_ptr<ParticleFilterEvaluation<State, Observation>> 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<ParticleFilterInitializer<State>> 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_ */

smc/filtering/ParticleFilterEvaluation.h

@@ -0,0 +1,29 @@
+#ifndef K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H
+#define K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H
+
+#include <vector>
+
+#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 <typename State, typename Observation>
+	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<Particle<State>>& particles, const Observation& observation) = 0;
+
+	};
+
+}
+
+#endif // K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H

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 <vector>
+#include <memory>
+
+#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 <typename State, typename Control, typename Observation>
+
+        class ParticleFilterHistory {
+
+	private:
+
+		/** all used particles */
+		std::vector<Particle<State>> particles;
+
+        /** all non resampled particles */
+        std::vector<Particle<State>> particlesNoResampling;
+
+		/** the resampler to use */
+		std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+		/** the estimation function to use */
+		std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+		/** the transition function to use */
+		std::unique_ptr<ParticleFilterTransition<State, Control>> transition;
+
+		/** the evaluation function to use */
+		std::unique_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
+
+		/** the initialization function to use */
+		std::unique_ptr<ParticleFilterInitializer<State>> initializer;
+
+		/** the percentage-of-efficient-particles-threshold for resampling */
+		double nEffThresholdPercent = 0.25;
+
+
+
+	public:
+
+		/** ctor */
+                ParticleFilterHistory(const uint32_t numParticles, std::unique_ptr<ParticleFilterInitializer<State>> initializer) {
+			particles.resize(numParticles);
+                        particlesNoResampling.resize(numParticles);
+			setInitializier(std::move(initializer));
+			init();
+		}
+
+		/** dtor */
+                ~ParticleFilterHistory() {
+			;
+		}
+
+		/** access to all particles */
+		const std::vector<Particle<State>>& getParticles() {
+			return particles;
+		}
+
+        /** access to all non resample particles */
+        const std::vector<Particle<State>>& 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<State>& p : particles){
+                p.state.walkState.heading = angle;
+            }
+        }
+
+
+		/** set the resampling method to use */
+		void setResampling(std::unique_ptr<ParticleFilterResampling<State>> 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<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+			this->transition = std::move(transition);
+		}
+
+		/** get the used transition method */
+		ParticleFilterTransition<State, Control>* getTransition() {
+			return this->transition.get();
+		}
+
+		/** set the evaluation method to use */
+		void setEvaluation(std::unique_ptr<ParticleFilterEvaluation<State, Observation>> 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<ParticleFilterInitializer<State>> 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<Particle<State>>& particlesWifi,
+                     std::function<double(std::vector<Particle<State>>&, State, std::vector<Particle<State>>&)> 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_ */

smc/filtering/ParticleFilterInitializer.h

@@ -0,0 +1,25 @@
+#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
+#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H
+
+#include <vector>
+#include "../Particle.h"
+
+namespace SMC {
+
+	/**
+	 * interface for particle filter initializers.
+	 * an initializer "configures" all particles before the filter starts: p(q_0)
+	 */
+	template <typename State>
+	class ParticleFilterInitializer {
+
+	public:
+
+		/** the initializer must setup each particle within the given vector */
+		virtual void initialize(std::vector<Particle<State>>& particles) = 0;
+
+	};
+
+}
+
+#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H

smc/filtering/ParticleFilterMixing.h

@@ -0,0 +1,241 @@
+#ifndef PARTICLEFILTERMIXING_H
+#define PARTICLEFILTERMIXING_H
+
+#include <vector>
+#include <memory>
+
+#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 <typename State, typename Control, typename Observation>
+    class ParticleFilterMixing {
+
+    private:
+
+        /** all used particles */
+        std::vector<Particle<State>> particles;
+
+        /** the current calculated estimation */
+        State estimation;
+
+        /** the resampler to use */
+        std::shared_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** the estimation function to use */
+        std::shared_ptr<ParticleFilterEstimation<State>> estimator;
+
+        /** the transition function to use */
+        std::shared_ptr<ParticleFilterTransition<State, Control>> transition;
+
+        /** the evaluation function to use */
+        std::shared_ptr<ParticleFilterEvaluation<State, Observation>> evaluation;
+
+        /** the initialization function to use */
+        std::shared_ptr<ParticleFilterInitializer<State>> 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<ParticleFilterInitializer<State>> initializer, double modePosteriorProbability) {
+            this->modePosteriorProbability = modePosteriorProbability;
+
+            particles.resize(numParticles);
+            setInitializier(std::move(initializer));
+            init();
+        }
+
+        /** dtor */
+        ~ParticleFilterMixing() {
+            ;
+        }
+
+        /** access to all particles */
+        const std::vector<Particle<State>>& getParticles() const {
+            return this->particles;
+        }
+
+        void setParticles(const std::vector<Particle<State>>& 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<ParticleFilterResampling<State>> 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<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** get the used transition method */
+        ParticleFilterTransition<State, Control>* getTransition() {
+            return this->transition.get();
+        }
+
+        /** set the evaluation method to use */
+        void setEvaluation(std::shared_ptr<ParticleFilterEvaluation<State, Observation>> 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<ParticleFilterInitializer<State>> 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

smc/filtering/ParticleFilterTransition.h

@@ -0,0 +1,26 @@
+#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H
+#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H
+
+#include <vector>
+#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 <typename State, typename Control>
+	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<Particle<State>>& particles, const Control* control) = 0;
+
+	};
+
+}
+
+#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H

smc/filtering/estimation/ParticleFilterEstimation.h

@@ -0,0 +1,24 @@
+#ifndef PARTICLEFILTERESTIMATION_H
+#define PARTICLEFILTERESTIMATION_H
+
+#include "../../Particle.h"
+#include <vector>
+
+namespace SMC {
+
+	template <typename State>
+	class ParticleFilterEstimation {
+
+	public:
+
+		// dtor
+		virtual ~ParticleFilterEstimation() {;}
+
+		// get the current state estimation for the given particle set
+		virtual State estimate(const std::vector<Particle<State>>& particles) = 0;
+
+	};
+
+}
+
+#endif // PARTICLEFILTERESTIMATION_H

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 <algorithm>
+#include <vector>
+#include "../../Particle.h"
+
+#include "../../../../misc/gnuplot/Gnuplot.h"
+#include "../../../optimization/NumOptVector.h"
+#include "../../../optimization/NumOptAlgoDownhillSimplex.h"
+
+
+
+namespace SMC {
+
+	template <typename State, int numParams>
+	class ParticleFilterEstimationKernelDensity : public ParticleFilterEstimation<State> {
+
+		Gnuplot gp;
+
+		K::NumOptAlgoDownhillSimplex<float, numParams> simplex;
+
+	private:
+
+		class OptFunc  {
+
+		private:
+
+			/** the particles to work on */
+			const std::vector<Particle<State>>& particles;
+
+		public:
+
+			/** ctor */
+			OptFunc(const std::vector<Particle<State>>& 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<State>& p = particles[i];
+					prob += p.state.getKernelDensityProbability(params) * p.weight;
+				}
+
+				// convert probability to "error"
+				return -prob;
+
+			}
+
+		};
+
+
+	public:
+
+		State estimate(std::vector<Particle<State>>& particles) override {
+
+
+			// comparator
+			auto comp = [] (const Particle<State>& p1, const Particle<State>& 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<Particle<State>>& 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

smc/filtering/estimation/ParticleFilterEstimationMax.h

@@ -0,0 +1,37 @@
+#ifndef PARTICLEFILTERESTIMATIONMAX_H
+#define PARTICLEFILTERESTIMATIONMAX_H
+
+#include "ParticleFilterEstimation.h"
+#include <algorithm>
+#include <vector>
+#include "../../Particle.h"
+
+namespace SMC {
+
+	/**
+	 * just use the particle with the maximum weight
+	 * as the currently sestimated state
+	 */
+	template <typename State>
+	class ParticleFilterEstimationMax : public ParticleFilterEstimation<State> {
+
+	public:
+
+		State estimate(const std::vector<Particle<State>>& particles) override {
+
+			// comparator
+			auto comp = [] (const Particle<State>& p1, const Particle<State>& 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

smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h

@@ -0,0 +1,78 @@
+#ifndef PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H
+#define PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H
+
+
+#include <vector>
+#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 <typename State>
+	class ParticleFilterEstimationOrderedWeightedAverage : public ParticleFilterEstimation<State> {
+
+	private:
+
+		const float percent;
+
+	public:
+
+		/** ctor */
+		ParticleFilterEstimationOrderedWeightedAverage(const float percent) : percent(percent) {;}
+
+		State estimate(const std::vector<Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+			static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+			static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+
+			// comparator (highest first)
+			auto comp = [] (const Particle<State>& p1, const Particle<State>& p2) {
+				return p1.weight > p2.weight;
+			};
+
+			// create a copy
+			std::vector<Particle<State>> 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<State>& 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

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 <algorithm>
+
+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 <typename State>
+    class ParticleFilterEstimationRegionalWeightedAverage : public ParticleFilterEstimation<State> {
+
+	public:
+
+		State estimate(const std::vector<Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+			static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+			static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+
+			//1) get the most probable particle
+			const auto comp = [] (const Particle<State>& p1, const Particle<State>& p2) {return p1.weight < p2.weight;};
+			const Particle<State>& max = *std::max_element(particles.begin(), particles.end(), comp);
+
+			//2) calculate the regional weighted average
+			double cumWeight = 0;
+			State res;
+			for (const Particle<State>& 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

smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h

@@ -0,0 +1,52 @@
+#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H
+#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H
+
+#include <vector>
+#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 <typename State>
+	class ParticleFilterEstimationWeightedAverage : public ParticleFilterEstimation<State> {
+
+	public:
+
+		State estimate(const std::vector<Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+			static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+			static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+
+			State tmp;
+
+			// calculate weighted average
+			double weightSum = 0;
+			for (const Particle<State>& 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

smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h

@@ -0,0 +1,66 @@
+#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H
+#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H
+
+#include <vector>
+#include <cmath>
+#include <math.h>
+#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 <typename State>
+    class ParticleFilterEstimationWeightedAverageWithAngle : public ParticleFilterEstimation<State> {
+
+	public:
+
+		State estimate(std::vector<Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+			static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+			static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+
+			State tmp;
+
+			// calculate weighted average
+			double weightSum = 0;
+
+            double xAngle = 0;
+            double yAngle = 0;
+			for (const Particle<State>& 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

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 <vector>
+#include <chrono>
+
+/**
+ * interface for all available resampling methods
+ * within the particle filter
+ * @param State the (user-defined) state
+ */
+
+namespace SMC {
+
+	template <typename State> class ParticleFilterResampling {
+
+	public:
+
+		/**
+		 * perform resampling on the given particle-vector
+		 * @param particles the vector of all particles to resample
+		 */
+        virtual void resample(std::vector<Particle<State>>& particles) = 0;
+
+        /**
+         * perform resampling on the given particle-vector
+         * @param particles the vector of all particles to resample
+         */
+        virtual void resample(std::vector<Particle<State>>& particles, double kld, std::vector<Particle<State>>& particlesWifi) {}
+
+	};
+
+}
+
+#endif /* PARTICLEFILTERRESAMPLING_H_ */

smc/filtering/resampling/ParticleFilterResamplingDivergence.h

@@ -0,0 +1,157 @@
+#ifndef PARTICLEFILTERRESAMPLINGDIVERGENCE_H
+#define PARTICLEFILTERRESAMPLINGDIVERGENCE_H
+
+#include <algorithm>
+#include <random>
+
+#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 <typename State>
+    class ParticleFilterResamplingDivergence : public ParticleFilterResampling<State> {
+
+    private:
+
+        /** this is a copy of the particle-set to draw from it */
+        std::vector<Particle<State>> particlesCopy;
+
+        /** this is a copy of the wifi particle-set */
+        std::vector<Particle<State>> particlesWifiCopy;
+
+        /** random number generator */
+        std::minstd_rand gen;
+
+    public:
+
+        /** ctor */
+        ParticleFilterResamplingDivergence() {
+            gen.seed(1234);
+        }
+
+
+        void resample(std::vector<Particle<State>>& particles) override{
+
+        }
+
+        void resample(std::vector<Particle<State>>& particles, double kld, std::vector<Particle<State>>& particlesWifi) override {
+
+            // compile-time sanity checks
+            // TODO: this solution requires EXPLICIT overloading which is bad...
+            //static_assert( HasOperatorAssign<State>::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<State> posteriorParticle = draw(cumWeight);
+                    Particle<State> 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<State>& draw(const double cumWeight) {
+
+            // generate random values between [0:cumWeight]
+            std::uniform_real_distribution<float> 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<State>& 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<State>& drawWifi(const double cumWeight) {
+
+            // generate random values between [0:cumWeight]
+            std::uniform_real_distribution<float> 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<State>& s, const float d) {return s.weight < d;};
+            auto it = std::lower_bound(particlesWifiCopy.begin(), particlesWifiCopy.end(), rand, comp);
+            return *it;
+
+        }
+
+
+    };
+
+
+}
+
+
+
+#endif // PARTICLEFILTERRESAMPLINGDIVERGENCE_H

smc/filtering/resampling/ParticleFilterResamplingKLD.h

@@ -0,0 +1,130 @@
+#ifndef PARTICLEFILTERRESAMPLINGKLD_H
+#define PARTICLEFILTERRESAMPLINGKLD_H
+
+#include <algorithm>
+#include <random>
+
+#include <Indoor/math/random/RandomGenerator.h>
+#include "ParticleFilterResampling.h"
+#include "../../ParticleAssertions.h"
+
+#include <Indoor/math/distribution/NormalCDF.h>
+#include <Indoor/misc/Binning.h>
+
+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 <typename State>
+    class ParticleFilterResamplingKLD : public ParticleFilterResampling<State> {
+
+    private:
+
+        /** this is a copy of the particle-set to draw from it */
+        std::vector<Particle<State>> 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<State> 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<Particle<State>>& 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<double>::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<State>& draw(const double cumWeight) {
+
+            // generate random values between [0:cumWeight]
+            std::uniform_real_distribution<float> 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<State>& s, const float d) {return s.weight < d;};
+            auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
+            return *it;
+
+        }
+    };
+
+
+}
+
+#endif // PARTICLEFILTERRESAMPLINGKLD_H

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 <typename State>
+	class ParticleFilterResamplingLog : public ParticleFilterResamplingSimple<State> {
+
+	public:
+
+		void resample(std::vector<Particle<State>>& particles) override {
+
+			for (Particle<State>& p : particles) {
+				//p.weight = - 1.0 / std::log(p.weight);
+				p.weight = std::pow(p.weight, 0.5);
+			}
+
+			ParticleFilterResamplingSimple<State>::resample(particles);
+
+		}
+
+	};
+
+}
+
+#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H

smc/filtering/resampling/ParticleFilterResamplingNEff.h

@@ -0,0 +1,163 @@
+#ifndef PARTICLEFILTERRESAMPLINGNEFF_H
+#define PARTICLEFILTERRESAMPLINGNEFF_H
+
+
+
+#include <algorithm>
+#include <functional>
+
+#include "ParticleFilterResampling.h"
+#include "../ParticleAssertions.h"
+
+namespace K {
+
+	/**
+	 * TODO
+	 */
+	template <typename State> class ParticleFilterResamplingNEff : public ParticleFilterResampling<State> {
+
+	public:
+
+		using DrawCallback = std::function<void(Particle<State>& 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<Particle<State>>& particles) override {
+
+			// comparator (highest first)
+			static auto comp = [] (const Particle<State>& p1, const Particle<State>& 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<float> 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<Particle<State>>& 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<Particle<State>>& 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<float>& cumWeights) {
+
+			// get the cumulative weight [= last entry]
+			const double cumWeight = cumWeights.back();
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> 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

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 <typename State>
+	class ParticleFilterResamplingNone : public ParticleFilterResampling<State> {
+
+	public:
+
+//		/** dtor */
+//		~ParticleFilterResamplingNone() {;}
+
+		void resample(std::vector<Particle<State>>& particles) override {
+			(void) particles;
+		}
+
+	};
+
+}
+
+#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ */

smc/filtering/resampling/ParticleFilterResamplingPercent.h

@@ -0,0 +1,94 @@
+#ifndef PARTICLEFILTERRESAMPLINGPERCENT_H
+#define PARTICLEFILTERRESAMPLINGPERCENT_H
+
+
+#include <algorithm>
+
+#include "ParticleFilterResampling.h"
+#include "../../ParticleAssertions.h"
+
+namespace SMC {
+
+	/**
+	 * TODO
+	 */
+	template <typename State> class ParticleFilterResamplingPercent : public ParticleFilterResampling<State> {
+
+	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<Particle<State>>& particles) override {
+
+			// comparator (highest first)
+			static auto comp = [] (const Particle<State>& p1, const Particle<State>& 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<int> 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<State>& draw(std::vector<Particle<State>>& copy, const double cumWeight) {
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> 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<State>& s, const float d) {return s.weight < d;};
+			auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
+			return *it;
+
+		}
+
+
+
+	};
+
+}
+
+#endif // PARTICLEFILTERRESAMPLINGPERCENT_H

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 <algorithm>
+#include <random>
+
+#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 <typename State>
+	class ParticleFilterResamplingSimple : public ParticleFilterResampling<State> {
+
+	private:
+
+		/** this is a copy of the particle-set to draw from it */
+		std::vector<Particle<State>> particlesCopy;
+
+		/** random number generator */
+		std::minstd_rand gen;
+
+	public:
+
+		/** ctor */
+		ParticleFilterResamplingSimple() {
+			gen.seed(1234);
+		}
+
+		void resample(std::vector<Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			// TODO: this solution requires EXPLICIT overloading which is bad...
+            // static_assert( HasOperatorAssign<State>::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<State>& draw(const double cumWeight) {
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> 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<State>& 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_ */

smc/filtering/resampling/ParticleFilterResamplingWheel.h

@@ -0,0 +1,95 @@
+#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_
+#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_
+
+#include <algorithm>
+#include <vector>
+
+#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 <typename State>
+	class ParticleFilterResamplingWheel: public ParticleFilterResampling<State> {
+
+	private:
+
+		/** this is a copy of the particle-set to draw from it */
+		std::vector<Particle<State>> particlesCopy;
+
+	public:
+
+		void resample(std::vector<Particle<State>>& particles) override {
+
+			uint64_t start = K::Time::getTimeMS();
+
+			// compile-time sanity checks
+			// TODO: this solution requires EXPLICIT overloading which is bad...
+			//static_assert( HasOperatorAssign<State>::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<State>& p1, const Particle<State>& p2) {return p1.weight < p2.weight;};
+			const Particle<State> 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_ */

smc/merging/InteractingMultipleModelParticleFilter.h

@@ -0,0 +1,135 @@
+#ifndef INTERACTINGMULTIPLEMODELPARTICLEFILTER_H
+#define INTERACTINGMULTIPLEMODELPARTICLEFILTER_H
+
+#include <vector>
+
+#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 <typename State, typename Control, typename Observation>
+    class InteractingMultipleModelParticleFilter {
+
+    private:
+
+        /** the used particle filters */
+        std::vector<ParticleFilterMixing<State, Control, Observation>> modes;
+
+        /** the mixing function to use */
+        std::unique_ptr<MixingSampler<State, Control, Observation>> mixing;
+
+        /** the function for calculating markov chain transition*/
+        std::unique_ptr<MarkovTransitionProbability<State, Control, Observation>> transition;
+
+        /** the function for calculating a joint estimation */
+        std::unique_ptr<JointEstimation<State, Control, Observation>> 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<ParticleFilterMixing<State, Control, Observation>>& 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<MixingSampler<State, Control, Observation>> 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<MarkovTransitionProbability<State, Control, Observation>> 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<JointEstimation<State, Control, Observation>> estimation) {
+            Assert::isNotNull(estimation, "setJointEstimation() MUST not be called with a nullptr!");
+            this->estimation = std::move(estimation);
+        }
+
+        const std::vector<ParticleFilterMixing<State, Control, Observation>>& 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<State, Control, Observation>& 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<State, Control, Observation>& filter : modes){
+
+                likelihoodSum += filter.getWeightSum() * filter.getPredictedModeProbability();
+            }
+
+            // set the last posterior probability p(m_t-1 | Z_t-1) for all modes
+            for(ParticleFilterMixing<State, Control, Observation>& filter : modes){
+
+                filter.setModePosteriorProbability(likelihoodSum);
+            }
+        }
+    };
+
+}
+
+#endif // INTERACTINGMULTIPLEMODELPARTICLEFILTER_H

smc/merging/MarkovTransitionProbability.h

@@ -0,0 +1,30 @@
+#ifndef MARKOVTRANSITIONPROBABILITY_H
+#define MARKOVTRANSITIONPROBABILITY_H
+
+#include "../filtering/ParticleFilterMixing.h"
+#include <eigen3/Eigen/Dense>
+
+namespace SMC {
+
+
+    /**
+     * interface for all available transition probability calculations
+     * within the IMMPF
+     */
+    template <typename State, typename Control, typename Observation>
+    class MarkovTransitionProbability {
+
+    public:
+
+        /**
+         * perform the calculation of the transition matrix
+         * @param vector of modes / particle filters
+         */
+         virtual Eigen::MatrixXd update(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes, const Observation& obs) = 0;
+
+    };
+
+}
+
+
+#endif // MARKOVTRANSITIONPROBABILITY_H

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 <typename State, typename Control, typename Observation>
+    class JointEstimation {
+
+    public:
+
+        // get the current state estimation for the given particle set
+        virtual const State estimate(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes) = 0;
+
+    };
+
+}
+
+#endif // JOINTESTIMATION_H

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 <typename State, typename Control, typename Observation>
+    class JointEstimationPosteriorOnly
+    : public JointEstimation<State, Control, Observation> {
+
+    public:
+
+        // get the current state estimation for the given particle set
+        const State estimate(std::vector<ParticleFilterMixing<State, Control, Observation>>& modes) override {
+            return modes[0].getEstimation();
+        }
+
+    };
+
+}
+
+#endif // JOINTESTIMATIONPOSTERIORONLY_H

smc/merging/mixing/MixingSampler.h

@@ -0,0 +1,29 @@
+#ifndef MIXINGSAMPLER_H
+#define MIXINGSAMPLER_H
+
+#include "../../filtering/ParticleFilterMixing.h"
+#include <eigen3/Eigen/Dense>
+
+namespace SMC {
+
+
+    /**
+     * interface for all available resampling methods
+     * within the particle filter
+     */
+    template <typename State, typename Control, typename Observation>
+    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<ParticleFilterMixing<State, Control, Observation>>& modes, Eigen::MatrixXd transitionProbabilityMatrix) = 0;
+
+    };
+
+}
+
+#endif // MIXINGSAMPLER_H

smc/merging/mixing/MixingSamplerDivergency.h

@@ -0,0 +1,157 @@
+#ifndef MIXINGSAMPLERDIVERGENCY_H
+#define MIXINGSAMPLERDIVERGENCY_H
+
+#include "MixingSampler.h"
+
+#include <algorithm>
+#include <random>
+#include <eigen3/Eigen/Dense>
+
+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 <typename State, typename Control, typename Observation> class MixingSamplerDivergency
+        : public MixingSampler<State, Control, Observation> {
+
+    /** random number generator */
+    std::minstd_rand genNorm;
+    std::minstd_rand genPart;
+
+    /** copy of the modes with cumulative probabilities for easy drawing*/
+    std::vector<ParticleFilterMixing<State, Control, Observation>> copyModes;
+
+    public:
+
+         void mixAndSample(std::vector<ParticleFilterMixing<State, Control, Observation>>& 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<State, Control, Observation>& copyFilter : copyModes){
+                double cumWeight = 0;
+                std::vector<Particle<State>> 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<State, Control, Observation>& 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<State, Control, Observation>& 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<State, Control, Observation>& 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<Particle<State>> 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<State, Control, Observation>& drawMode(const double cumTransitionModeProbabilities){
+
+             // generate random values between [0:cumWeight]
+             std::uniform_real_distribution<float> 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<State, Control, Observation>& 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<State>& drawParticle(ParticleFilterMixing<State, Control, Observation>& filter) {
+
+            double weights = filter.getParticles().back().weight;
+
+            // generate random values between [0:cumWeight]
+            std::uniform_real_distribution<float> 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<State>& 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

smc/sampling/CumulativeSampler.h

@@ -0,0 +1,100 @@
+#ifndef CUMULATIVESAMPLER_H
+#define CUMULATIVESAMPLER_H
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include "../Particle.h"
+#include "ParticleTrajectorieSampler.h"
+
+namespace SMC {
+
+	/**
+     * drawing trajectories using a cumulative drawer
+	 */
+    template <typename State>
+    class CumulativeSampler : public ParticleTrajectorieSampler<State> {
+
+	public:
+
+        /** ctor */
+        CumulativeSampler(){
+
+        }
+
+        /** draw a single particle */
+        Particle<State> drawSingleParticle(std::vector<Particle<State>> const& particles){
+
+            // ensure the copy vector has the same size as the real particle vector
+            std::vector<Particle<State>> 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<Particle<State>> drawTrajectorie(std::vector<Particle<State>> const& particles, const int numRealizations){
+
+            // ensure the copy vector has the same size as the real particle vector
+            std::vector<Particle<State>> 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<Particle<State>> 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<State> draw(const double cumWeight, std::vector<Particle<State>> const& cumParticles, std::vector<Particle<State>> 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<State>& 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<State> drawnParticle = *it;
+            drawnParticle.weight = origParticles[idx].weight;
+
+            return drawnParticle;
+
+        }
+	};
+
+}
+
+#endif // ARTIFICIALDISTRIBUTION_H

smc/sampling/ParticleTrajectorieSampler.h

@@ -0,0 +1,31 @@
+#ifndef PARTICLETRAJECTORIESAMPLER_H
+#define PARTICLETRAJECTORIESAMPLER_H
+
+#include <vector>
+#include "..//Particle.h"
+
+namespace SMC {
+
+	/**
+     * interface for Sampling Trajectories of Particles
+	 */
+    template <typename State>
+    class ParticleTrajectorieSampler {
+
+	public:
+
+        /** draw a single particle */
+        virtual Particle<State> drawSingleParticle(std::vector<Particle<State>> const& particles) = 0;
+
+        /** draw a trajectorie of all incoming particles / like resampling*/
+        virtual std::vector<Particle<State>> drawTrajectorie(std::vector<Particle<State>>const& particles, const int num) = 0;
+
+    private:
+
+        /** function for drawing particles */
+        virtual Particle<State> draw(const double cumWeight, std::vector<Particle<State>> const& cumParticles, std::vector<Particle<State>> const& origParticles) = 0;
+	};
+
+}
+
+#endif // ARTIFICIALDISTRIBUTION_H

smc/smoothing/ArtificialDistribution.h

@@ -0,0 +1,24 @@
+#ifndef ARTIFICIALDISTRIBUTION_H
+#define ARTIFICIALDISTRIBUTION_H
+
+#include <vector>
+#include "../Particle.h"
+
+namespace SMC {
+
+	/**
+     * interface for artificial distributions
+	 */
+    template <typename State>
+    class ArtificialDistribution {
+
+	public:
+
+        /** calculate the probability/density*/
+        virtual double calculate(Particle<State> const& particle) = 0;
+
+	};
+
+}
+
+#endif // ARTIFICIALDISTRIBUTION_H

smc/smoothing/BackwardFilter.h

@@ -0,0 +1,68 @@
+#ifndef BACKWARDFILTER_H
+#define BACKWARDFILTER_H
+
+
+#include <vector>
+#include <memory>
+
+#include "BackwardFilterTransition.h"
+#include "ForwardFilterHistory.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 <typename State, typename Control, typename Observation>
+        class BackwardFilter {
+
+        public:
+
+            /**
+             * @brief updating the backward filter (smoothing) using the informations made in the forward step (filtering)
+             * @param forwardFilter is a given ForwardFilterHistory containing all Filtering steps made.
+             * @return return the last smoothed estimation ordered backwards in time direction.
+             */
+            virtual State update(ForwardFilterHistory<State, Control, Observation>& forwardFilter, int lag = 0) = 0;
+
+            /**
+             * @brief getEstimations
+             * @return vector with all estimations running backward in time. T -> 0
+             */
+            virtual std::vector<State> getEstimations() = 0;
+
+            /** access to all backward / smoothed particles */
+            virtual const std::vector<std::vector<Particle<State>>>& getbackwardParticles() = 0;
+
+            /** set the estimation method to use */
+            virtual void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> estimation) = 0;
+
+            /** set the transition method to use */
+            virtual void setTransition(std::unique_ptr<BackwardFilterTransition<State, Control>> transition) = 0;
+
+            /** set the resampling method to use */
+            virtual void setResampling(std::unique_ptr<ParticleFilterResampling<State>> 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<ParticleTrajectorieSampler<State>> sampler) { (void) sampler; };
+
+            /** get the used transition method */
+            virtual BackwardFilterTransition<State, Control>* getTransition() = 0;
+
+    };
+}
+
+#endif // BACKWARDFILTER_H

smc/smoothing/BackwardFilterTransition.h

@@ -0,0 +1,33 @@
+#ifndef BACKWARDFILTERTRANSITION_H
+#define BACKWARDFILTERTRANSITION_H
+
+#include <vector>
+#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 <typename State, typename Control>
+    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<std::vector<double>> transition(std::vector<Particle<State>> const& toBeSmoothedParticles_qt, std::vector<Particle<State>>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<Particle<State>> transition(std::vector<Particle<State>> const& toBeSmoothedParticles_qt, std::vector<Control> const& controls_1T) = 0;
+    };
+
+}
+
+#endif // BACKWARDFILTERTRANSITION_H

smc/smoothing/BackwardSimulation.h

@@ -0,0 +1,282 @@
+/*
+ * CondensationBackwardFilter.h
+ *
+ *  Created on: Jun 23, 2015
+ *      Author: Toni Fetzer
+ */
+
+#ifndef BACKWARDSIMULATION_H_
+#define BACKWARDSIMULATION_H_
+
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#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 <typename State, typename Control, typename Observation>
+    class BackwardSimulation : public BackwardFilter<State, Control, Observation>{
+
+    private:
+
+        /** all smoothed particles T -> 1*/
+        std::vector<std::vector<Particle<State>>> backwardParticles;
+
+        /** all estimations calculated */
+        std::vector<State> estimatedStates;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State, Control>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** the sampler for drawing trajectories */
+        std::unique_ptr<ParticleTrajectorieSampler<State>> 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);
+            firstFunctionCall = true;
+        }
+
+        /** dtor */
+        ~BackwardSimulation() {
+            backwardParticles.clear();
+            estimatedStates.clear();
+        }
+
+        /** access to all backward / smoothed particles */
+        const std::vector<std::vector<Particle<State>>>& getbackwardParticles() {
+            return backwardParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> 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<BackwardFilterTransition<State, Control>> 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<ParticleFilterResampling<State>> 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<ParticleTrajectorieSampler<State>> 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<State, Control>* getTransition() {
+            return this->transition.get();
+        }
+
+        /**
+         * @brief update
+         * @param forwardHistory
+         * @return
+         */
+        State update(ForwardFilterHistory<State, Control, Observation>& forwardHistory, int lag = 666) {
+
+            // 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!");
+
+            //init for backward filtering
+            std::vector<Particle<State>> smoothedParticles;
+            smoothedParticles.reserve(numRealizations);
+            firstFunctionCall = true;
+
+            if(lag == 666){
+                lag = forwardHistory.size() - 1;
+            }
+
+            //check if we have enough data for lag
+            if(forwardHistory.size() <= lag){
+                lag = forwardHistory.size() - 1;
+            }
+
+            //iterate through all forward filtering steps
+            for(int i = 0; i <= lag; ++i){
+                std::vector<Particle<State>> forwardParticles = forwardHistory.getParticleSet(i);
+
+                //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);
+
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                    continue;
+                }
+
+                // compute weights using the transition model
+                // transitionWeigths[numRealizations][numParticles]
+                std::vector<std::vector<double>> 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<Particle<State>> 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 k = 0;
+                    for (auto& w : transitionWeights.at(j)) {
+
+                        // multiply the weight of the particles at time t and normalize
+                        smoothedWeights.at(k).weight = (smoothedWeights.at(k).weight * w);
+                        if(smoothedWeights.at(k).weight != smoothedWeights.at(k).weight) {throw "detected NaN";}
+
+                        // iter
+                        ++k;
+                    }
+
+                    //get the sum of all weights
+                    auto lambda = [](double current, const Particle<State>& 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 l = 0; l < smoothedWeights.size(); ++l){
+                            smoothedWeights.at(l).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?
+                    Assert::doThrow("Warning - Resampling is not yet implemented!");
+                }
+
+                // push_back the smoothedParticles
+                backwardParticles.push_back(smoothedParticles);
+
+                // estimate the current state
+                if(lag == (forwardHistory.size() - 1) ){ //fixed lag
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                }
+                else if (i == lag) { //fixed interval
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                }
+
+            }
+
+            //return the calculated estimations
+            // TODO: Wir interessieren uns beim fixed-lag smoothing immer nur für die letzte estimation und den letzten satz gesmoothet particle da wir ja weiter vorwärts in der Zeit gehen
+            // und pro zeitschritt ein neues particle set hinzu kommt. also wenn lag = 3, dann smoothen wir t - 3 und sollten auch nur die estimation von t-3 und das particle set von t-3 abspeichern
+            //
+            // beim interval smoothing dagegen interessieren uns alle, da ja keine neuen future informationen kommen und wir einfach sequentiell zurück in die zeit wandern.
+            //
+            // lösungsvorschlag.
+            // - observer-pattern? immer wenn eine neue estimation und ein neues particle set kommt, sende. (wird nix bringen, da keine unterschiedlichen threads?)
+            // - wie vorher machen, also pro update eine estimation aber jedem update einfach alle observations und alle controls mitgeben?!?!?
+            // - ich gebe zusätzlich den lag mit an. dann kann die forwardfilterhistory auch ständig alles halten. dann gibt es halt keine ständigen updates, sondern man muss die eine
+            // berechnung abwarten. eventl. eine art ladebalken hinzufügen. (30 von 120 timestamps done) (ich glaube das ist die beste blackboxigste version) man kann den lag natürlich auch beim
+            // init des backwardsimulation objects mit übergeben. ABER: damit könntem an kein dynamic-lag smoothing mehr machen. also lieber variable lassen :). durch den lag wissen wir einfach was wir
+            // genau in estimatedStates und backwardParticles speichern müssen ohne über das problem oben zu stoßen. haben halt keine ständigen updates. observer-pattern hier nur bei mehrere threads,
+            // das wäre jetzt aber overkill und deshalb einfach ladebalken :):):):)
+            // - oder man macht einfach zwei update funktionen mit den beiden möglichkeiten. halte ich aber für nen dummen kompromiss. )
+
+            // würde es sinn machen, die estimations auch mit zu speichern?
+
+            return estimatedStates.back();
+
+        }
+
+        std::vector<State> getEstimations(){
+            return estimatedStates;
+        }
+
+
+    };
+
+
+}
+
+
+#endif /* BACKWARDSIMULATION_H_ */

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 <vector>
+#include <memory>
+
+#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 <typename State>
+    class CondensationBackwardFilter : public BackwardFilter<State> {
+
+    private:
+
+        /** all smoothed particles 1 -> T*/
+        std::vector<std::vector<Particle<State>>> backwardParticles;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> 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<std::vector<Particle<State>>>& getbackwardParticles() {
+            return backwardParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> 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<BackwardFilterTransition<State>> 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<ParticleFilterResampling<State>> 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<State>* 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<Particle<State>> 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<Particle<State>> 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<std::vector<double>> predictionProbabilities = transition->transition(forwardParticles, backwardParticles.back());
+
+            // calculate the correction factors
+            std::vector<double> 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<Particle<State>> 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_ */

smc/smoothing/FastKDESmoothing.h

@@ -0,0 +1,271 @@
+#ifndef FASTKDESMOOTHING_H
+#define FASTKDESMOOTHING_H
+
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "BackwardFilterTransition.h"
+#include "BackwardFilter.h"
+
+#include "../Particle.h"
+
+#include "../../floorplan/v2/FloorplanHelper.h";
+#include "../../grid/Grid.h";
+
+#include "../filtering/resampling/ParticleFilterResampling.h"
+#include "../filtering/estimation/ParticleFilterEstimation.h"
+#include "../filtering/ParticleFilterEvaluation.h"
+#include "../filtering/ParticleFilterInitializer.h"
+#include "../filtering/ParticleFilterTransition.h"
+
+#include "../sampling/ParticleTrajectorieSampler.h"
+
+#include "../../math/boxkde/benchmark.h"
+#include "../../math/boxkde/DataStructures.h"
+#include "../../math/boxkde/Image2D.h"
+#include "../../math/boxkde/BoxGaus.h"
+#include "../../math/boxkde/Grid2D.h"
+
+#include "../../Assertions.h"
+namespace SMC {
+
+    template <typename State, typename Control, typename Observation>
+    class FastKDESmoothing : public BackwardFilter<State, Control, Observation>{
+
+    private:
+
+        /** all smoothed particles T -> 1*/
+        std::vector<std::vector<Particle<State>>> backwardParticles;
+
+        /** all estimations calculated */
+        std::vector<State> estimatedStates;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<ParticleFilterTransition<State, Control>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** the sampler for drawing trajectories */
+        std::unique_ptr<ParticleTrajectorieSampler<State>> sampler;
+
+        /** the percentage-of-efficient-particles-threshold for resampling */
+        double nEffThresholdPercent = 0.25;
+
+        /** number of realizations to be calculated */
+        int numParticles;
+
+        /** is update called the first time? */
+        bool firstFunctionCall;
+
+        /** boundingBox for the boxKDE */
+        BoundingBox<float> bb;
+
+        /** histogram/grid holding the particles*/
+        Grid2D<float> grid;
+
+        /** bandwith for KDE */
+        Point2 bandwith;
+
+    public:
+
+        /** ctor */
+        FastKDESmoothing(int numParticles, const Floorplan::IndoorMap* map, const int gridsize_cm, const Point2 bandwith) {
+            this->numParticles = numParticles;
+            //backwardParticles.reserve(numParticles);
+            this->firstFunctionCall = true;
+
+            const Point3 maxBB = FloorplanHelper::getBBox(map).getMax() * 100.0;
+            const Point3 minBB = FloorplanHelper::getBBox(map).getMin() * 100.0;
+            this->bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
+
+            // Create histogram
+            size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_cm);
+            size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_cm);
+            this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
+
+            this->bandwith = bandwith;
+        }
+
+        /** dtor */
+        ~FastKDESmoothing() {
+            backwardParticles.clear();
+            estimatedStates.clear();
+        }
+
+        /** access to all backward / smoothed particles */
+        const std::vector<std::vector<Particle<State>>>& getbackwardParticles() {
+            return backwardParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> transition) {
+            Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!");
+            this->transition = std::move(transition);
+        }
+
+        /** set the transition method to use */
+        void setTransition(std::unique_ptr<BackwardFilterTransition<State, Control>> transition) {
+            Assert::doThrow("Do not use a backward transition for fast smoothing! Forward Transition");
+            //TODO: two times setTransition is not the best solution
+        }
+        /** set the resampling method to use */
+        void setResampling(std::unique_ptr<ParticleFilterResampling<State>> 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<ParticleTrajectorieSampler<State>> 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<State, Control>* getTransition() {
+            return nullptr;
+        }
+
+        /**
+         * @brief update
+         * @param forwardHistory
+         * @return
+         */
+        State update(ForwardFilterHistory<State, Control, Observation>& forwardHistory, int lag = 666) {
+
+            // 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!");
+
+            //init for backward filtering
+            std::vector<Particle<State>> smoothedParticles;
+            smoothedParticles.reserve(numParticles);
+            firstFunctionCall = true;
+
+            // if no lag is given, we have a fixed interval smoothing
+            if(lag == 666){
+                lag = forwardHistory.size() - 1;
+            }
+
+            //check if we have enough data for lag
+            if(forwardHistory.size() <= lag){
+                lag = forwardHistory.size() - 1;
+            }
+
+            //iterate through all forward filtering steps
+            for(int i = 0; i <= lag; ++i){
+                std::vector<Particle<State>> forwardParticlesForTransition_t1 = forwardHistory.getParticleSet(i);
+
+                //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(forwardParticlesForTransition_t1, numParticles);
+
+                    firstFunctionCall = false;
+                    backwardParticles.push_back(smoothedParticles);
+
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                    continue;
+                }
+
+                // transition p(q_t+1* | q_t): so we are performing again a forward transition step.
+                // we are doing this to track single particles between two timesteps! normaly, resampling would destroy
+                // any identifier given to particles.
+                // Node: at this point we can integrate future observation and control information for better smoothing
+                Control controls = forwardHistory.getControl(i-1);
+                Observation obs = forwardHistory.getObservation(i-1);
+                transition->transition(forwardParticlesForTransition_t1, &controls);
+
+                // KDE auf q_t+1 Samples = p(q_t+1 | o_1:T) - Smoothed samples from the future
+                grid.clear();
+                for (Particle<State> p : backwardParticles.back())
+                {
+                    grid.add(p.state.position.x_cm, p.state.position.y_cm, p.weight);
+                }
+
+                int nFilt = 3;
+                float sigmaX = bandwith.x / grid.binSizeX;
+                float sigmaY = bandwith.y / grid.binSizeY;
+                BoxGaus<float> boxGaus;
+                boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
+
+                // Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T)
+                // Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation
+                smoothedParticles = forwardHistory.getParticleSet(i);
+                for(Particle<State> p : smoothedParticles){
+                    p.weight = p.weight * grid.fetch(p.state.position.x_cm, p.state.position.y_cm);
+                    //Assert::isNot0(p.weight, "smoothed particle has zero weight");
+                }
+
+                //normalization
+                auto lambda = [](double current, const Particle<State>& a){return current + a.weight; };
+                double weightSumSmoothed = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda);
+
+                if(weightSumSmoothed != 0.0){
+
+                    for (Particle<State> p : smoothedParticles){
+                        p.weight /= weightSumSmoothed;
+                    }
+
+                    //check if normalization worked
+                    double normWeightSum = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda);
+                    Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1");
+                } else {
+                    Assert::doThrow("Weight Sum of smoothed particles is zero!");
+                }
+
+
+                if(resampler)
+                {
+                    //TODO - does this even make sense?
+                    Assert::doThrow("Warning - Resampling is not yet implemented!");
+                }
+
+                // push_back the smoothedParticles
+                backwardParticles.push_back(smoothedParticles);
+
+                // estimate the current state
+                if(lag == (forwardHistory.size() - 1) ){ //fixed interval
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                }
+                else if (i == lag) { //fixed lag
+                    State est = estimation->estimate(smoothedParticles);
+                    estimatedStates.push_back(est);
+                }
+
+            }
+            return estimatedStates.back();
+
+        }
+
+        std::vector<State> getEstimations(){
+            return estimatedStates;
+        }
+
+
+    };
+
+}
+#endif // FASTKDESMOOTHING_H

smc/smoothing/ForwardFilterHistory.h

@@ -0,0 +1,148 @@
+#ifndef FORWARDFILTERHISTORY_H
+#define FORWARDFILTERHISTORY_H
+
+#include <vector>
+#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 <typename State, typename Control, typename Observation>
+    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<Timestamp> timestamps;
+        std::vector<std::vector<Particle<State>>> particleSets;
+        std::vector<Control> controls;
+        std::vector<Observation> observations;
+
+    public:
+
+        ForwardFilterHistory(){
+            //empty ctor
+        }
+
+        void add(Timestamp time, std::vector<Particle<State>> set, Control control, Observation obs){
+
+            // Is empty? Null? 0?`etc.
+            Assert::isNot0(time.ms(), "Timestamp is 0");
+            if(set.empty()){Assert::doThrow("Particle Set is Empty");}
+
+            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<Particle<State>> getParticleSet(int idx = 0){
+            return particleSets.at(particleSets.size() - 1 - idx);
+        }
+
+        /**
+         * @brief getControl from [latestFilterUpdate - @param idx]
+         * @return const controls object
+         */
+        const Control getControl(int idx = 0){
+            return controls.at(controls.size() - 1 - idx);
+        }
+
+        /**
+         * @brief getObservationf rom [latestFilterUpdate - @param idx]
+         * @return const obervations object
+         */
+        const Observation getObservation(int idx = 0){
+            return observations.at(observations.size() - 1 - idx);
+        }
+
+        /**
+         * @brief Return the timestamp from [latestFilterUpdate - @param idx]
+         * @return returns a Timstampf object
+         */
+        const Timestamp getTimestamp(int idx = 0){
+            return timestamps.at(timestamps.size() - 1 - idx);
+        }
+
+        /**
+         * @brief size
+         * @return num of particleSets size
+         */
+        int size(){
+            return particleSets.size();
+        }
+
+        /**
+         * @brief getLatestParticleSet Reference
+         * @return return particle set Note: c11 std::move by vector
+         */
+        std::vector<Particle<State>> 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();
+        }
+
+        /**
+         * @brief getFirstimestamp
+         * @return const Timestamp object
+         */
+        const Timestamp getFirstTimestamp(){
+            return timestamps.front();
+        }
+
+
+
+    };
+
+}
+
+#endif // FORWARDFILTERHISTORY_H

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 <vector>
+#include <memory>
+
+#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 <typename State>
+    class TwoFilterSmoothing {
+
+    private:
+
+        /** all smoothed particles 1 -> T*/
+        std::vector<std::vector<Particle<State>>> smoothedParticles;
+
+        /** the estimation function to use */
+        std::unique_ptr<ParticleFilterEstimation<State>> estimation;
+
+        /** the transition function to use */
+        std::unique_ptr<BackwardFilterTransition<State>> transition;
+
+        /** the resampler to use */
+        std::unique_ptr<ParticleFilterResampling<State>> resampler;
+
+        /** artificial distribuation */
+        std::unique_ptr<ArtificialDistribution<State>> 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<std::vector<Particle<State>>>& getsmoothedParticles() {
+            return smoothedParticles;
+        }
+
+        /** set the estimation method to use */
+        void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> 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<BackwardFilterTransition<State>> 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<ParticleFilterResampling<State>> resampler) {
+            Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!");
+            this->resampler = std::move(resampler);
+        }
+
+        void setArtificialDistribution(std::unique_ptr<ArtificialDistribution<State>> 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<State>* 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<Particle<State>> const& forwardParticles, std::vector<Particle<State>> 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<std::vector<double>> predictionProbabilities = transition->transition(forwardParticles, backwardParticles);
+
+            //we are using the forwardparticles to re-weight the backward filter (other direction also possible?)
+            std::vector<Particle<State>> 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_ */

tests/math/TestDistribution.cpp

@@ -69,7 +69,33 @@ TEST(Distribution, VonMises) {
 }
 
 
+TEST(Distribution, normalCDF1) {
 
+    Distribution::NormalCDF<double> nd(0,1);
+    ASSERT_NEAR(0.5, nd.getProbability(0), 0.00001);
+    ASSERT_NEAR(0.5, Distribution::NormalCDF<double>::getProbability(0, 1, 0), 0.00001);
+
+    ASSERT_NEAR(1.0, nd.getProbability(5), 0.00001);
+    ASSERT_NEAR(1.0, Distribution::NormalCDF<double>::getProbability(0, 1, 5), 0.00001);
+
+    ASSERT_NEAR(0.0, nd.getProbability(-5), 0.00001);
+    ASSERT_NEAR(0.0, Distribution::NormalCDF<double>::getProbability(0, 1, -5), 0.00001);
+
+}
+
+TEST(Distribution, normalCDF2) {
+
+    Distribution::NormalCDF<double> nd(3,1);
+    ASSERT_NEAR(0.5, nd.getProbability(3), 0.00001);
+    ASSERT_NEAR(0.5, Distribution::NormalCDF<double>::getProbability(3, 1, 3), 0.00001);
+
+    ASSERT_NEAR(1.0, nd.getProbability(3+5), 0.00001);
+    ASSERT_NEAR(1.0, Distribution::NormalCDF<double>::getProbability(3, 1, 3+5), 0.00001);
+
+    ASSERT_NEAR(0.0, nd.getProbability(3-5), 0.00001);
+    ASSERT_NEAR(0.0, Distribution::NormalCDF<double>::getProbability(3, 1, 3-5), 0.00001);
+
+}
 
 
 //#include <fstream>

tests/sensors/pressure/TestBarometer.cpp

@@ -79,7 +79,7 @@ TEST(Barometer, LIVE_tendence) {
 
 	}
 
-	sleep(1);
+    sleep(1);
 
 }
 

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 <iostream>
+#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 <chrono>
+#include <thread>
+
+typedef Point TestState;
+
+
+TEST(Particles, init) {
+
+    //create filter
+
+
+}
+
+#endif
+
+#endif

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<MyState> {
+        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
+            for (SMC::Particle<MyState>& p : particles) {
+                p.state.x = 0;
+                p.state.y = 0;
+                p.weight = 1.0 / particles.size();
+            }
+        }
+    };
+
+    class MyInitializer2 : public SMC::ParticleFilterInitializer<MyState> {
+        void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
+            for (SMC::Particle<MyState>& 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<MyState, MyControl, MyObservation> mode1(numParticles, std::unique_ptr<MyInitializer1>(new MyInitializer1()), 0.5);
+        SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode2(numParticles, std::unique_ptr<MyInitializer2>(new MyInitializer2()), 0.5);
+
+        std::vector<SMC::ParticleFilterMixing<MyState, MyControl, MyObservation>> modes;
+        modes.push_back(mode1);
+        modes.push_back(mode2);
+
+
+        SMC::MixingSamplerDivergency<MyState, MyControl, MyObservation> 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