From c8063bc86224b36da1f6741614a15055272f0a52 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 15 Nov 2017 17:43:32 +0100 Subject: [PATCH 1/4] ref #39 #40 moved all stuff left in KLib into Indoor. We are now able to perform localization without the need of KLib. Only K::Gnuplot is needed for drawing, but this will be separated into an own project in the future --- Assertions.h | 2 +- CMakeLists.txt | 3 + grid/walk/GridWalkLightAtTheEndOfTheTunnel.h | 2 +- grid/walk/GridWalkPathControl.h | 2 +- grid/walk/GridWalkPushForward.h | 2 +- grid/walk/GridWalkRandomHeadingUpdate.h | 2 +- grid/walk/GridWalkRandomHeadingUpdateAdv.h | 2 +- grid/walk/GridWalkShortestPathControl.h | 2 +- grid/walk/GridWalkSimpleControl.h | 2 +- grid/walk/GridWalkWeighted.h | 2 +- grid/walk/GridWalkWeighted2.h | 2 +- grid/walk/TestWalkWeighted3.h | 2 +- grid/walk/v2/GridWalker.h | 2 +- grid/walk/v3/Walker.h | 2 +- main.cpp | 6 +- math/Distributions.h | 1 + math/DrawList.h | 8 +- math/Random.h | 28 --- math/distribution/Exponential.h | 4 +- math/distribution/KernelDensity.h | 2 +- math/distribution/Normal.h | 4 +- math/distribution/NormalCDF.h | 65 +++++++ math/distribution/NormalN.h | 4 +- math/distribution/Rectangular.h | 2 +- math/distribution/Region.h | 2 +- math/distribution/Triangle.h | 2 +- math/distribution/Uniform.h | 4 +- math/stats/Statistics.h | 169 ++++++++++++++++++ .../activity/ActivityButterPressurePercent.h | 20 +-- sensors/imu/MagnetometerData.h | 8 +- tests/math/TestDistribution.cpp | 26 +++ tests/math/filter/TestButter.cpp | 2 +- tests/sensors/imu/TestMotionDetection.cpp | 5 +- tests/sensors/imu/TestTurnDetection.cpp | 40 +++-- tests/sensors/pressure/TestBarometer.cpp | 9 +- 35 files changed, 340 insertions(+), 100 deletions(-) delete mode 100644 math/Random.h create mode 100644 math/distribution/NormalCDF.h create mode 100644 math/stats/Statistics.h diff --git a/Assertions.h b/Assertions.h index 98ca4aa..4c727b8 100644 --- a/Assertions.h +++ b/Assertions.h @@ -51,7 +51,7 @@ namespace Assert { if (v != nullptr) {doThrow(err);} } - template static inline void isNotNull(const T v, const STR err) { + template static inline void isNotNull(const T& v, const STR err) { if (v == nullptr) {doThrow(err);} } diff --git a/CMakeLists.txt b/CMakeLists.txt index 908fa41..8a1f58a 100755 --- a/CMakeLists.txt +++ b/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) diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index ae59caa..d59aa97 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -35,7 +35,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index de7d3a4..93ce5fc 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkPushForward.h b/grid/walk/GridWalkPushForward.h index 7d01da3..4234d39 100644 --- a/grid/walk/GridWalkPushForward.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdate.h b/grid/walk/GridWalkRandomHeadingUpdate.h index 2d5b05e..b8520b9 100644 --- a/grid/walk/GridWalkRandomHeadingUpdate.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkRandomHeadingUpdateAdv.h b/grid/walk/GridWalkRandomHeadingUpdateAdv.h index a387755..2135d2b 100644 --- a/grid/walk/GridWalkRandomHeadingUpdateAdv.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkShortestPathControl.h b/grid/walk/GridWalkShortestPathControl.h index 9254fb9..3e2dbc6 100644 --- a/grid/walk/GridWalkShortestPathControl.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index cc3ad88..e4dcb6c 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/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 headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h index cd97f5a..5159398 100644 --- a/grid/walk/GridWalkWeighted.h +++ b/grid/walk/GridWalkWeighted.h @@ -39,7 +39,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/GridWalkWeighted2.h b/grid/walk/GridWalkWeighted2.h index 26a8456..4e44043 100644 --- a/grid/walk/GridWalkWeighted2.h +++ b/grid/walk/GridWalkWeighted2.h @@ -40,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/TestWalkWeighted3.h b/grid/walk/TestWalkWeighted3.h index c8eec62..bfdc3fd 100644 --- a/grid/walk/TestWalkWeighted3.h +++ b/grid/walk/TestWalkWeighted3.h @@ -40,7 +40,7 @@ private: DrawList drawer; /** fast random-number-generator */ - RandomGenerator gen; + Random::RandomGenerator gen; /** 0-mean normal distribution */ std::normal_distribution headingChangeDist = std::normal_distribution(0.0, HEADING_CHANGE_SIGMA); diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index eaf10de..737caa6 100644 --- a/grid/walk/v2/GridWalker.h +++ b/grid/walk/v2/GridWalker.h @@ -19,7 +19,7 @@ private: /** all modules to evaluate */ std::vector*> modules; - RandomGenerator rnd; + Random::RandomGenerator rnd; public: diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 543059b..80a3dea 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -32,7 +32,7 @@ namespace GW3 { template class WalkerDirectDestination : public WalkerBase { - //RandomGenerator rnd; + //Random::RandomGenerator rnd; std::vector*> evals; diff --git a/main.cpp b/main.cpp index 3c2ccd8..939cb5e 100755 --- a/main.cpp +++ b/main.cpp @@ -90,9 +90,9 @@ int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); // skip all tests starting with LIVE_ - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; - //::testing::GTEST_FLAG(filter) = "*Distribution.T*"; + ::testing::GTEST_FLAG(filter) = "*Distribution*"; //::testing::GTEST_FLAG(filter) = "*RingBuffer*"; //::testing::GTEST_FLAG(filter) = "*Grid.*"; @@ -109,7 +109,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; + //::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/math/Distributions.h b/math/Distributions.h index 1f2bb44..40d49a4 100644 --- a/math/Distributions.h +++ b/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 diff --git a/math/DrawList.h b/math/DrawList.h index dd034ad..00d098b 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -3,7 +3,7 @@ #include -#include "Random.h" +#include "random/RandomGenerator.h" #include "../Assertions.h" /** @@ -41,13 +41,13 @@ private: std::vector 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) { ; } diff --git a/math/Random.h b/math/Random.h deleted file mode 100644 index ae8d8c5..0000000 --- a/math/Random.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RANDOM_H -#define RANDOM_H - -#include -#include -#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 diff --git a/math/distribution/Exponential.h b/math/distribution/Exponential.h index db5dabe..39a81fc 100644 --- a/math/distribution/Exponential.h +++ b/math/distribution/Exponential.h @@ -3,7 +3,7 @@ #include #include -#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 dist; public: diff --git a/math/distribution/KernelDensity.h b/math/distribution/KernelDensity.h index a103e3e..21be963 100644 --- a/math/distribution/KernelDensity.h +++ b/math/distribution/KernelDensity.h @@ -8,7 +8,7 @@ #include #include "../../Assertions.h" -#include "../Random.h" +#include "../random/RandomGenerator.h" namespace Distribution { diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index b40ce67..9424974 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -3,7 +3,7 @@ #include #include -#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 dist; public: diff --git a/math/distribution/NormalCDF.h b/math/distribution/NormalCDF.h new file mode 100644 index 0000000..62ba191 --- /dev/null +++ b/math/distribution/NormalCDF.h @@ -0,0 +1,65 @@ +#ifndef NORMALCDF_H +#define NORMALCDF_H + +#include +#include +#include "../../Assertions.h" + +namespace Distribution { + + /** cumulative density version of the normal distribution */ + template 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 diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index 5075dea..db86011 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -7,7 +7,7 @@ #include #include "../../Assertions.h" -#include "../Random.h" +#include "../random/RandomGenerator.h" namespace Distribution { @@ -24,7 +24,7 @@ namespace Distribution { const Eigen::SelfAdjointEigenSolver eigenSolver; Eigen::MatrixXd transform; //can i make this const? - RandomGenerator gen; + Random::RandomGenerator gen; std::normal_distribution<> dist; public: diff --git a/math/distribution/Rectangular.h b/math/distribution/Rectangular.h index 3f3a5fb..45e0b40 100644 --- a/math/distribution/Rectangular.h +++ b/math/distribution/Rectangular.h @@ -4,7 +4,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Region.h b/math/distribution/Region.h index 884cf3f..d21c23b 100644 --- a/math/distribution/Region.h +++ b/math/distribution/Region.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Triangle.h b/math/distribution/Triangle.h index dbffbff..d385848 100644 --- a/math/distribution/Triangle.h +++ b/math/distribution/Triangle.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include "../../Assertions.h" #include "Normal.h" diff --git a/math/distribution/Uniform.h b/math/distribution/Uniform.h index d6011fe..6c17452 100644 --- a/math/distribution/Uniform.h +++ b/math/distribution/Uniform.h @@ -3,7 +3,7 @@ #include #include -#include "../Random.h" +#include "../random/RandomGenerator.h" #include @@ -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::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; diff --git a/math/stats/Statistics.h b/math/stats/Statistics.h new file mode 100644 index 0000000..ed4609b --- /dev/null +++ b/math/stats/Statistics.h @@ -0,0 +1,169 @@ +#ifndef STATISTICS_H +#define STATISTICS_H + +#include +#include +#include +#include + +namespace Stats { + + /** + * store values here and get statistics about their + * avg, median, std-dev, etc. + */ + template 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& 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& 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 values; + + }; + +} + +#endif // STATISTICS_H diff --git a/sensors/activity/ActivityButterPressurePercent.h b/sensors/activity/ActivityButterPressurePercent.h index 80541fc..9dba76b 100644 --- a/sensors/activity/ActivityButterPressurePercent.h +++ b/sensors/activity/ActivityButterPressurePercent.h @@ -139,17 +139,17 @@ public: float densityStairsUp = Distribution::Normal::getProbability(-muStairs, variance, actValue); float densityElevatorUp = Distribution::Normal::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; diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h index 24136c7..ecd02d3 100644 --- a/sensors/imu/MagnetometerData.h +++ b/sensors/imu/MagnetometerData.h @@ -36,7 +36,7 @@ struct MagnetometerData { } float magnitude() const { - return std::sqrt( x*x + y*y + z*z ); + return std::sqrt( x*x + y*y + z*z ); } MagnetometerData& operator += (const MagnetometerData& o) { @@ -73,9 +73,9 @@ private: }; namespace std { - MagnetometerData sqrt(const MagnetometerData& o) { - return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); - } + inline MagnetometerData sqrt(const MagnetometerData& o) { + return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); + } } #endif // INDOOR_IMU_MAGNETOMETERDATA_H diff --git a/tests/math/TestDistribution.cpp b/tests/math/TestDistribution.cpp index 8886d45..68d5655 100644 --- a/tests/math/TestDistribution.cpp +++ b/tests/math/TestDistribution.cpp @@ -69,7 +69,33 @@ TEST(Distribution, VonMises) { } +TEST(Distribution, normalCDF1) { + Distribution::NormalCDF nd(0,1); + ASSERT_NEAR(0.5, nd.getProbability(0), 0.00001); + ASSERT_NEAR(0.5, Distribution::NormalCDF::getProbability(0, 1, 0), 0.00001); + + ASSERT_NEAR(1.0, nd.getProbability(5), 0.00001); + ASSERT_NEAR(1.0, Distribution::NormalCDF::getProbability(0, 1, 5), 0.00001); + + ASSERT_NEAR(0.0, nd.getProbability(-5), 0.00001); + ASSERT_NEAR(0.0, Distribution::NormalCDF::getProbability(0, 1, -5), 0.00001); + +} + +TEST(Distribution, normalCDF2) { + + Distribution::NormalCDF nd(3,1); + ASSERT_NEAR(0.5, nd.getProbability(3), 0.00001); + ASSERT_NEAR(0.5, Distribution::NormalCDF::getProbability(3, 1, 3), 0.00001); + + ASSERT_NEAR(1.0, nd.getProbability(3+5), 0.00001); + ASSERT_NEAR(1.0, Distribution::NormalCDF::getProbability(3, 1, 3+5), 0.00001); + + ASSERT_NEAR(0.0, nd.getProbability(3-5), 0.00001); + ASSERT_NEAR(0.0, Distribution::NormalCDF::getProbability(3, 1, 3-5), 0.00001); + +} //#include diff --git a/tests/math/filter/TestButter.cpp b/tests/math/filter/TestButter.cpp index 1946e1a..d0f39d6 100644 --- a/tests/math/filter/TestButter.cpp +++ b/tests/math/filter/TestButter.cpp @@ -5,7 +5,7 @@ #include "../../../misc/Time.h" #include "../../../math/Interpolator.h" -#include"../../../sensors/pressure/ActivityButterPressure.h" +#include"../../../sensors/activity/ActivityButterPressure.h" #include #include diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp index eb76a40..8ec30bf 100644 --- a/tests/sensors/imu/TestMotionDetection.cpp +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -97,7 +97,8 @@ TEST(MotionDetection, motionAxis) { TEST(MotionDetection, motionAngle) { MotionDetection md; - TurnDetection td; + PoseDetection pd; + TurnDetection td(&pd); //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; @@ -147,7 +148,7 @@ TEST(MotionDetection, motionAngle) { } else if (e.type == Offline::Sensor::ACC) { const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(ts, _acc.data); + pd.addAccelerometer(ts, _acc.data); } else if (e.type == Offline::Sensor::GYRO) { const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; diff --git a/tests/sensors/imu/TestTurnDetection.cpp b/tests/sensors/imu/TestTurnDetection.cpp index 9af88f3..0aa2d69 100644 --- a/tests/sensors/imu/TestTurnDetection.cpp +++ b/tests/sensors/imu/TestTurnDetection.cpp @@ -6,33 +6,34 @@ TEST(TurnDetection, rotationMatrix) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 1, 1, 0; src.normalize(); + Vector3 dst(0, 0, 1); + Vector3 src(1, 1, 0); + src = src.normalized(); // get a matrix that rotates "src" into "dst" - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; - ASSERT_NEAR(dst(0), res(0), 0.01); - ASSERT_NEAR(dst(1), res(1), 0.01); - ASSERT_NEAR(dst(2), res(2), 0.01); + ASSERT_NEAR(dst.x, res.x, 0.01); + ASSERT_NEAR(dst.y, res.y, 0.01); + ASSERT_NEAR(dst.z, res.z, 0.01); } TEST(TurnDetection, gyroRotate) { - Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - Eigen::Vector3f acc; acc << 0, 7.0, 7.0; + Vector3 zAxis(0, 0, 1); + Vector3 acc(0, 7.0, 7.0); - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis); + Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis); - Eigen::Vector3f gyro; gyro << 0, 60, 60; + Vector3 gyro(0, 60, 60); - Eigen::Vector3f gyro2; gyro2 << 0, 0, 84; + Vector3 gyro2(0, 0, 84); - Eigen::Vector3f gyro3 = rot * gyro; + Vector3 gyro3 = rot * gyro; ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0); @@ -41,10 +42,11 @@ TEST(TurnDetection, gyroRotate) { TEST(TurnDetection, xx) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize(); // sample accelerometer readings + Vector3 dst(0, 0, 1); + Vector3 src(0.0, 2.9, -10.0); + src = src.normalized(); // sample accelerometer readings - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); // Eigen::Vector3f x; x << 1, 0, 0; // Eigen::Vector3f z = src.normalized(); @@ -55,14 +57,14 @@ TEST(TurnDetection, xx) { // rot.row(1) = y; // rot.row(2) = z; - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; // ASSERT_NEAR(dst(0), res(0), 0.01); // ASSERT_NEAR(dst(1), res(1), 0.01); // ASSERT_NEAR(dst(2), res(2), 0.01); - Eigen::Vector3f gyro; gyro << 0, 10, 30; + Vector3 gyro(0, 10, 30); - Eigen::Vector3f gyro2 = rot * gyro; + Vector3 gyro2 = rot * gyro; int i = 0; (void) i; } diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 7c3451f..7ad31f6 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -4,8 +4,8 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" -#include "../../../sensors/pressure/ActivityButterPressure.h" -#include "../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../sensors/activity/ActivityButterPressure.h" +#include "../../../sensors/activity/ActivityButterPressurePercent.h" #include @@ -141,9 +141,10 @@ TEST(Barometer, Activity) { double value; while (iss >> ts >> value) { - ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + act.add(Timestamp::fromMS(ts), BarometerData(value)); + Activity currentAct = act.get(); rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); - actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData((int)currentAct))); } } From 95a5c8f34fd0af09e7f5ac4ed50b75100d53d4da Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 15 Nov 2017 17:46:06 +0100 Subject: [PATCH 2/4] #39 #40 git add for last commit --- math/random/DrawList.h | 169 +++++++++++ math/random/DrawWheel.h | 174 +++++++++++ math/random/Normal.h | 27 ++ math/random/RandomGenerator.h | 29 ++ math/random/RandomIterator.h | 118 ++++++++ math/random/Uniform.h | 43 +++ math/random/Unique.h | 26 ++ misc/Binning.h | 114 +++++++ smc/Particle.h | 54 ++++ smc/ParticleAssertions.h | 69 +++++ smc/filtering/ParticleFilter.h | 256 ++++++++++++++++ smc/filtering/ParticleFilterEvaluation.h | 29 ++ smc/filtering/ParticleFilterHistory.h | 283 ++++++++++++++++++ smc/filtering/ParticleFilterInitializer.h | 25 ++ smc/filtering/ParticleFilterMixing.h | 241 +++++++++++++++ smc/filtering/ParticleFilterTransition.h | 26 ++ .../estimation/ParticleFilterEstimation.h | 24 ++ .../ParticleFilterEstimationKernelDensity.h | 160 ++++++++++ .../estimation/ParticleFilterEstimationMax.h | 37 +++ ...leFilterEstimationOrderedWeightedAverage.h | 78 +++++ ...eFilterEstimationRegionalWeightedAverage.h | 60 ++++ .../ParticleFilterEstimationWeightedAverage.h | 52 ++++ ...FilterEstimationWeightedAverageWithAngle.h | 66 ++++ .../resampling/ParticleFilterResampling.h | 43 +++ .../ParticleFilterResamplingDivergence.h | 157 ++++++++++ .../resampling/ParticleFilterResamplingKLD.h | 130 ++++++++ .../resampling/ParticleFilterResamplingLog.h | 31 ++ .../resampling/ParticleFilterResamplingNEff.h | 163 ++++++++++ .../resampling/ParticleFilterResamplingNone.h | 34 +++ .../ParticleFilterResamplingPercent.h | 94 ++++++ .../ParticleFilterResamplingSimple.h | 102 +++++++ .../ParticleFilterResamplingWheel.h | 95 ++++++ .../InteractingMultipleModelParticleFilter.h | 135 +++++++++ smc/merging/MarkovTransitionProbability.h | 30 ++ smc/merging/estimation/JointEstimation.h | 25 ++ .../estimation/JointEstimationPosteriorOnly.h | 28 ++ smc/merging/mixing/MixingSampler.h | 29 ++ smc/merging/mixing/MixingSamplerDivergency.h | 157 ++++++++++ smc/sampling/CumulativeSampler.h | 100 +++++++ smc/sampling/ParticleTrajectorieSampler.h | 31 ++ smc/smoothing/ArtificialDistribution.h | 24 ++ smc/smoothing/BackwardFilter.h | 57 ++++ smc/smoothing/BackwardFilterTransition.h | 33 ++ smc/smoothing/BackwardSimulation.h | 258 ++++++++++++++++ smc/smoothing/CondensationBackwardFilter.h | 226 ++++++++++++++ smc/smoothing/ForwardFilterHistory.h | 142 +++++++++ smc/smoothing/TwoFilterSmoothing.h | 187 ++++++++++++ tests/smc/filtering/TestParticles.cpp | 44 +++ .../mixing/TestMixingSamplerDivergency.cpp | 146 +++++++++ 49 files changed, 4661 insertions(+) create mode 100644 math/random/DrawList.h create mode 100644 math/random/DrawWheel.h create mode 100644 math/random/Normal.h create mode 100644 math/random/RandomGenerator.h create mode 100644 math/random/RandomIterator.h create mode 100644 math/random/Uniform.h create mode 100644 math/random/Unique.h create mode 100644 misc/Binning.h create mode 100644 smc/Particle.h create mode 100644 smc/ParticleAssertions.h create mode 100644 smc/filtering/ParticleFilter.h create mode 100644 smc/filtering/ParticleFilterEvaluation.h create mode 100644 smc/filtering/ParticleFilterHistory.h create mode 100644 smc/filtering/ParticleFilterInitializer.h create mode 100644 smc/filtering/ParticleFilterMixing.h create mode 100644 smc/filtering/ParticleFilterTransition.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimation.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationMax.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h create mode 100644 smc/filtering/resampling/ParticleFilterResampling.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingDivergence.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingKLD.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingLog.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingNEff.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingNone.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingPercent.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingSimple.h create mode 100644 smc/filtering/resampling/ParticleFilterResamplingWheel.h create mode 100644 smc/merging/InteractingMultipleModelParticleFilter.h create mode 100644 smc/merging/MarkovTransitionProbability.h create mode 100644 smc/merging/estimation/JointEstimation.h create mode 100644 smc/merging/estimation/JointEstimationPosteriorOnly.h create mode 100644 smc/merging/mixing/MixingSampler.h create mode 100644 smc/merging/mixing/MixingSamplerDivergency.h create mode 100644 smc/sampling/CumulativeSampler.h create mode 100644 smc/sampling/ParticleTrajectorieSampler.h create mode 100644 smc/smoothing/ArtificialDistribution.h create mode 100644 smc/smoothing/BackwardFilter.h create mode 100644 smc/smoothing/BackwardFilterTransition.h create mode 100644 smc/smoothing/BackwardSimulation.h create mode 100644 smc/smoothing/CondensationBackwardFilter.h create mode 100644 smc/smoothing/ForwardFilterHistory.h create mode 100644 smc/smoothing/TwoFilterSmoothing.h create mode 100644 tests/smc/filtering/TestParticles.cpp create mode 100644 tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp diff --git a/math/random/DrawList.h b/math/random/DrawList.h new file mode 100644 index 0000000..4d252c1 --- /dev/null +++ b/math/random/DrawList.h @@ -0,0 +1,169 @@ +#ifndef K_MATH_RANDOM_DRAWLIST_H +#define K_MATH_RANDOM_DRAWLIST_H + +#include + +/** + * add entries to a list and be able to draw from them depending oh their probability + */ +namespace Random { + + /** + * this class represents one entry within a DrawList. + * such an entry consists of userData denoted by the template argument + * and a probability. + */ + template struct DrawListEntry { + + template friend class DrawList; + friend class DrawList_Cumulative_Test; + + public: + + /** the user data behind this entry */ + Entry entry; + + /** this entry's probability */ + double probability; + + private: + + /** the cumulative probability, tracked among all entries within the DrawList */ + double cumulativeProbability; + + public: + + /** empty ctor */ + DrawListEntry() : + entry(), probability(0), cumulativeProbability(0) {;} + + /** ctor */ + DrawListEntry(const Entry& entry, double probability) : + entry(entry), probability(probability), cumulativeProbability(0) {;} + + /** compare this entrie's summed probability with the given probability */ + bool operator < (double probability) const {return cumulativeProbability < probability;} + + private: + + /** ctor */ + DrawListEntry(Entry& e, double probability, double summedProbability) : + entry(entry), probability(probability), cumulativeProbability(summedProbability) {;} + + + }; + + + + /** + * a DrawList is a data-structure containing entries that have a + * probability assigned to them. + * using the draw() function one may draw from these entries according + * to their assigned probability in O(log(n)) + */ + template class DrawList { + + friend class DrawList_Cumulative_Test; + + public: + + /** ctor */ + DrawList() : sumValid(false) {;} + + + /** append a new entry to the end of the list */ + void push_back(const Entry& entry, const double probability) { + const DrawListEntry dle(entry, probability); + entries.push_back(dle); + sumValid = false; + } + + /** change the entry at the given position. ensure the vector is resized()! */ + void set(const uint32_t idx, const Entry& entry, const double probability) { + entries[idx].entry = entry; + entries[idx].probability = probability; + sumValid = false; + } + + /** resize the underlying vector to hold the given number of entries */ + void resize(const uint32_t numEntries) {entries.resize(numEntries);} + + /** clear all currently inserted entries */ + void clear() {entries.clear();} + + /** does the underlying vector contain any entries? */ + bool empty() const {return entries.empty();} + + /** the number of entries */ + uint32_t size() const {return entries.size();} + + /** draw a random entry from the draw list */ + Entry& draw() { + + // random value between [0, 1] + double rand01 = double(rand()) / double(RAND_MAX); + + return draw(rand01); + + } + + /** draw an entry according to the given probability [0,1] */ + Entry& draw(double rand01) { + + // sanity check + assert(!entries.empty()); + + ensureCumulativeProbability(); + + // random value between [0, summedProbability] + // (this prevents us from norming the list to [0, 1]) + double rand = rand01 * entries[entries.size()-1].cumulativeProbability; + + // binary search for the matching entry O(log(n)) + auto tmp = std::lower_bound(entries.begin(), entries.end(), rand); + return (*tmp).entry; + +// // O(n) +// for (DrawListEntry& dle : entries) { +// if (dle.cumulativeProbability > rand) {return dle.entry;} +// } +// return entries[this->size()-1].entry; + + } + + private: + + /** ensure the cumulative probability is valid. if not -> calculate it */ + void ensureCumulativeProbability() { + + // already valid? + if (sumValid) {return;} + + // calculate the cumulative probability + double sum = 0; + for (DrawListEntry& dle : entries) { + sum += dle.probability; + dle.cumulativeProbability = sum; + } + + // the sum is now valid + sumValid = true; + + } + + private: + + /** all entries within the DrawList */ + std::vector> entries; + + /** track wether the summedProbability is valid or not */ + bool sumValid; + + }; + + + + +} + +#endif // K_MATH_RANDOM_DRAWLIST_H diff --git a/math/random/DrawWheel.h b/math/random/DrawWheel.h new file mode 100644 index 0000000..6e9bbf5 --- /dev/null +++ b/math/random/DrawWheel.h @@ -0,0 +1,174 @@ +#ifndef K_MATH_RANDOM_DRAWLIST_H +#define K_MATH_RANDOM_DRAWLIST_H + +#include +#include "../distribution/Uniform.h" + +/** + * add entries to a list and be able to draw from them depending oh their probability + * + * souces: + * https://www.udacity.com/course/viewer#!/c-cs373/l-48704330/e-48748082/m-48740082 + * https://www.youtube.com/watch?list=PLpUPoM7Rgzi_7YWn14Va2FODh7LzADBSm&feature=player_detailpage&v=kZhOJgooMxI#t=567 + */ +namespace Random { + + /** + * this class represents one entry within a DrawWheel. + * such an entry consists of userData denoted by the template argument + * and a probability. + */ + template struct DrawWheelEntry { + + template friend class DrawWheel; + + public: + + /** the user data behind this entry */ + Entry entry; + + /** this entry's probability */ + double probability; + + public: + + /** empty ctor */ + DrawWheelEntry() : entry(), probability(0) {;} + + /** ctor */ + DrawWheelEntry(const Entry& entry, double probability) : entry(entry), probability(probability) {;} + + }; + + /** + * a DrawWheel is a data-structure containing entries that have a + * probability assigned to them. + * using the draw() function one may draw from these entries according + * to their assigned probability in ~O(n) + */ + template class DrawWheel { + + private: + + /** all entries within the DrawWheel */ + std::vector> entries; + + /** is the current maximum valid? */ + bool maxValid = true; + + /** the maximum weight among all entries */ + double curMax = 0; + + /** the current index within the wheel */ + int curIdx = 0; + + /** the current offset at the wheel's index */ + double curOffset = 0; + + /** draw random numbers for the offset */ + K::UniformDistribution dist; + + + public: + + /** ctor */ + DrawWheel() {;} + + + /** append a new entry to the end of the list */ + void push_back(const Entry& entry, const double probability) { + entries.push_back( DrawWheelEntry(entry, probability) ); + if (curMax < probability) {curMax = probability;} + } + + /** change the entry at the given position. ensure the vector is resized()! */ + void set(const uint32_t idx, const Entry& entry, const double probability) { + entries[idx].entry = entry; + entries[idx].probability = probability; + maxValid = false; + } + + /** resize the underlying vector to hold the given number of entries */ + void resize(const uint32_t numEntries) {entries.resize(numEntries);} + + /** clear all currently inserted entries */ + void clear() {entries.clear();} + + /** does the underlying vector contain any entries? */ + bool empty() const {return entries.empty();} + + /** the number of entries */ + uint32_t size() const {return entries.size();} + + + /** call this once before drawing anything */ + void init() { + + // ensure the maximum number is correct + ensureMaxProbability(); + + // setup the distribution to draw a new offset + dist.reset(0, 2 * curMax); + + // draw starting values + curIdx = K::UniformDistribution::draw( (int)0, (int)entries.size() - 1); + curOffset = dist.draw(); + + } + + /** draw a random entry from the wheel */ + Entry& draw() { + + while(true) { + + // found a suitable particle? use it and draw the next random number + if (entries[curIdx].probability >= curOffset) { + + // next offset + curOffset += dist.draw(); + + // return + return entries[curIdx].entry; + + // weight to small, subtract the elements weight and move on to the next element + } else { + + // remove the current entries probability + curOffset -= entries[curIdx].probability; + + // resume with the next one along the wheel + curIdx = (curIdx + 1) % ((int)entries.size()); + + } + + } + + } + + + + private: + + /** ensure the max probability is valid. if not -> calculate it */ + void ensureMaxProbability() { + + // valid? + if (maxValid) {return;} + + // comparisen + const auto lambda = [] (const DrawWheelEntry& e1, const DrawWheelEntry& e2) {return e1.probability < e2.probability;}; + + // find the largest entry + const DrawWheelEntry max = *std::max_element(entries.begin(), entries.end(), lambda); + this->curMax = max.probability; + + // the max is now valid + maxValid = true; + + } + + }; + +} + +#endif // K_MATH_RANDOM_DRAWLIST_H diff --git a/math/random/Normal.h b/math/random/Normal.h new file mode 100644 index 0000000..f5e09bd --- /dev/null +++ b/math/random/Normal.h @@ -0,0 +1,27 @@ +#ifndef K_MATH_RND_NORMAL_H +#define K_MATH_RND_NORMAL_H + +namespace Random { + + + /** + * provides some common functions + * for handling normal-distributed random numbers + */ + class Normal { + + public: + + /** get normal-distributed random number for given mu/sigma */ + static double get(double mu, double sigma) { + static std::random_device rd; + static std::mt19937 gen(rd()); + std::normal_distribution<> d(mu, sigma); + return d(gen); + } + + }; + +} + +#endif // K_MATH_RND_NORMAL_H diff --git a/math/random/RandomGenerator.h b/math/random/RandomGenerator.h new file mode 100644 index 0000000..4c41f36 --- /dev/null +++ b/math/random/RandomGenerator.h @@ -0,0 +1,29 @@ +#ifndef RANDOM_Random::RandomGenerator_H +#define RANDOM_Random::RandomGenerator_H + +#include +#include + +#ifdef USE_FIXED_SEED + #define RANDOM_SEED 1234 +#else + #define RANDOM_SEED ( std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1) ) +#endif + +namespace Random { + + class RandomGenerator : public std::minstd_rand { + + public: + + /** ctor with default seed */ + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + + /** ctor with custom seed */ + RandomGenerator(result_type) : std::minstd_rand(RANDOM_SEED) {;} + + }; + +} + +#endif // K_MATH_RANDOM_Random::RandomGenerator_H diff --git a/math/random/RandomIterator.h b/math/random/RandomIterator.h new file mode 100644 index 0000000..b37b3e3 --- /dev/null +++ b/math/random/RandomIterator.h @@ -0,0 +1,118 @@ +#ifndef RANDOMITERATOR_H +#define RANDOMITERATOR_H + +#include +#include +#include "../../Assertions.h" + +namespace Random { + + template class RandomIterator { + + private: + + /** the user's data-vector to randomly iterate */ + const std::vector& vec; + + /** the number of random indices */ + int cnt; + + /** the random number generator */ + std::minstd_rand gen; + bool isRandomized = false; + + /** X random indices */ + std::vector indices; + + + public: + + /** ctor */ + RandomIterator(const std::vector& vec, const int cnt) : vec(vec), cnt(cnt) { + + //const uint64_t ts = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + static int seed = 0; ++seed; + gen.seed(seed); + + // sanity check + if ((int)vec.size() < cnt) {throw Exception("number of elements in list is smaller than the requested number to draw");} + if (cnt == 0) {throw Exception("number of elements in list must be at least 1");} + if (vec.empty()) {throw Exception("empty input vector given");} + + indices.resize(cnt); + + } + + /** create random samples (vector-indicies) that are hereafter available for iteration */ + void randomize() { + + // random-number generator between [0:size-1] + std::uniform_int_distribution dist(0, (int) vec.size()-1); + + // ensure we use each vector-index only ONCE + bool used[vec.size()] = {false}; + + // draw X random samples + for (int i = 0; i < cnt; ) { + const int rnd = dist(gen); + if (used[rnd]) {continue;} // already used? try again! + used[rnd] = true; // mark as used + indices[i] = rnd; // add to the index + ++i; + } + + // the vector is setup correctly + isRandomized = true; + + } + + /** the iterator state */ + struct Iterator { + + /** the current position within "indicies" */ + int pos; + + /** the vector with the user-data to randomly iterate */ + const std::vector& vec; + + /** the vector containing the random indices */ + const std::vector& indices; + + /** ctor */ + Iterator(const int pos, const std::vector& vec, const std::vector& indices) : pos(pos), vec(vec), indices(indices) {;} + + /** end-of-iteration? */ + bool operator != (const Iterator& o) const {return pos != o.pos;} + + /** next value */ + Iterator& operator ++ () {++pos; return *this;} + + /** get the user-data */ + Element operator * () {return vec[indices[pos]];} + + }; + + //const Element& operator [] (const int idx) const {return vec[indices[idx]]; } + + /** number of available random entries */ + size_t size() const {return cnt;} + + + /** for-each access */ + Iterator begin() const { ensureRandomized(); return Iterator(0, vec, indices); } + + /** for-each access */ + Iterator end() const { ensureRandomized(); return Iterator(cnt, vec, indices); } + +private: + + /** ensure the coder called randomize() before using the iterator */ + void ensureRandomized() const { + Assert::isTrue(isRandomized, "call randomize() before using the iterator!"); + } + + }; + +} + +#endif // RANDOMITERATOR_H diff --git a/math/random/Uniform.h b/math/random/Uniform.h new file mode 100644 index 0000000..dadfe0e --- /dev/null +++ b/math/random/Uniform.h @@ -0,0 +1,43 @@ +#ifndef K_MATH_RANDOM_UNIFORM_H +#define K_MATH_RANDOM_UNIFORM_H + +#include "RandomGenerator.h" + +namespace Random { + + /** + * provides some common functions + * for handling normal-distributed random numbers + */ + template class Uniform { + + private: + + Random::RandomGenerator gen; + + /** depending on T, Dist is either a uniform_real or uniform_int distribution */ + typedef typename std::conditional< std::is_floating_point::value, std::uniform_real_distribution, std::uniform_int_distribution >::type Dist; + Dist dist; + + public: + + /** ctor */ + Uniform(const T min, const T max) : gen(RANDOM_SEED), dist(min, max) { + + } + + /** get a uniformaly distributed random number */ + T draw() { + return dist(gen); + } + + /** set the seed to use */ + void setSeed(const long seed) { + gen.seed(seed); + } + + }; + +} + +#endif // K_MATH_RANDOM_UNIFORM_H diff --git a/math/random/Unique.h b/math/random/Unique.h new file mode 100644 index 0000000..f7f829b --- /dev/null +++ b/math/random/Unique.h @@ -0,0 +1,26 @@ +#ifndef K_MATH_RND_UNIQUE_H +#define K_MATH_RND_UNIQUE_H + +namespace Random { + + /** + * provides some common functions + * for handling uniquely distributed random numbers + */ + class Unique { + + public: + + /** get uniquely distributed random number between min and max */ + static double getBetween(double min, double max) { + double rnd = (double) rand() / (double) RAND_MAX; + rnd *= (max-min); + rnd += min; + return rnd; + } + + }; + +} + +#endif // K_MATH_RND_UNIQUE_H diff --git a/misc/Binning.h b/misc/Binning.h new file mode 100644 index 0000000..7a65eca --- /dev/null +++ b/misc/Binning.h @@ -0,0 +1,114 @@ +#ifndef BINNING_H +#define BINNING_H + +#include +#include + +struct BinningRange { + float min; + float max; + BinningRange(float min, float max) : min(min), max(max) {;} + float getWidth() const {return max-min;} +}; + +template class Binning { + +private: + + /** size to use for each dimension's bin */ + std::vector binSizes; + + std::vector binRanges; + + std::unordered_set used; + +public: + + /** add a new dimension for binning with its corresponding size */ + void setBinSizes(const std::vector binSizes) { + this->binSizes = binSizes; + } + + /** manually set min/max range for each dimension */ + void setRanges(const std::vector ranges) { + this->binRanges = ranges; + } + + /** estimate each dimension's min/max from the given entry set */ + void setRanges(const std::vector& entries) { + + clearUsed(); + binRanges.clear(); + + // process each to-be-binned dimension + const int numDimensions = binSizes.size(); + for (int dim = 0; dim < numDimensions; ++dim) { + + // determin min and max value for the current dimension + BinningRange r(+1e30, -1e30); + for (const Binable& b : entries) { + const float val = b.getBinValue(dim); + if(val < r.min) {r.min = val;} + if(val > r.max) {r.max = val;} + } + + // remember + binRanges.push_back(r); + + } + + } + + /** remove all tracked usages */ + void clearUsed() { + used.clear(); + } + + /** does the given element relate to an used or unsed bin? */ + bool isFree(const Binable& b) const { + const uint64_t hash = getHash(b); + return used.find(hash) == used.end(); + } + + /** mark the bin the given element belongs to as used */ + void markUsed(const Binable& b) { + const uint64_t hash = getHash(b); + used.insert(hash); + } + +private: + + /** calculate unique-bin-hash for the given element */ + uint64_t getHash(const Binable& b) const { + + uint64_t hash = 0; + + const int numDimensions = binSizes.size(); + for (int dim = 0; dim < numDimensions; ++dim) { + + // get binable's value for the current dimension + const float val = b.getBinValue(dim); + + // snap value to bin-number + const int binNr = std::round((val-binRanges[dim].min) / binSizes[dim]); + + // maximum binNr + const int binNrMax = binRanges[dim].getWidth() / binSizes[dim]; + + // sanity check + //if (binNr < 0 || binNr > 255) {throw "bin index out of range!!";} + + // update hash + hash *= (binNrMax+1); + hash += binNr; + + } + + // done + return hash; + + } + +}; + +#endif // BINNING_H diff --git a/smc/Particle.h b/smc/Particle.h new file mode 100644 index 0000000..f9023d3 --- /dev/null +++ b/smc/Particle.h @@ -0,0 +1,54 @@ +/* + * Particle.h + * + * Created on: Sep 17, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLE_H_ +#define PARTICLE_H_ + +/** + * a particle consists of a (user-defined) state + * assigned with a weight (importance). + * + * depending on the particle filter's configuration, + * the (user-defined) state must provide several methods + * like: + * assigning values from another state + * multiplication + * etc.. + * + */ + +namespace SMC { + + template struct Particle { + + /** the particles state */ + State state; + + /** the (current) probability for this state */ + double weight; + + + /** empty ctor */ + Particle() : state(), weight(0) {;} + + /** ctor */ + Particle(const State& state, double weight) : state(state), weight(weight) {;} + + + /** assignment operator */ + Particle& operator = (const Particle& other) { + this->state = other.state; + this->weight = other.weight; + return *this; + } + + + }; + +} + +#endif /* PARTICLE_H_ */ diff --git a/smc/ParticleAssertions.h b/smc/ParticleAssertions.h new file mode 100644 index 0000000..5c65ae6 --- /dev/null +++ b/smc/ParticleAssertions.h @@ -0,0 +1,69 @@ +#ifndef PARTICLEASSERTIONS_H +#define PARTICLEASSERTIONS_H + +namespace SMC { + + /** check whether T provides a += operator */ + template + class HasOperatorPlusEq { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator+=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + /** check whether T provides a /= operator */ + template + class HasOperatorDivEq { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator/=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + + /** check whether T provides a * operator */ + template + class HasOperatorMul { + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator*) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + + /** check whether T provides an assignment operator */ + template + class HasOperatorAssign{ + + typedef char one; + typedef long two; + + template static one test( decltype(&C::operator=) ) ; + template static two test(...); + + public: + enum { value = sizeof(test(0)) == sizeof(one) }; + + }; + +} + +#endif // PARTICLEASSERTIONS_H diff --git a/smc/filtering/ParticleFilter.h b/smc/filtering/ParticleFilter.h new file mode 100644 index 0000000..48d3aa2 --- /dev/null +++ b/smc/filtering/ParticleFilter.h @@ -0,0 +1,256 @@ +/* + * ParticleFilter.h + * + * Created on: Sep 17, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLEFILTER_H_ +#define PARTICLEFILTER_H_ + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + + class ParticleFilter { + + private: + + /** all used particles */ + std::vector> particles; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the evaluation function to use */ + std::unique_ptr> evaluation; + + /** the initialization function to use */ + std::unique_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + + public: + + /** ctor */ + ParticleFilter(const uint32_t numParticles, std::unique_ptr> initializer) { + particles.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilter() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() { + return particles; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::unique_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::unique_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + double lastNEff = 9999999999999; + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // if the number of efficient particles is too low, perform resampling + if (lastNEff < particles.size() * nEffThresholdPercent) {resampler->resample(particles); } + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + lastNEff = normalize(); + + //std::cout << "normalized. n_eff is " << lastNEff << std::endl; + + // estimate the current state + const State est = estimation->estimate(particles); + + // done + return est; + + } + + /** perform only the transition step */ + void updateTransitionOnly(const Control* control) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + + // perform the transition step + transition->transition(particles, control); + + } + + /** perform only the evaluation step */ + State updateEvaluationOnly(const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the evaluation step and calculate the sum of all particle weights + evaluation->evaluation(particles, observation); + + // not needed anymore.. was to tricky to forget etc... + // sanity check + //Assert::isNotNaN(weightSum, "sum of all particle weights (returned from eval) is NAN!"); + //Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(); + + // estimate the current state + const State est = estimation->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** estimate the current state without update or transition just based on the current weights */ + State estimate() { + + // sanity checks (if enabled) + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + return estimation->estimate(particles); + + } + + private: + + /** normalize the weight of all particles to 1.0 and perform some sanity checks */ + double normalize() { + + // calculate sum(weights) + //double min1 = 9999999; + double weightSum = 0.0; + for (const auto& p : particles) { + weightSum += p.weight; + //if (p.weight < min1) {min1 = p.weight;} + } + + // sanity check. always! + if (weightSum != weightSum) { + throw Exception("sum of paticle-weights is NaN"); + } + if (weightSum == 0) { + throw Exception("sum of paticle-weights is 0.0"); + } + + // normalize and calculate N_eff + double sum2 = 0.0; + //double min2 = 9999999; + for (auto& p : particles) { + p.weight /= weightSum; + //if (p.weight < min2) {min2 = p.weight;} + sum2 += (p.weight * p.weight); + } + + // N_eff + return 1.0 / sum2; + + } + +// /** calculate the number of efficient particles (N_eff) */ +// double getNeff() const { +// double sum = 0.0; +// for (auto& p : particles) {sum += (p.weight * p.weight);} +// return 1.0 / sum; +// } + + }; + +} + +#endif /* PARTICLEFILTER_H_ */ diff --git a/smc/filtering/ParticleFilterEvaluation.h b/smc/filtering/ParticleFilterEvaluation.h new file mode 100644 index 0000000..7283767 --- /dev/null +++ b/smc/filtering/ParticleFilterEvaluation.h @@ -0,0 +1,29 @@ +#ifndef K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H +#define K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H + +#include + +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined particle-evaluation. + * the evaluation weighs the particle by comparing its state with the observation p(o_t | q_t) + */ + template + class ParticleFilterEvaluation { + + public: + + /** + * evaluate all particles (update p.weight) depending on their state and the current observation. + * this method MUST return the sum of all weights (used for normalization) + */ + virtual double evaluation(std::vector>& particles, const Observation& observation) = 0; + + }; + +} + +#endif // K_MATH_FILTERS_PARTICLE_PARTICLEFILTEREVALUATION_H diff --git a/smc/filtering/ParticleFilterHistory.h b/smc/filtering/ParticleFilterHistory.h new file mode 100644 index 0000000..15cacb2 --- /dev/null +++ b/smc/filtering/ParticleFilterHistory.h @@ -0,0 +1,283 @@ +/* + * ParticleFilterHistory.h + * + * Created on: Jul 13, 2015 + * Author: Toni Fetzer + */ + +#ifndef PARTICLEFILTERHISTORY_H_ +#define PARTICLEFILTERHISTORY_H_ + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + + class ParticleFilterHistory { + + private: + + /** all used particles */ + std::vector> particles; + + /** all non resampled particles */ + std::vector> particlesNoResampling; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the evaluation function to use */ + std::unique_ptr> evaluation; + + /** the initialization function to use */ + std::unique_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + + public: + + /** ctor */ + ParticleFilterHistory(const uint32_t numParticles, std::unique_ptr> initializer) { + particles.resize(numParticles); + particlesNoResampling.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilterHistory() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() { + return particles; + } + + /** access to all non resample particles */ + const std::vector>& getNonResamplingParticles() { + return particlesNoResampling; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + void setAngle(const double angle){ + for(SMC::Particle& p : particles){ + p.state.walkState.heading = angle; + } + } + + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::unique_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::unique_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + //save particels before resampling to save the weight at this timestep + particlesNoResampling = particles; + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** perform resampling -> transition -> evaluation -> estimation */ + State update(const Control* control, const Observation& observation, std::vector>& particlesWifi, + std::function>&, State, std::vector>&)> calcKLD, double& kld) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + //Assert::isNotNull(kld, "kld MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + //save particels before resampling to save the weight at this timestep + particlesNoResampling = particles; + + //calc the current divergence between wifi and particle propability + kld = calcKLD(particles, est, particlesWifi); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles, kld, particlesWifi); } + + // done + return est; + + } + + + /** perform only the transition step */ + void updateTransitionOnly(const Control* control) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + + // perform the transition step + transition->transition(particles, control); + + } + + /** perform only the evaluation step */ + State updateEvaluationOnly(const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the evaluation step and calculate the sum of all particle weights + const double weightSum = evaluation->evaluation(particles, observation); + + // sanity check + Assert::isNotNaN(weightSum, "sum of all particle weights (returned from eval) is NAN!"); + Assert::isNotNull(weightSum, "sum of all particle weights (returned from eval) is 0.0!"); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + const State est = estimation->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + return est; + + } + + /** estimate the current state without update or transition just based on the current weights */ + State estimate() { + + // sanity checks (if enabled) + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + return estimation->estimate(particles); + + } + + private: + + /** normalize the weight of all particles to one */ + double normalize(const double weightSum) { + double sum = 0.0; + for (auto& p : particles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + return 1.0 / sum; + } + + /** calculate the number of efficient particles (N_eff) */ + double getNeff() const { + double sum = 0.0; + for (auto& p : particles) {sum += (p.weight * p.weight);} + return 1.0 / sum; + } + }; + +} + +#endif /* PARTICLEFILTERHISTORY_H_ */ diff --git a/smc/filtering/ParticleFilterInitializer.h b/smc/filtering/ParticleFilterInitializer.h new file mode 100644 index 0000000..17e9087 --- /dev/null +++ b/smc/filtering/ParticleFilterInitializer.h @@ -0,0 +1,25 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for particle filter initializers. + * an initializer "configures" all particles before the filter starts: p(q_0) + */ + template + class ParticleFilterInitializer { + + public: + + /** the initializer must setup each particle within the given vector */ + virtual void initialize(std::vector>& particles) = 0; + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERINITIALIZER_H diff --git a/smc/filtering/ParticleFilterMixing.h b/smc/filtering/ParticleFilterMixing.h new file mode 100644 index 0000000..fa3085c --- /dev/null +++ b/smc/filtering/ParticleFilterMixing.h @@ -0,0 +1,241 @@ +#ifndef PARTICLEFILTERMIXING_H +#define PARTICLEFILTERMIXING_H + +#include +#include + +#include "../Particle.h" + +#include "resampling/ParticleFilterResampling.h" +#include "estimation/ParticleFilterEstimation.h" +#include "ParticleFilterTransition.h" +#include "ParticleFilterEvaluation.h" +#include "ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the particle filter + * @param State the (user-defined) state for each particle + * @param Observation the observation (sensor) data + */ + template + class ParticleFilterMixing { + + private: + + /** all used particles */ + std::vector> particles; + + /** the current calculated estimation */ + State estimation; + + /** the resampler to use */ + std::shared_ptr> resampler; + + /** the estimation function to use */ + std::shared_ptr> estimator; + + /** the transition function to use */ + std::shared_ptr> transition; + + /** the evaluation function to use */ + std::shared_ptr> evaluation; + + /** the initialization function to use */ + std::shared_ptr> initializer; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** the current sum of all weights NOT normalized*/ + double weightSum = 1.0; + + /** the predicted mode probability P(m_t|Z_t-1) */ + double predictedModeProbability = 1.0; + + /** the posterior probability of the mode p(m_t | Z_t)*/ + double modePosteriorProbability = 1.0; + + /** the transition mode probability P(m_t-1 | m_t, Z_t-1)*/ + double transitionModeProbability = 1.0; + + public: + + /** ctor + * NOTE: The modePosteriorProbability needs the be normalized depending on the number of filters within the IMMPF!! + */ + ParticleFilterMixing(const uint32_t numParticles, std::shared_ptr> initializer, double modePosteriorProbability) { + this->modePosteriorProbability = modePosteriorProbability; + + particles.resize(numParticles); + setInitializier(std::move(initializer)); + init(); + } + + /** dtor */ + ~ParticleFilterMixing() { + ; + } + + /** access to all particles */ + const std::vector>& getParticles() const { + return this->particles; + } + + void setParticles(const std::vector>& newParticles){ + this->particles = newParticles; + } + + /** get the current estimation */ + const State getEstimation() const { + return estimation; + } + + /** initialize/re-start the particle filter */ + void init() { + Assert::isNotNull(initializer, "initializer MUST not be null! call setInitializer() first!"); + initializer->initialize(particles); + } + + + /** set the resampling method to use */ + void setResampling(std::shared_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the estimation method to use */ + void setEstimator(std::shared_ptr> estimator) { + Assert::isNotNull(estimator, "setEstimation() MUST not be called with a nullptr!"); + this->estimator = std::move(estimator); + } + + /** set the transition method to use */ + void setTransition(std::shared_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** get the used transition method */ + ParticleFilterTransition* getTransition() { + return this->transition.get(); + } + + /** set the evaluation method to use */ + void setEvaluation(std::shared_ptr> evaluation) { + Assert::isNotNull(evaluation, "setEvaluation() MUST not be called with a nullptr!"); + this->evaluation = std::move(evaluation); + } + + /** set the initialization method to use */ + void setInitializier(std::shared_ptr> initializer) { + Assert::isNotNull(initializer, "setInitializer() MUST not be called with a nullptr!"); + this->initializer = std::move(initializer); + } + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the unormalized weight sum of all particles */ + double getWeightSum() const + { + return this->weightSum; + } + + /** get the predicted mode probability P(m_t|Z_t-1)*/ + double getPredictedModeProbability() const + { + return this->predictedModeProbability; + } + + /** set the predicted mode probability P(m_t|Z_t-1)*/ + void setPredictedModeProbability(const double val) { + this->predictedModeProbability = val; + } + + /** get the posterior mode probability P(m_t|Z_t)*/ + double getModePosteriorProbability() const + { + return this->modePosteriorProbability; + } + + /** set the posterior mode probability P(m_t|Z_t)*/ + void setModePosteriorProbability(const double likelihoodSum) { + + Assert::isNotNull(likelihoodSum, "likelihoodsum is zero.. thats not possible"); + Assert::isNotNull(this->weightSum, "weightSum is zero.. thats not possible"); + //Assert::isNotNull(this->predictedModeProbability, "predictedModeProbability is zero.. thats not possible"); + + this->modePosteriorProbability = (this->weightSum * this->predictedModeProbability) / likelihoodSum; + + //Assert::isNotNull(this->modePosteriorProbability, "modePosteriorProbability is zero.. thats not possible"); + } + + /** get the transition mode probability P(m_t|Z_t) + * NOTE: Dont use this value! It is only needed for more beatiful mixed sampling! + */ + double getTransitionModeProbability() const + { + return this->transitionModeProbability; + } + + /** set the transition mode probability P(m_t|Z_t)*/ + void setTransitionModeProbability(const double val) { + this->transitionModeProbability = val; + } + + /** perform resampling -> transition -> evaluation -> estimation */ + void update(const Control* control, const Observation& observation) { + + // sanity checks (if enabled) + Assert::isNotNull(resampler, "resampler MUST not be null! call setResampler() first!"); + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(evaluation, "evaluation MUST not be null! call setEvaluation() first!"); + Assert::isNotNull(estimator, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step + transition->transition(particles, control); + + // perform the evaluation step and calculate the sum of all particle weights + this->weightSum = evaluation->evaluation(particles, observation); + + // normalize the particle weights and thereby calculate N_eff + const double neff = normalize(weightSum); + + // estimate the current state + this->estimation = estimator->estimate(particles); + + // if the number of efficient particles is too low, perform resampling + if (neff < particles.size() * nEffThresholdPercent) { resampler->resample(particles); } + + // done + } + + private: + + /** normalize the weight of all particles to one */ + double normalize(const double weightSum) { + double sum = 0.0; + for (auto& p : particles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + return 1.0 / sum; + } + + /** calculate the number of efficient particles (N_eff) */ + double getNeff() const { + double sum = 0.0; + for (auto& p : particles) {sum += (p.weight * p.weight);} + return 1.0 / sum; + } + }; + +} + +#endif // PARTICLEFILTERMIXING_H diff --git a/smc/filtering/ParticleFilterTransition.h b/smc/filtering/ParticleFilterTransition.h new file mode 100644 index 0000000..6e0d220 --- /dev/null +++ b/smc/filtering/ParticleFilterTransition.h @@ -0,0 +1,26 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined particle transition. + * the transition describes the particles change during the transition phase p(q_t | q_t-1) + * depending on the control data (if any) + */ + template + class ParticleFilterTransition { + + public: + + /** perform the transition p(q_t | q_t-1) for all particles based on the given control data */ + virtual void transition(std::vector>& particles, const Control* control) = 0; + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERTRANSITION_H diff --git a/smc/filtering/estimation/ParticleFilterEstimation.h b/smc/filtering/estimation/ParticleFilterEstimation.h new file mode 100644 index 0000000..932bc0e --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimation.h @@ -0,0 +1,24 @@ +#ifndef PARTICLEFILTERESTIMATION_H +#define PARTICLEFILTERESTIMATION_H + +#include "../../Particle.h" +#include + +namespace SMC { + + template + class ParticleFilterEstimation { + + public: + + // dtor + virtual ~ParticleFilterEstimation() {;} + + // get the current state estimation for the given particle set + virtual State estimate(const std::vector>& particles) = 0; + + }; + +} + +#endif // PARTICLEFILTERESTIMATION_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h b/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h new file mode 100644 index 0000000..ba37b59 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationKernelDensity.h @@ -0,0 +1,160 @@ +#if FIXME + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H + + +#include "ParticleFilterEstimation.h" +#include +#include +#include "../../Particle.h" + +#include "../../../../misc/gnuplot/Gnuplot.h" +#include "../../../optimization/NumOptVector.h" +#include "../../../optimization/NumOptAlgoDownhillSimplex.h" + + + +namespace SMC { + + template + class ParticleFilterEstimationKernelDensity : public ParticleFilterEstimation { + + Gnuplot gp; + + K::NumOptAlgoDownhillSimplex simplex; + + private: + + class OptFunc { + + private: + + /** the particles to work on */ + const std::vector>& particles; + + public: + + /** ctor */ + OptFunc(const std::vector>& particles) : particles(particles) {;} + + float operator () (const float* params) const { + + double prob = 0; + const int size = particles.size(); + + //#pragma omp parallel for + for (int i = 0; i < size; i+=10) { + const Particle& p = particles[i]; + prob += p.state.getKernelDensityProbability(params) * p.weight; + } + + // convert probability to "error" + return -prob; + + } + + }; + + + public: + + State estimate(std::vector>& particles) override { + + + // comparator + auto comp = [] (const Particle& p1, const Particle& p2) { return p1.weight < p2.weight; }; + +// // find max state +// auto el = std::max_element(particles.begin(), particles.end(), comp); +// State max = el->state; + + +// // region to check +// BBox2 bbox; +// bbox.add(Point2f(-50,-50)); +// bbox.add(Point2f(100,100)); + +// const float stepSize = 1.0f; + +// const int pxX = (bbox.getMax().x - bbox.getMin().x) / stepSize; +// const int pxY = (bbox.getMax().y - bbox.getMin().y) / stepSize; + +// // optimize using simplex + OptFunc func(particles); + +// // calculate the optimum +// simplex.calculateOptimum(func, params); +// std::cout << params << std::endl; + + + float params[numParams]; + getGlobalMax(particles, params); + + // create output state from optimized params + State res(params); + + + gp << "splot '-' with lines\n"; + + int x1 = 0;//params[0]-2500; + int x2 = 100*100;//params[0]+2500; + int y1 = 0;//params[1]-2500; + int y2 = 60*100;//params[1]+2500; + + for (int x = x1; x < x2; x += 400) { + for (int y = y1; y < y2; y += 400) { + params[0] = x; + params[1] = y; + params[2] = 0; + gp << x << " " << y << " " << -func(params) << "\n"; + } + gp << "\n"; + } + gp << "e\n"; + gp.flush(); + + return res; + + } + + void getGlobalMax(const std::vector>& particles, float* startParams) { + + OptFunc func(particles); + double bestP = 0; + float bestParams[numParams]; + + simplex.setMaxIterations(10); + simplex.setNumRestarts(0); + + + for (int i = 0; i < 15; ++i) { + + // start at a random particle + const int idx = rand() % particles.size(); + + // start optimization at this particle's paramters + particles[idx].state.fillKernelDenstityParameters(startParams); + + simplex.calculateOptimum(func, startParams); + + const float prob = -func(startParams); + if (prob > bestP) { + bestP = prob; + memcpy(bestParams, startParams, numParams*sizeof(float)); + //std::cout << bestParams << std::endl; + } + + } + + memcpy(startParams, bestParams, numParams*sizeof(float)); + + } + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERESTIMATIONKERNELDENSITY_H + +#endif diff --git a/smc/filtering/estimation/ParticleFilterEstimationMax.h b/smc/filtering/estimation/ParticleFilterEstimationMax.h new file mode 100644 index 0000000..b07718e --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationMax.h @@ -0,0 +1,37 @@ +#ifndef PARTICLEFILTERESTIMATIONMAX_H +#define PARTICLEFILTERESTIMATIONMAX_H + +#include "ParticleFilterEstimation.h" +#include +#include +#include "../../Particle.h" + +namespace SMC { + + /** + * just use the particle with the maximum weight + * as the currently sestimated state + */ + template + class ParticleFilterEstimationMax : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // comparator + auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight < p2.weight; + }; + + // find max element + auto el = std::max_element(particles.begin(), particles.end(), comp); + return el->state; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONMAX_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h new file mode 100644 index 0000000..61102aa --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h @@ -0,0 +1,78 @@ +#ifndef PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H + + +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * the X% best weighted particles + */ + template + class ParticleFilterEstimationOrderedWeightedAverage : public ParticleFilterEstimation { + + private: + + const float percent; + + public: + + /** ctor */ + ParticleFilterEstimationOrderedWeightedAverage(const float percent) : percent(percent) {;} + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + // comparator (highest first) + auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + // create a copy + std::vector> copy; + copy.insert(copy.begin(), particles.begin(), particles.end()); + + // sort the copy (highest weight first) + std::sort (copy.begin(), copy.end(), comp); + + State tmp; + + // calculate weighted average + const int numBest = copy.size() * percent; + double weightSum = 0; + for (int i = 0; i < numBest; ++i) { + const Particle& p = copy[i]; + tmp += p.state * p.weight; + weightSum += p.weight; + } + + if (weightSum != weightSum) { + int i = 0; (void) i; + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONORDEREDWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h new file mode 100644 index 0000000..20c3bc2 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h @@ -0,0 +1,60 @@ +#ifndef PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H + +#include "ParticleFilterEstimation.h" +#include "../../Particle.h" +#include "../../ParticleAssertions.h" + +#include + +namespace SMC { + + /** + * this implementation estimates the current state + * by using the most probable particle and some near particles + * combining them by their weight (weighted average) + * + * the checker, whether a particle is near or not, uses a special, + * user-defined metric "belongsToRegion()". This user-method must + * return a boolean, whether to include the provided particle + * within the region around the maximum particle, or not. + */ + template + class ParticleFilterEstimationRegionalWeightedAverage : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + //1) get the most probable particle + const auto comp = [] (const Particle& p1, const Particle& p2) {return p1.weight < p2.weight;}; + const Particle& max = *std::max_element(particles.begin(), particles.end(), comp); + + //2) calculate the regional weighted average + double cumWeight = 0; + State res; + for (const Particle& p : particles) { + if (!p.state.belongsToRegion(max.state)) {continue;} + cumWeight += p.weight; + res += p.state * p.weight; + } + + // 3) normalize + res /= cumWeight; + + // done + return res; + + } + + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONREGIONALWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h new file mode 100644 index 0000000..f533161 --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h @@ -0,0 +1,52 @@ +#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H +#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H + +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * all particles and their weight + */ + template + class ParticleFilterEstimationWeightedAverage : public ParticleFilterEstimation { + + public: + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + State tmp; + + // calculate weighted average + double weightSum = 0; + for (const Particle& p : particles) { + tmp += p.state * p.weight; + weightSum += p.weight; + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H diff --git a/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h new file mode 100644 index 0000000..37e8edc --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h @@ -0,0 +1,66 @@ +#ifndef PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H +#define PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGEWITHANGLE_H + +#include +#include +#include +#include "../../Particle.h" +#include "../../ParticleAssertions.h" +#include "ParticleFilterEstimation.h" + +#include "../../../Assertions.h" + +namespace SMC { + + /** + * calculate the (weighted) average state using + * all particles and their weight + */ + template + class ParticleFilterEstimationWeightedAverageWithAngle : public ParticleFilterEstimation { + + public: + + State estimate(std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + + State tmp; + + // calculate weighted average + double weightSum = 0; + + double xAngle = 0; + double yAngle = 0; + for (const Particle& p : particles) { + tmp += p.state * p.weight; + weightSum += p.weight; + + xAngle += std::cos(p.state.walkState.heading.getRAD()); + yAngle += std::sin(p.state.walkState.heading.getRAD()); + } + + Assert::isTrue( (weightSum == weightSum), "the sum of particle weights is NaN!"); + Assert::isTrue( (weightSum != 0), "the sum of particle weights is null!"); + + // normalize + tmp /= weightSum; + + //calculated avg angle + tmp.avgAngle = std::fmod((std::atan2(yAngle / particles.size(), xAngle / particles.size()) * 180.0/PI) + 720, 360.0); + + if(tmp.avgAngle > 360.0) + std::cout << "fuck" << std::endl; + + return tmp; + + } + + }; + +} + +#endif // PARTICLEFILTERESTIMATIONWEIGHTEDAVERAGE_H diff --git a/smc/filtering/resampling/ParticleFilterResampling.h b/smc/filtering/resampling/ParticleFilterResampling.h new file mode 100644 index 0000000..906e91f --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResampling.h @@ -0,0 +1,43 @@ +/* + * ParticleResampling.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef PARTICLEFILTERRESAMPLING_H_ +#define PARTICLEFILTERRESAMPLING_H_ + +#include "../../Particle.h" +#include +#include + +/** + * interface for all available resampling methods + * within the particle filter + * @param State the (user-defined) state + */ + +namespace SMC { + + template class ParticleFilterResampling { + + public: + + /** + * perform resampling on the given particle-vector + * @param particles the vector of all particles to resample + */ + virtual void resample(std::vector>& particles) = 0; + + /** + * perform resampling on the given particle-vector + * @param particles the vector of all particles to resample + */ + virtual void resample(std::vector>& particles, double kld, std::vector>& particlesWifi) {} + + }; + +} + +#endif /* PARTICLEFILTERRESAMPLING_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingDivergence.h b/smc/filtering/resampling/ParticleFilterResamplingDivergence.h new file mode 100644 index 0000000..b8b2335 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingDivergence.h @@ -0,0 +1,157 @@ +#ifndef PARTICLEFILTERRESAMPLINGDIVERGENCE_H +#define PARTICLEFILTERRESAMPLINGDIVERGENCE_H + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +int fuck_off = 0; + + +namespace SMC { + + /** + * draws new particle depending on the current divergence between two + * probability distributations based on Jensen–Shannon divergence + */ + template + class ParticleFilterResamplingDivergence : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** this is a copy of the wifi particle-set */ + std::vector> particlesWifiCopy; + + /** random number generator */ + std::minstd_rand gen; + + public: + + /** ctor */ + ParticleFilterResamplingDivergence() { + gen.seed(1234); + } + + + void resample(std::vector>& particles) override{ + + } + + void resample(std::vector>& particles, double kld, std::vector>& particlesWifi) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + //static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + const uint32_t cntWifi = (uint32_t) particlesWifi.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + particlesWifiCopy.resize(cntWifi); + + // swap both vectors + particlesCopy.swap(particles); + particlesWifiCopy = particlesWifi; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + double cumWeightWifi = 0; + for (uint32_t i = 0; i < cntWifi; ++i) { + cumWeightWifi += particlesWifiCopy[i].weight; + particlesWifiCopy[i].weight = cumWeightWifi; + } + + double maxKld = 250; + double minKld = 20; + + double diffKld = kld - minKld; + if(diffKld < 0){ + diffKld = 0; + } + + int numWifiParticel = (diffKld / maxKld) * cnt; + + //skip the first observations + static const int skipStart = 5; + if(fuck_off++ < skipStart){ + numWifiParticel = 0; + } + + + // now draw from the copy vector and fill the original one + // with the resampled particle-set keep the number of particles. + for (uint32_t i = 0; i < cnt; ++i) { + if(i < numWifiParticel){ + + //we draw a particle from posterior and change to position based on the wifi distribution + Particle posteriorParticle = draw(cumWeight); + Particle wifiParticle = drawWifi(cumWeightWifi); + + posteriorParticle.state.position = wifiParticle.state.position; + particles[i] = posteriorParticle; + } else { + particles[i] = draw(cumWeight); + } + particles[i].weight = equalWeight; + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + /** draw one particle according to its weight from the copy vector */ + const Particle& drawWifi(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesWifiCopy.begin(), particlesWifiCopy.end(), rand, comp); + return *it; + + } + + + }; + + +} + + + +#endif // PARTICLEFILTERRESAMPLINGDIVERGENCE_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingKLD.h b/smc/filtering/resampling/ParticleFilterResamplingKLD.h new file mode 100644 index 0000000..5f7e58b --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingKLD.h @@ -0,0 +1,130 @@ +#ifndef PARTICLEFILTERRESAMPLINGKLD_H +#define PARTICLEFILTERRESAMPLINGKLD_H + +#include +#include + +#include +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +#include +#include + +namespace SMC { + + /** + * "Adapting sample size in particle filters through KLD-resampling" T.Li et al. + * Resample a dynamic size of new particles. for resampling we use the simple (multinomial version) + * We cann guarente with the probability 1 - delt, the KLD between posterior and the true posterior is + * less then epsilon. + * + * + * Note: the State template parameter needs to implement a float getBinValue(const int dim) const {..} function + */ + template + class ParticleFilterResamplingKLD : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + /** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/ + double epsilon; + + /** the upper 1 - delta quantil of the normal distribution. something like 0.01 */ + double delta; + + /** the bins */ + Binning bins; + + /** max particle size */ + uint32_t N_max; + + public: + + /** ctor */ + ParticleFilterResamplingKLD(double delta = 0.01, double epsilon = 0.13, uint32_t N_max = 25000) : delta(delta), epsilon(epsilon), N_max(N_max) { + gen.seed(RANDOM_SEED); + + bins.setBinSizes({0.01, 0.01, 0.2, 0.3}); + bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)}); + } + + void resample(std::vector>& particles) override { + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + //clean the bins and particles + bins.clearUsed(); + particles.clear(); + + // draw a new particle and check if it is within a bin or not + uint32_t k = 1; + double N = 0; + int i = 0; + while(i <= N && i <= N_max){ + particles.push_back(draw(cumWeight)); + particles.back().weight = equalWeight; + + //is bin free? + if(bins.isFree(particles[i].state)){ + k++; + bins.markUsed(particles[i].state); + + //calculate the new N + double z_delta = Distribution::NormalCDF::getProbit(1 - delta); + double front = (k - 1) / (2 * epsilon); + double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta ); + + N = front * std::pow(back, 3.0); + } + ++i; + + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + }; + + +} + +#endif // PARTICLEFILTERRESAMPLINGKLD_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingLog.h b/smc/filtering/resampling/ParticleFilterResamplingLog.h new file mode 100644 index 0000000..500391d --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingLog.h @@ -0,0 +1,31 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H + +#include "ParticleFilterResamplingSimple.h" + +namespace K { + + /** + * draw particles using their log-weight + */ + template + class ParticleFilterResamplingLog : public ParticleFilterResamplingSimple { + + public: + + void resample(std::vector>& particles) override { + + for (Particle& p : particles) { + //p.weight = - 1.0 / std::log(p.weight); + p.weight = std::pow(p.weight, 0.5); + } + + ParticleFilterResamplingSimple::resample(particles); + + } + + }; + +} + +#endif // K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGLOG_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingNEff.h b/smc/filtering/resampling/ParticleFilterResamplingNEff.h new file mode 100644 index 0000000..aed655b --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingNEff.h @@ -0,0 +1,163 @@ +#ifndef PARTICLEFILTERRESAMPLINGNEFF_H +#define PARTICLEFILTERRESAMPLINGNEFF_H + + + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../ParticleAssertions.h" + +namespace K { + + /** + * TODO + */ + template class ParticleFilterResamplingNEff : public ParticleFilterResampling { + + public: + + using DrawCallback = std::function& p)>; + + private: + + /** random number generator */ + std::minstd_rand gen; + + const float desiredNEff; + + const float maxAdapt = 0.05; + + DrawCallback drawCallback; + + public: + + /** ctor */ + ParticleFilterResamplingNEff(const float desiredNEff = 0.75, const float maxAdapt = 0.05) : desiredNEff(desiredNEff), maxAdapt(maxAdapt) { + gen.seed(1234); + } + + /** callback to inform about redrawn particles */ + void setDrawCallback(const DrawCallback& callback) {this->drawCallback = callback;} + + void resample(std::vector>& particles) override { + + // comparator (highest first) + static auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + // ensure all particle-weights sum up to 1.0 + normalize(particles); + + // calculate current N_eff + const double curNEff = getNEff(particles); + + // cur N_eff is > desired N_eff -> nothing to do + if (curNEff > desiredNEff) {return;} + + const size_t cnt = particles.size(); + + // how many particles to discard and resample based on N_eff + // example: N_eff = 75% -> 25% to discard/resample + // cur N_eff < desired N_eff -> calculate adaption + float adapt = desiredNEff - curNEff; + if (adapt > maxAdapt) {adapt = maxAdapt;} + const size_t toResample = cnt * adapt; + + + + // sort orig particles by weight (highest first) + std::sort(particles.begin(), particles.end(), comp); + + // to-be-removed region [at the end of the vector] + const size_t start = particles.size() - toResample; + + // zero the weight of the to-be-removed region + for (size_t i = start; i < cnt; ++i) { + particles[i].weight = 0; + } + + // renormalize the remaining set + normalize(particles); + + // replace the to-be-removed region [start:cnt] + for (size_t i = start; i < cnt; ++i) { + + // track cumulative weight for each particle-index + std::vector cumWeights; + + // calculate cumulative weight for the copy [0:start] = the to-be-kept particles + double cumWeight = 0; + for (size_t i = 0; i < start; ++i) { + cumWeight += particles[i].weight; + cumWeights.push_back(cumWeight); + } + + // get a random particle between [0:start] + const size_t idx = draw(cumWeights); + + // split + particles[i] = particles[idx]; + particles[i].weight /= 2; + particles[idx].weight /= 2; + + // callback? + if (drawCallback) {drawCallback(particles[i]);} + + } + + // sanity check. sum of weights must now still be 1.0! + double weightSum = 0; + for (const auto& p : particles) {weightSum += p.weight;} + + if (std::abs(weightSum - 1.0) > 0.01) { + throw Exception("particle weight does not sum up to 1.0"); + } + + } + + private: + + void normalize(std::vector>& particles) const { + double sum = 0.0; + for (const auto& p : particles) {sum += p.weight;} // calculate sum + for (auto& p : particles) {p.weight /= sum;} // normalize + } + + double getNEff(const std::vector>& particles) const { + double sum = 0.0; + for (auto& p : particles) { + sum += (p.weight * p.weight); + } + return 1.0 / sum / particles.size(); + } + + size_t draw(const std::vector& cumWeights) { + + // get the cumulative weight [= last entry] + const double cumWeight = cumWeights.back(); + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto it = std::lower_bound(cumWeights.begin(), cumWeights.end(), rand); + + // convert iterator to index + const int idx = it - cumWeights.begin(); + + return idx; + + } + + }; + +} + + +#endif // PARTICLEFILTERRESAMPLINGNEFF_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingNone.h b/smc/filtering/resampling/ParticleFilterResamplingNone.h new file mode 100644 index 0000000..1c1bb02 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingNone.h @@ -0,0 +1,34 @@ +/* + * ParticleResamplingNone.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ + +#include "ParticleFilterResampling.h" + +namespace K { + + /** + * dummy resampler simply doing nothing + */ + template + class ParticleFilterResamplingNone : public ParticleFilterResampling { + + public: + +// /** dtor */ +// ~ParticleFilterResamplingNone() {;} + + void resample(std::vector>& particles) override { + (void) particles; + } + + }; + +} + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGNONE_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingPercent.h b/smc/filtering/resampling/ParticleFilterResamplingPercent.h new file mode 100644 index 0000000..42ec764 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingPercent.h @@ -0,0 +1,94 @@ +#ifndef PARTICLEFILTERRESAMPLINGPERCENT_H +#define PARTICLEFILTERRESAMPLINGPERCENT_H + + +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +namespace SMC { + + /** + * TODO + */ + template class ParticleFilterResamplingPercent : public ParticleFilterResampling { + + private: + + /** random number generator */ + std::minstd_rand gen; + + float percent; + + public: + + /** ctor */ + ParticleFilterResamplingPercent(const float percent) : percent(percent) { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // comparator (highest first) + static auto comp = [] (const Particle& p1, const Particle& p2) { + return p1.weight > p2.weight; + }; + + const uint32_t cnt = (uint32_t) particles.size(); + + // sort particles by weight (highest first) + std::sort(particles.begin(), particles.end(), comp); + + + // to-be-removed region + const int start = particles.size() * (1-percent); + const int end = particles.size(); + std::uniform_int_distribution dist(0, start-1); + + // remove by re-drawing + for (uint32_t i = start; i < end; ++i) { + const int rnd = dist(gen); + particles[i] = particles[rnd]; + particles[i].weight /= 2; + particles[rnd].weight /= 2; + } + + // calculate weight-sum + double weightSum = 0; + for (const auto& p : particles) { + weightSum += p.weight; + } + + + int i = 0; + + + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(std::vector>& copy, const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp); + return *it; + + } + + + + }; + +} + +#endif // PARTICLEFILTERRESAMPLINGPERCENT_H diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimple.h b/smc/filtering/resampling/ParticleFilterResamplingSimple.h new file mode 100644 index 0000000..4a2a9bf --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingSimple.h @@ -0,0 +1,102 @@ +/* + * ParticleResamplingSimple.h + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../../ParticleAssertions.h" + +namespace SMC { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight. + * O(log(n)) per particle + */ + template + class ParticleFilterResamplingSimple : public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + /** random number generator */ + std::minstd_rand gen; + + public: + + /** ctor */ + ParticleFilterResamplingSimple() { + gen.seed(1234); + } + + void resample(std::vector>& particles) override { + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + // static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // swap both vectors + particlesCopy.swap(particles); + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < cnt; ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // now draw from the copy vector and fill the original one + // with the resampled particle-set + for (uint32_t i = 0; i < cnt; ++i) { + particles[i] = draw(cumWeight); + particles[i].weight = equalWeight; + } + + } + + private: + + /** draw one particle according to its weight from the copy vector */ + const Particle& draw(const double cumWeight) { + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumWeight); + + // draw a random value between [0:cumWeight] + const float rand = dist(gen); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp); + return *it; + + } + + + + }; + + +} + + + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGSIMPLE_H_ */ diff --git a/smc/filtering/resampling/ParticleFilterResamplingWheel.h b/smc/filtering/resampling/ParticleFilterResamplingWheel.h new file mode 100644 index 0000000..9c1c4d3 --- /dev/null +++ b/smc/filtering/resampling/ParticleFilterResamplingWheel.h @@ -0,0 +1,95 @@ +#ifndef K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ +#define K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ + +#include +#include + +#include "ParticleFilterResampling.h" +#include "../ParticleAssertions.h" +#include "../../../distribution/Uniform.h" +#include "../../../../os/Time.h" + +/** + * sources: + * https://www.udacity.com/course/viewer#!/c-cs373/l-48704330/e-48748082/m-48740082 + * https://www.youtube.com/watch?list=PLpUPoM7Rgzi_7YWn14Va2FODh7LzADBSm&feature=player_detailpage&v=kZhOJgooMxI#t=567 + */ + +namespace K { + + /** + * uses simple probability resampling by drawing particles according + * to their current weight using a "wheel" + * -> O(n) for all particles + */ + template + class ParticleFilterResamplingWheel: public ParticleFilterResampling { + + private: + + /** this is a copy of the particle-set to draw from it */ + std::vector> particlesCopy; + + public: + + void resample(std::vector>& particles) override { + + uint64_t start = K::Time::getTimeMS(); + + // compile-time sanity checks + // TODO: this solution requires EXPLICIT overloading which is bad... + //static_assert( HasOperatorAssign::value, "your state needs an assignment operator!" ); + + const uint32_t cnt = (uint32_t) particles.size(); + + // equal weight for all particles. sums up to 1.0 + const double equalWeight = 1.0 / (double) cnt; + + + + // get the weight of the "heaviest" particle + const auto lambda = [] (const Particle& p1, const Particle& p2) {return p1.weight < p2.weight;}; + const Particle max = *std::max_element(particles.begin(), particles.end(), lambda); + + // draw form the input particle set by treating it as a wheel + K::UniformDistribution dist(0, 2 * max.weight); + int curIdx = K::UniformDistribution::draw( (int)0, (int)particles.size() - 1); + double curOffset = dist.draw(); + + // ensure the copy vector has the same size as the real particle vector + particlesCopy.resize(cnt); + + // draw the new set of particles + for (uint32_t i = 0; i < cnt; ) { + + // found a suitable particle, use it and draw the next random number + if (particles[curIdx].weight >= curOffset) { + particlesCopy[i] = particles[curIdx]; + particlesCopy[i].weight = equalWeight; + curOffset += dist.draw(); + ++i; + + // weight to small, subtract the particles weight and move on to the next particle + } else { + curOffset -= particles[curIdx].weight; + curIdx = (curIdx + 1) % (particles.size()); + } + + } + + // swap both vectors + particlesCopy.swap(particles); + + uint64_t end = K::Time::getTimeMS(); + std::cout << (end-start) << std::endl; + + } + + }; + + +} + + + +#endif /* K_MATH_FILTER_PARTICLES_PARTICLEFILTERRESAMPLINGWHEEL_H_ */ diff --git a/smc/merging/InteractingMultipleModelParticleFilter.h b/smc/merging/InteractingMultipleModelParticleFilter.h new file mode 100644 index 0000000..fb96967 --- /dev/null +++ b/smc/merging/InteractingMultipleModelParticleFilter.h @@ -0,0 +1,135 @@ +#ifndef INTERACTINGMULTIPLEMODELPARTICLEFILTER_H +#define INTERACTINGMULTIPLEMODELPARTICLEFILTER_H + +#include + +#include "mixing/MixingSampler.h" +#include "MarkovTransitionProbability.h" +#include "estimation/JointEstimation.h" + +#include "../filtering/ParticleFilterMixing.h" +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for IMMPF based on Driessen and Boers + * @param vector of particle filters as modes + */ + template + class InteractingMultipleModelParticleFilter { + + private: + + /** the used particle filters */ + std::vector> modes; + + /** the mixing function to use */ + std::unique_ptr> mixing; + + /** the function for calculating markov chain transition*/ + std::unique_ptr> transition; + + /** the function for calculating a joint estimation */ + std::unique_ptr> estimation; + + /** the transition probability matrix */ + Eigen::MatrixXd transitionProbabilityMatrix; + + public: + + /** ctor + * NOTE: The single rows of the transition matrix need to sum to 1!!! + */ + InteractingMultipleModelParticleFilter(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) { + + // TODO: this is a deep copy... we could or should change that later.. since slooooooooow =) + this->modes = modes; + + //init transmatrix + this->transitionProbabilityMatrix = transitionProbabilityMatrix; + } + + /** dtor */ + ~InteractingMultipleModelParticleFilter() { + ; + } + + /** set mixing function */ + void setMixingSampler(std::unique_ptr> mixing) { + Assert::isNotNull(mixing, "setMixingSampler() MUST not be called with a nullptr!"); + this->mixing = std::move(mixing); + } + + /** set mode transition function */ + void setMarkovTransitionProbability(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setMarkovTransitionProbability() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set joint estimation function */ + void setJointEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setJointEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + const std::vector>& getModes() const{ + return modes; + } + + /** perform the mixed update -> update particle filters -> estimation -> mixing*/ + State update(const Control* control, const Observation& observation){ + + // sanity checks (if enabled) + Assert::isNotNull(mixing, "mixingsampler MUST not be null! call setResampler() first!"); + //Assert::isNotNull(transition, "transition MUST not be null! call setResampler() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setResampler() first!"); + + // mix both modes depending on the kld divergency and their calculated + // impact (mode posterior prob) to draw new particles + mixing->mixAndSample(modes, this->transitionProbabilityMatrix); + + for(ParticleFilterMixing& filter : modes){ + + //update the particle filter and save estimation + filter.update(control, observation); + } + + // calculate the transition probability matrix for the markov chain based on + // kld divergency. + this->transitionProbabilityMatrix = transition->update(modes, observation); + + // calc posterior probability + this->calcPosteriorProbability(); + + // calculate current estimation + const State jointEst = estimation->estimate(modes); + + //done + return jointEst; + } + + + private: + + void calcPosteriorProbability(){ + + // calculate the likelihood Sum lambda for all modes + // the Posterior is init with P(m_0 | Z_0) + double likelihoodSum; + for(ParticleFilterMixing& filter : modes){ + + likelihoodSum += filter.getWeightSum() * filter.getPredictedModeProbability(); + } + + // set the last posterior probability p(m_t-1 | Z_t-1) for all modes + for(ParticleFilterMixing& filter : modes){ + + filter.setModePosteriorProbability(likelihoodSum); + } + } + }; + +} + +#endif // INTERACTINGMULTIPLEMODELPARTICLEFILTER_H diff --git a/smc/merging/MarkovTransitionProbability.h b/smc/merging/MarkovTransitionProbability.h new file mode 100644 index 0000000..0a43a7e --- /dev/null +++ b/smc/merging/MarkovTransitionProbability.h @@ -0,0 +1,30 @@ +#ifndef MARKOVTRANSITIONPROBABILITY_H +#define MARKOVTRANSITIONPROBABILITY_H + +#include "../filtering/ParticleFilterMixing.h" +#include + +namespace SMC { + + + /** + * interface for all available transition probability calculations + * within the IMMPF + */ + template + class MarkovTransitionProbability { + + public: + + /** + * perform the calculation of the transition matrix + * @param vector of modes / particle filters + */ + virtual Eigen::MatrixXd update(std::vector>& modes, const Observation& obs) = 0; + + }; + +} + + +#endif // MARKOVTRANSITIONPROBABILITY_H diff --git a/smc/merging/estimation/JointEstimation.h b/smc/merging/estimation/JointEstimation.h new file mode 100644 index 0000000..b5c0a6f --- /dev/null +++ b/smc/merging/estimation/JointEstimation.h @@ -0,0 +1,25 @@ +#ifndef JOINTESTIMATION_H +#define JOINTESTIMATION_H + +#include "../../filtering/ParticleFilterMixing.h" + +namespace SMC { + + /** + * interface for all available joint estimations + * within the IMMPF we have multiple particle filters + * the "true" estimation is a joint state of all + */ + template + class JointEstimation { + + public: + + // get the current state estimation for the given particle set + virtual const State estimate(std::vector>& modes) = 0; + + }; + +} + +#endif // JOINTESTIMATION_H diff --git a/smc/merging/estimation/JointEstimationPosteriorOnly.h b/smc/merging/estimation/JointEstimationPosteriorOnly.h new file mode 100644 index 0000000..77271f4 --- /dev/null +++ b/smc/merging/estimation/JointEstimationPosteriorOnly.h @@ -0,0 +1,28 @@ +#ifndef JOINTESTIMATIONPOSTERIORONLY_H +#define JOINTESTIMATIONPOSTERIORONLY_H + +#include "JointEstimation.h" + +#include "../../filtering/ParticleFilterMixing.h" + +namespace SMC { + + /** + * Use only the posterior distribution (first mode entry) for the joint estimaton + */ + template + class JointEstimationPosteriorOnly + : public JointEstimation { + + public: + + // get the current state estimation for the given particle set + const State estimate(std::vector>& modes) override { + return modes[0].getEstimation(); + } + + }; + +} + +#endif // JOINTESTIMATIONPOSTERIORONLY_H diff --git a/smc/merging/mixing/MixingSampler.h b/smc/merging/mixing/MixingSampler.h new file mode 100644 index 0000000..3a8d712 --- /dev/null +++ b/smc/merging/mixing/MixingSampler.h @@ -0,0 +1,29 @@ +#ifndef MIXINGSAMPLER_H +#define MIXINGSAMPLER_H + +#include "../../filtering/ParticleFilterMixing.h" +#include + +namespace SMC { + + + /** + * interface for all available resampling methods + * within the particle filter + */ + template + class MixingSampler { + + public: + + /** + * perform mixing of modes and sample according to the modes probability + * @param particles the vector of all particles to resample + */ + virtual void mixAndSample(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) = 0; + + }; + +} + +#endif // MIXINGSAMPLER_H diff --git a/smc/merging/mixing/MixingSamplerDivergency.h b/smc/merging/mixing/MixingSamplerDivergency.h new file mode 100644 index 0000000..88e4663 --- /dev/null +++ b/smc/merging/mixing/MixingSamplerDivergency.h @@ -0,0 +1,157 @@ +#ifndef MIXINGSAMPLERDIVERGENCY_H +#define MIXINGSAMPLERDIVERGENCY_H + +#include "MixingSampler.h" + +#include +#include +#include + +namespace SMC { + + /** + * Using the direct sampling approach of Driessen and Boers in + * "Efficient particle filter for jump Markov nonlinear systems". + * as transition probability matrix for the markov chain we use + * a divergence based on Jensen–Shannon divergence + */ + template class MixingSamplerDivergency + : public MixingSampler { + + /** random number generator */ + std::minstd_rand genNorm; + std::minstd_rand genPart; + + /** copy of the modes with cumulative probabilities for easy drawing*/ + std::vector> copyModes; + + public: + + void mixAndSample(std::vector>& modes, Eigen::MatrixXd transitionProbabilityMatrix) override{ + + genNorm.seed(std::chrono::system_clock::now().time_since_epoch().count()); + genPart.seed(std::chrono::system_clock::now().time_since_epoch().count() + 233); + + // set copyModes for later drawing + copyModes = modes; + + // create cumulative particlesets for the copy + // Note: in most cases, the particles are already resampled within the filtering stage + // but in some cases they are not and therefore we need to draw cumulatively and not equally + for(ParticleFilterMixing& copyFilter : copyModes){ + double cumWeight = 0; + std::vector> copyParticles; + copyParticles = copyFilter.getParticles(); + + for(int i = 0; i < copyParticles.size(); ++i){ + cumWeight += copyParticles[i].weight; + copyParticles[i].weight = cumWeight; + } + + copyFilter.setParticles(copyParticles); + } + + // calculate the new predicted mode prob P(m_t|Z_t-1) and P(m_t-1 | m_t, Z_t-1) + int m = 0; //this is m_t + for(ParticleFilterMixing& focusedFilter : modes){ + + // P(m_t|Z_t-1) = sum(P(m_t | m_t-1) P(m_t-1 | Z_t-1)) + int i = 0; //this are all possible m_t-1 + double predictedModeProbability = 0; + for(ParticleFilterMixing& filter : modes){ + predictedModeProbability += transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability(); + ++i; + } + + //Assert::isNotNull(predictedModeProbability, "predictedModeProbability is zero.. thats not possible!"); + focusedFilter.setPredictedModeProbability(predictedModeProbability); + + // cumulative sum of TransitionModeProbabilities for drawing modes from the perspective of ONE filter! + // this means, the transition mode probabilities are calculated for each filter NEW! + // calculate P(m_t-1 | m_t, Z_t-1) = P(m_t | m_t-1) * p(m_t-1 | Z_t-1) / P(m_t|Z_t-1) + double cumTransitionModeProbability = 0; + i = 0; + for(ParticleFilterMixing& filter : modes){ + + double prob = (transitionProbabilityMatrix(m,i) * filter.getModePosteriorProbability()) / focusedFilter.getPredictedModeProbability(); + filter.setTransitionModeProbability(prob); + + cumTransitionModeProbability += prob; + copyModes[i].setTransitionModeProbability(cumTransitionModeProbability); + + //std::cout << "Draw Mode Probability from mode " << m << i << " : " << prob << std::endl; + ++i; + } + + // draw new modes and particles + // Note: in most cases, the particles are already resampled within the filtering stage + // but in some cases they are not and therefore we need to draw cumulatively and not equally + + // todo: make the particle size dynamic depending on the kld or something else + // number of particles for this timestep + int numParticles = focusedFilter.getParticles().size(); + + std::vector> newParticles; + newParticles.resize(numParticles); + double equalWeight = 1.0 / numParticles; + + // draw modes cumulative + for(int k = 0; k < numParticles; ++k){ + + auto mode = drawMode(cumTransitionModeProbability); + newParticles[k] = drawParticle(mode); + newParticles[k].weight = equalWeight; + } + + focusedFilter.setParticles(newParticles); + + //iter + ++m; + + } + } + + private: + + /** draw a mode depending upon the transition mode probabilities */ + ParticleFilterMixing& drawMode(const double cumTransitionModeProbabilities){ + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, cumTransitionModeProbabilities); + + // draw a random value between [0:cumWeight] + const float rand = dist(genNorm); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (ParticleFilterMixing& filter, const float d) {return filter.getTransitionModeProbability() < d;}; + auto it = std::lower_bound(copyModes.begin(), copyModes.end(), rand, comp); + return *it; + + } + + + /** draw one particle according to its weight from the copy vector of a given mode */ + const Particle& drawParticle(ParticleFilterMixing& filter) { + + double weights = filter.getParticles().back().weight; + + // generate random values between [0:cumWeight] + std::uniform_real_distribution dist(0, filter.getParticles().back().weight); + + // draw a random value between [0:cumWeight] + const float rand = dist(genPart); + + // search comparator (cumWeight is ordered -> use binary search) + auto comp = [] (const Particle& s, const float d) {return s.weight < d;}; + auto it = std::lower_bound(filter.getParticles().begin(), filter.getParticles().end(), rand, comp); + return *it; + + } + + }; + + +} + + +#endif // MIXINGSAMPLERDIVERGENCY_H diff --git a/smc/sampling/CumulativeSampler.h b/smc/sampling/CumulativeSampler.h new file mode 100644 index 0000000..677d244 --- /dev/null +++ b/smc/sampling/CumulativeSampler.h @@ -0,0 +1,100 @@ +#ifndef CUMULATIVESAMPLER_H +#define CUMULATIVESAMPLER_H + +#include +#include +#include +#include "../Particle.h" +#include "ParticleTrajectorieSampler.h" + +namespace SMC { + + /** + * drawing trajectories using a cumulative drawer + */ + template + class CumulativeSampler : public ParticleTrajectorieSampler { + + public: + + /** ctor */ + CumulativeSampler(){ + + } + + /** draw a single particle */ + Particle drawSingleParticle(std::vector> const& particles){ + + // ensure the copy vector has the same size as the real particle vector + std::vector> particlesCopy; + particlesCopy.resize(particles.size()); + + // swap both vectors + particlesCopy = particles; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < particles.size(); ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + return draw(cumWeight, particlesCopy, particles); + + } + + /** draw a trajectorie of n particles */ + std::vector> drawTrajectorie(std::vector> const& particles, const int numRealizations){ + + // ensure the copy vector has the same size as the real particle vector + std::vector> particlesCopy; + particlesCopy.reserve(particles.size()); + particlesCopy = particles; + + // calculate cumulative weight + double cumWeight = 0; + for (uint32_t i = 0; i < particles.size(); ++i) { + cumWeight += particlesCopy[i].weight; + particlesCopy[i].weight = cumWeight; + } + + // now draw the initial weights and therefore the corresponding particles + std::vector> trajectorie; + trajectorie.reserve(numRealizations); + for (uint32_t i = 0; i < numRealizations; ++i) { + trajectorie.push_back(draw(cumWeight, particlesCopy, particles)); + } + + return trajectorie; + } + + private: + + /** function for drawing particles */ + Particle draw(const double cumWeight, std::vector> const& cumParticles, std::vector> const& origParticles){ + + // random value between [0;1] + const double rand01 = double(rand()) / double(RAND_MAX); + + // random value between [0; cumulativeWeight] + const double rand = rand01 * cumWeight; + + // search comparator + auto comp = [] (const Particle& s, const double d) {return s.weight < d;}; + auto it = std::lower_bound(cumParticles.begin(), cumParticles.end(), rand, comp); + + //get the idx for the iterator it. this is the same as std::distance(..,..) + int idx = it - cumParticles.begin(); + + //get the original weight instead of the cumulative weight + Particle drawnParticle = *it; + drawnParticle.weight = origParticles[idx].weight; + + return drawnParticle; + + } + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/sampling/ParticleTrajectorieSampler.h b/smc/sampling/ParticleTrajectorieSampler.h new file mode 100644 index 0000000..13501c3 --- /dev/null +++ b/smc/sampling/ParticleTrajectorieSampler.h @@ -0,0 +1,31 @@ +#ifndef PARTICLETRAJECTORIESAMPLER_H +#define PARTICLETRAJECTORIESAMPLER_H + +#include +#include "..//Particle.h" + +namespace SMC { + + /** + * interface for Sampling Trajectories of Particles + */ + template + class ParticleTrajectorieSampler { + + public: + + /** draw a single particle */ + virtual Particle drawSingleParticle(std::vector> const& particles) = 0; + + /** draw a trajectorie of all incoming particles / like resampling*/ + virtual std::vector> drawTrajectorie(std::vector>const& particles, const int num) = 0; + + private: + + /** function for drawing particles */ + virtual Particle draw(const double cumWeight, std::vector> const& cumParticles, std::vector> const& origParticles) = 0; + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/smoothing/ArtificialDistribution.h b/smc/smoothing/ArtificialDistribution.h new file mode 100644 index 0000000..671dca5 --- /dev/null +++ b/smc/smoothing/ArtificialDistribution.h @@ -0,0 +1,24 @@ +#ifndef ARTIFICIALDISTRIBUTION_H +#define ARTIFICIALDISTRIBUTION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for artificial distributions + */ + template + class ArtificialDistribution { + + public: + + /** calculate the probability/density*/ + virtual double calculate(Particle const& particle) = 0; + + }; + +} + +#endif // ARTIFICIALDISTRIBUTION_H diff --git a/smc/smoothing/BackwardFilter.h b/smc/smoothing/BackwardFilter.h new file mode 100644 index 0000000..969c360 --- /dev/null +++ b/smc/smoothing/BackwardFilter.h @@ -0,0 +1,57 @@ +#ifndef BACKWARDFILTER_H +#define BACKWARDFILTER_H + + +#include +#include + +#include "BackwardFilterTransition.h" + +#include "../sampling/ParticleTrajectorieSampler.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + + +namespace SMC { + + template + class BackwardFilter { + + public: + virtual State update(std::vector> const& forwardParticles) = 0; + + /** access to all backward / smoothed particles */ + virtual const std::vector>>& getbackwardParticles() = 0; + + /** set the estimation method to use */ + virtual void setEstimation(std::unique_ptr> estimation) = 0; + + /** set the transition method to use */ + virtual void setTransition(std::unique_ptr> transition) = 0; + + /** set the resampling method to use */ + virtual void setResampling(std::unique_ptr> resampler) = 0; + + /** set the resampling threshold as the percentage of efficient particles */ + virtual void setNEffThreshold(const double thresholdPercent) = 0; + + /** set sampler */ + virtual void setSampler(std::unique_ptr> sampler) { (void) sampler; }; + + /** get the used transition method */ + virtual BackwardFilterTransition* getTransition() = 0; + + /** reset */ + virtual void reset() {}; + + }; +} + +#endif // BACKWARDFILTER_H diff --git a/smc/smoothing/BackwardFilterTransition.h b/smc/smoothing/BackwardFilterTransition.h new file mode 100644 index 0000000..474b98b --- /dev/null +++ b/smc/smoothing/BackwardFilterTransition.h @@ -0,0 +1,33 @@ +#ifndef BACKWARDFILTERTRANSITION_H +#define BACKWARDFILTERTRANSITION_H + +#include +#include "../Particle.h" + +namespace SMC { + + /** + * interface for the user-defined backward filter transition. + * the transition describes the probability of a state change during the transition phase p(q_t+1 | q_t) + */ + template + class BackwardFilterTransition { + + public: + + /** + * @brief perform the transition p(q_t+1 | q_t) for all particles and possibilities + * if you do not use this abstract function, do not forget to throw an error if the user does + */ + virtual std::vector> transition(std::vector> const& toBeSmoothedParticles_qt, std::vector>const& alreadySmoothedParticles_qt1) = 0; + + /** + * @brief perform a forward transition based on the to be smoothed particles at time q_t and sample particles at time q_t+1, also gets an vector with controls c_1:T + * if you do not use this abstract function, do not forget to throw an error if the user does + */ + virtual std::vector> transition(std::vector> const& toBeSmoothedParticles_qt, std::vector const& controls_1T) = 0; + }; + +} + +#endif // BACKWARDFILTERTRANSITION_H diff --git a/smc/smoothing/BackwardSimulation.h b/smc/smoothing/BackwardSimulation.h new file mode 100644 index 0000000..5f3729b --- /dev/null +++ b/smc/smoothing/BackwardSimulation.h @@ -0,0 +1,258 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef BACKWARDSIMULATION_H_ +#define BACKWARDSIMULATION_H_ + + +#include +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../sampling/ParticleTrajectorieSampler.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * the main-class for the Backward Simulation Filter + * running "backwards" in time, generates multiple backwards trajectories + * (Realizations) by repeating the backward simulation M time. + * it can be started at a random time T of any forward particle filter + * [Monte Carlo smoothing for non-linear time series Godsill et al. '03] + * @param State the (user-defined) state for each particle + * @param numRealizations is the number of backward trajectories starting + */ + template + class BackwardSimulation : public BackwardFilter{ + + private: + + /** all smoothed particles T -> 1*/ + std::vector>> backwardParticles; + + /** container for particles */ + std::vector> smoothedParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the sampler for drawing trajectories */ + std::unique_ptr> sampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** number of realizations to be calculated */ + int numRealizations; + + /** is update called the first time? */ + bool firstFunctionCall; + + + public: + + /** ctor */ + BackwardSimulation(int numRealizations) { + this->numRealizations = numRealizations; + backwardParticles.reserve(numRealizations); + smoothedParticles.reserve(numRealizations); + firstFunctionCall = true; + } + + /** dtor */ + ~BackwardSimulation() { + ; + } + + /** reset **/ + void reset(){ + this->numRealizations = numRealizations; + + backwardParticles.clear(); + backwardParticles.reserve(numRealizations); + + smoothedParticles.clear(); + smoothedParticles.reserve(numRealizations); + + firstFunctionCall = true; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the sampler method to use */ + void setSampler(std::unique_ptr> sampler){ + Assert::isNotNull(sampler, "setSampler() MUST not be called with a nullptr!"); + this->sampler = std::move(sampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * gets the weighted sample set of a standard condensation + * particle filter in REVERSED order! + */ + State update(std::vector> const& forwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + //storage for single trajectories / smoothed particles + smoothedParticles.clear(); + + // Choose \tilde x_T = x^(i)_T with probability w^(i)_T + // Therefore sample independently from the categorical distribution of weights. + if(firstFunctionCall){ + + smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); + + firstFunctionCall = false; + backwardParticles.push_back(smoothedParticles); + + const State es = estimation->estimate(smoothedParticles); + return es; + } + + // compute weights using the transition model + // transitionWeigths[numRealizations][numParticles] + std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); + + //get the next trajectorie for a realisation + for(int j = 0; j < numRealizations; ++j){ + + //vector for the current smoothedWeights at time t + std::vector> smoothedWeights; + smoothedWeights.resize(forwardParticles.size()); + smoothedWeights = forwardParticles; + + //check if all transitionWeights are zero + double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0); + Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero"); + + int i = 0; + for (auto& w : transitionWeights.at(j)) { + + // multiply the weight of the particles at time t and normalize + smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w); + if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";} + + // iter + ++i; + } + + //get the sum of all weights + auto lambda = [](double current, const Particle& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + + //normalize the weights + if(weightSumSmoothed != 0.0){ + for (int i = 0; i < smoothedWeights.size(); ++i){ + smoothedWeights.at(i).weight /= weightSumSmoothed; + } + + //check if normalization worked + double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1"); + } + + + //draw the next trajectorie at time t for a realization and save them + smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights)); + + //throw if weight of smoothedParticle is zero + //in practice this is possible, if a particle is completely separated from the rest and is therefore + //weighted zero or very very low. + Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight"); + } + + + if(resampler) + { + + //TODO - does this even make sense? + std::cout << "Warning - Resampling is not yet implemented!" << std::endl; +// //resampling if necessery +// double sum = 0.0; +// double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0); +// for (auto& p : smoothedParticles) { +// p.weight /= weightSum; +// sum += (p.weight * p.weight); +// } + +// const double neff = 1.0/sum; +// if (neff != neff) {throw "detected NaN";} + +// // if the number of efficient particles is too low, perform resampling +// if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } + } + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // estimate the current state + const State est = estimation->estimate(smoothedParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* BACKWARDSIMULATION_H_ */ diff --git a/smc/smoothing/CondensationBackwardFilter.h b/smc/smoothing/CondensationBackwardFilter.h new file mode 100644 index 0000000..d970755 --- /dev/null +++ b/smc/smoothing/CondensationBackwardFilter.h @@ -0,0 +1,226 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef CONDENSATIONBACKWARDFILTER_H_ +#define CONDENSATIONBACKWARDFILTER_H_ + + +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +long long count = 0.0; + +namespace SMC { + + /** + * the main-class for the Condensation Backward Filter + * running "backwards" in time, updating every timestep, no resampling + * it can be started at a random time T of an forward particle filter + * [Smoothing filter for condensation by Isard and Blake '98] + * @param State the (user-defined) state for each particle + */ + template + class CondensationBackwardFilter : public BackwardFilter { + + private: + + /** all smoothed particles 1 -> T*/ + std::vector>> backwardParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + public: + + /** ctor */ + CondensationBackwardFilter() { + } + + /** dtor */ + ~CondensationBackwardFilter() { + ; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * gets the weighted sample set of a standard condensation + * particle filter in REVERSED order! + */ + State update(std::vector> const& forwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // since the algorithm starts at T-1 we need to initialize with the first set of forwardParticels + // psi_T = pi_T + static bool firstFunctionCall = true; + if(firstFunctionCall){ + backwardParticles.push_back(forwardParticles); + firstFunctionCall = false; + + std::vector> tt = forwardParticles; + const State es = estimation->estimate(tt); + return es; + } + + //weightsume for normalization + double weightSum = 0.0; + + // perform the transition step p(x_t+1|x_t) + std::vector> predictionProbabilities = transition->transition(forwardParticles, backwardParticles.back()); + + // calculate the correction factors + std::vector correctionFactors; + for(int m = 0; m < forwardParticles.size(); ++m){ + + double gamma = 0.0; + for(int k = 0; k < forwardParticles.size(); ++k){ + // gamma(m) = sum(pi(k) * alpha(m,k)) + gamma += forwardParticles[k].weight * predictionProbabilities[m][k]; + + if (gamma != gamma) {throw "detected NaN";} + } + correctionFactors.push_back(gamma); + } + + // approximate backward variables + std::vector> smoothedParticles = forwardParticles; + for(int n = 0; n < forwardParticles.size(); ++n){ + + double delta = 0.0; + for(int m = 0; m < forwardParticles.size(); ++m){ + // delta(n) = sum(psi(m) * alpha(m,n) / gamma(m)) + + //!! THIS IS A HACK !! Gamma is getting zero if the prob is to damn low. This would results in NaN for gamma + //!! Therefore we set alpha(m,n) / gamma(m) = 1.0; + if(correctionFactors[m] == 0.0){ + delta += backwardParticles.back().at(m).weight; + std::cout << "Gamma is Zero" << count ++ << std::endl; + } + else + delta += backwardParticles.back().at(m).weight * (predictionProbabilities[m][n] / correctionFactors[m]); + + if (delta != delta) {throw "detected NaN";} + } + + // Evaluate smoothing weights + // psi(n) = pi(n) * delta(n) + double weight = delta * forwardParticles[n].weight; + smoothedParticles[n].weight = weight; + + // fill weightsum + weightSum += weight; + + if (forwardParticles[n].weight != forwardParticles[n].weight) {throw "detected NaN";} + if (delta != delta) {throw "detected NaN";} + if (weight != weight) {throw "detected NaN";} + if (weightSum != weightSum) {throw "detected NaN";} + } + + // normalize the particle weights and thereby calculate N_eff + double sum = 0.0; + for (auto& p : smoothedParticles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + + // sanity check +// if (p.state.heading != p.state.heading) {throw "detected NaN";} +// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";} +// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";} +// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";} +// if (p.weight != p.weight) {throw "detected NaN";} + } + + const double neff = 1.0/sum; + if (neff != neff) {throw "detected NaN";} + + // estimate the current state + const State est = estimation->estimate(smoothedParticles); + +// if (est.heading != est.heading) {throw "detected NaN";} +// if (est.z_nr != est.z_nr) {throw "detected NaN";} +// if (est.x_cm != est.x_cm) {throw "detected NaN";} +// if (est.y_cm != est.y_cm) {throw "detected NaN";} + + if(resampler) + { + // if the number of efficient particles is too low, perform resampling + if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } + } + + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* CONDENSATIONBACKWARDFILTER_H_ */ diff --git a/smc/smoothing/ForwardFilterHistory.h b/smc/smoothing/ForwardFilterHistory.h new file mode 100644 index 0000000..b8841c7 --- /dev/null +++ b/smc/smoothing/ForwardFilterHistory.h @@ -0,0 +1,142 @@ +#ifndef FORWARDFILTERHISTORY_H +#define FORWARDFILTERHISTORY_H + +#include +#include "../Particle.h" +#include "../../data/Timestamp.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * @brief Provides a data structur for the data available at a specific timestamp of the forward filtering procedure. + * @brief Timestamp; ParticleSet (After Transition and Update); Controls; Observations + */ + template + class ForwardFilterHistory { + + + private: + + // NOTE: it would be possible to make some kind of struct for this, but in many upcoming functions and methods, i am not able + // to use all this informations. sometimes if have something like p(q_t+1| q_t, o_t) or p(o_t | q_t, c_t). So keep it flexible! + std::vector timestamps; + std::vector>> particleSets; + std::vector controls; + std::vector observations; + + public: + + ForwardFilterHistory(){ + //empty ctor + } + + void add(Timestamp time, std::vector>> set, Control control, Observation obs){ + + // Is empty? Null? etc. + Assert::isNotNull(time, "Timestamp is Null"); + Assert::isNotNull(set, "Particle Set is Null"); + Assert::isNotNull(control, "Control is Null"); + Assert::isNotNull(obs, "Observation is Null"); + + timestamps.push_back(time); + particleSets.push_back(set); + controls.push_back(control); + observations.push_back(obs); + } + + void removeLatest(){ + + particleSets.pop_back(); + controls.pop_back(); + observations.pop_back(); + } + + void removeFirst(){ + + particleSets.erase(particleSets.begin()); + controls.erase(controls.begin()); + observations.erase(observations.begin()); + } + + /** + * @brief Return the particles from [latestFilterUpdate - @param idx] + * @return returns vector of particles. note: c11 makes a std::move here + */ + std::vector> getParticleSet(idx = 0){ + return particleSets.at(particleSets.end() - idx); + } + + /** + * @brief getControl from [latestFilterUpdate - @param idx] + * @return const controls object + */ + const Control getControl(idx = 0){ + return controls.at(controls.end() - idx); + } + + /** + * @brief getObservationf rom [latestFilterUpdate - @param idx] + * @return const obervations object + */ + const Observation getObservation (idx = 0){ + return observations.at(observations.end() - idx); + } + + /** + * @brief Return the timestamp from [latestFilterUpdate - @param idx] + * @return returns a Timstampf object + */ + std::vector> getTimestamp(idx = 0){ + return timestamps.at(particleSets.end() - idx); + } + + /** + * @brief getLatestFilterUpdateNum + * @return num of particleSets size + */ + const int getLatestFilterUpdateNum(){ + return particleSets.size(); + } + + /** + * @brief getLatestParticleSet Reference + * @return return particle set Note: c11 std::move by vector + */ + std::vector> getLatestParticleSet(){ + return particleSets.back(); + } + + /** + * @brief getLatestControls + * @return const control object + */ + const Control getLatestControls(){ + return controls.back(); + } + + /** + * @brief getLatestObservation + * @return const observation object + */ + const Observation getLatestObservation(){ + return observations.back(); + } + + + /** + * @brief getLatestTimestamp + * @return const Timestamp object + */ + const Timestamp getLatestTimestamp(){ + return timestamps.back(); + } + + + + }; + +} + +#endif // FORWARDFILTERHISTORY_H diff --git a/smc/smoothing/TwoFilterSmoothing.h b/smc/smoothing/TwoFilterSmoothing.h new file mode 100644 index 0000000..0ae69f3 --- /dev/null +++ b/smc/smoothing/TwoFilterSmoothing.h @@ -0,0 +1,187 @@ +/* + * CondensationBackwardFilter.h + * + * Created on: Jun 23, 2015 + * Author: Toni Fetzer + */ + +#ifndef TWOFILTERSMOOTHING_H_ +#define TWOFILTERSMOOTHING_H_ + + +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" +#include "ArtificialDistribution.h" + +#include "../Particle.h" + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" + +#include "../../Assertions.h" + +namespace SMC { + + /** + * Smoothing Forward and Backward Filter together. + * Call the Update Function. + * Algorithm taken from [Briers04] Smoothing Algorithms for State-Space Models + */ + template + class TwoFilterSmoothing { + + private: + + /** all smoothed particles 1 -> T*/ + std::vector>> smoothedParticles; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** artificial distribuation */ + std::unique_ptr> artificialDistribution; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + + public: + + /** ctor */ + TwoFilterSmoothing() { + } + + /** dtor */ + ~TwoFilterSmoothing() { + ; + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getsmoothedParticles() { + return smoothedParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + void setArtificialDistribution(std::unique_ptr> artificialDistribution){ + Assert::isNotNull(artificialDistribution, "setArtificialDistribution() MUST not be called with a nullptr!"); + this->artificialDistribution = std::move(artificialDistribution); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return this->transition.get(); + } + + /** + * perform update: transition -> correction -> approximation + * particles from a forwards filter are used to re-weight those from a backwards filter + * so that they represent the target distribution. + * @param: forwardParticles at t-1 + * @param: backwardparticles at t + */ + State update(std::vector> const& forwardParticles, std::vector> const& backwardParticles) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + // perform the transition step p(backward_x_t|forward_x_t-1) + std::vector> predictionProbabilities = transition->transition(forwardParticles, backwardParticles); + + //we are using the forwardparticles to re-weight the backward filter (other direction also possible?) + std::vector> currentParticles = backwardParticles; + + double weightSum = 0.0; + + // calculate the correction factors + for(int j = 0; j < backwardParticles.size(); ++j){ + + double alpha = 0.0; + for(int i = 0; i < backwardParticles.size(); ++i){ + // alpha(j) = sum(forward_weight_t-1 * prediction) + alpha += forwardParticles[i].weight * predictionProbabilities[j][i]; + + if (alpha != alpha) {throw "detected NaN";} + } + + double gamma = 1.0; + if(artificialDistribution){ + gamma = artificialDistribution->calculate(backwardParticles[j]); + } + + //calc the weight + double weight = (currentParticles[j].weight / gamma) * alpha; + + + currentParticles[j].weight = weight; + weightSum += weight; + } + + // normalize the particle weights and thereby calculate N_eff + double sum = 0.0; + for (auto& p : currentParticles) { + p.weight /= weightSum; + sum += (p.weight * p.weight); + } + + double neff = 1.0/sum; + if (neff != neff) {neff = 1.0;} + + // estimate the current state + const State est = estimation->estimate(currentParticles); + + if(resampler) + { + // if the number of efficient particles is too low, perform resampling + if (neff < currentParticles.size() * nEffThresholdPercent) { resampler->resample(currentParticles); } + } + + + // push_back the smoothedParticles + smoothedParticles.push_back(currentParticles); + + // done + return est; + + } + + }; + +} + + +#endif /* TWOFILTERSMOOTHING_H_ */ diff --git a/tests/smc/filtering/TestParticles.cpp b/tests/smc/filtering/TestParticles.cpp new file mode 100644 index 0000000..c4da265 --- /dev/null +++ b/tests/smc/filtering/TestParticles.cpp @@ -0,0 +1,44 @@ +/* + * TestParticles.cpp + * + * Created on: Sep 18, 2013 + * Author: Frank Ebner + */ + +#ifdef FIXME + +#ifdef WITH_TESTS + +#include "../../../Tests.h" + +#include "../particles/ParticleFilter.h" +#include "../particles/ParticleModel.h" +#include "../particles/ParticleSensor.h" +#include "../wifi/math/WiFiMath.h" +#include "../lib/gnuplot/Gnuplot.h" +#include +#include "../floorplan/FloorPlan.h" +#include "../floorplan/FloorPlanFactory.h" +#include "../lib/misc/File.h" +#include "../particles/ParticleMath.h" +#include "../particles/resampling/ParticleResamplingSimple.h" +#include "../particles/resampling/ParticleResamplingNone.h" + +#include "../wifi/factory/WiFiHelper.h" + +#include +#include + +typedef Point TestState; + + +TEST(Particles, init) { + + //create filter + + +} + +#endif + +#endif diff --git a/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp new file mode 100644 index 0000000..159161d --- /dev/null +++ b/tests/smc/merging/mixing/TestMixingSamplerDivergency.cpp @@ -0,0 +1,146 @@ +#ifdef WITH_TESTS + +#include "../../../../smc/merging/mixing/MixingSamplerDivergency.h" +#include "../../../../smc/filtering/ParticleFilterMixing.h" + +#include "../../../Tests.h" + +namespace K { + + struct MyState { + double x; + double y; + MyState() : x(0), y(0) {;} + MyState(double x, double y) : x(x), y(y) {;} + MyState& operator += (const MyState& other) { + x += other.x; + y += other.y; + return *this; + } + MyState& operator /= (double d) { + x /= d; + y /= d; + return *this; + } + MyState operator * (double d) const { + return MyState(x*d, y*d); + } + MyState& operator = (const MyState& other) { + this->x = other.x; + this->y = other.y; + return *this; + } + double distance(const MyState& o) const { + return std::sqrt( (x-o.x)*(x-o.x) + (y-o.y)*(y-o.y) ) / 4.9; + } + bool belongsToRegion(const MyState& o) const { + return distance(o) <= 1.0; + } + + }; + + struct MyControl { + + }; + + struct MyObservation { + double x; + double y; + MyObservation() : x(0), y(0) {;} + void set(double x, double y) {this->x = x; this->y = y;} + }; + + class MyInitializer1 : public SMC::ParticleFilterInitializer { + void initialize(std::vector>& particles) override { + for (SMC::Particle& p : particles) { + p.state.x = 0; + p.state.y = 0; + p.weight = 1.0 / particles.size(); + } + } + }; + + class MyInitializer2 : public SMC::ParticleFilterInitializer { + void initialize(std::vector>& particles) override { + for (SMC::Particle& p : particles) { + p.state.x = 1; + p.state.y = 1; + p.weight = 1.0 / particles.size(); + } + } + }; + + + TEST(Mixing, standard) { + + //init particle filters + int numParticles = 1000; + Eigen::MatrixXd transition(2,2); + transition << 0.8, 0.2, 0.2, 0.8; + + SMC::ParticleFilterMixing mode1(numParticles, std::unique_ptr(new MyInitializer1()), 0.5); + SMC::ParticleFilterMixing mode2(numParticles, std::unique_ptr(new MyInitializer2()), 0.5); + + std::vector> modes; + modes.push_back(mode1); + modes.push_back(mode2); + + + SMC::MixingSamplerDivergency mixer; + + //run the mixing and check results + for(int t = 0; t < 100; ++t){ + mixer.mixAndSample(modes, transition); + + int cnt_zero = 0; + int cnt_ones = 0; + for(int i = 0; i < modes[0].getParticles().size(); ++i){ + + if(modes[0].getParticles()[i].state.x == 0){ + cnt_zero++; + }else{ + cnt_ones++; + } + } + + std::cout << "MODE1 Zeros: " << (double) cnt_zero / numParticles<< std::endl; + std::cout << "MODE1 Ones: " << (double) cnt_ones/ numParticles << std::endl; + + + cnt_zero = 0; + cnt_ones = 0; + for(int i = 0; i < modes[0].getParticles().size(); ++i){ + + if(modes[1].getParticles()[i].state.x == 0){ + cnt_zero++; + }else{ + cnt_ones++; + } + } + + std::cout << "MODE2 Zeros: " << (double) cnt_zero / numParticles<< std::endl; + std::cout << "MODE2 Ones: " << (double) cnt_ones/ numParticles << std::endl; + } + + + + } + + TEST(Mixing, differentParticleSize) { + + } + + + TEST(Mixing, dynamicTransitionMatrix) { + + } + + TEST(Mixing, ThreeFilters) { + + } + +} + + + +#endif From 74981c6a4538e91ec09d1b6d158fe99eedb682ce Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 6 Dec 2017 17:37:14 +0100 Subject: [PATCH 3/4] ref #39 smoothing is refactored KDE smoothing algorithmisch mal geschrieben, jetzt noch testen --- Assertions.h | 2 +- .../v2/modules/WalkModuleHeadingVonMises.h | 2 +- math/boxkde/BoxGaus.h | 127 ++++++++ math/boxkde/BoxSizes.h | 85 ++++++ math/boxkde/DataStructures.h | 115 ++++++++ math/boxkde/GausLib.h | 34 +++ math/boxkde/Grid2D.h | 211 ++++++++++++++ math/boxkde/Image2D.h | 188 ++++++++++++ math/boxkde/benchmark.h | 105 +++++++ smc/filtering/ParticleFilterHistory.h | 20 +- smc/smoothing/BackwardFilter.h | 19 +- smc/smoothing/BackwardSimulation.h | 213 ++++++++------ smc/smoothing/FastKDESmoothing.h | 271 ++++++++++++++++++ smc/smoothing/ForwardFilterHistory.h | 38 +-- 14 files changed, 1303 insertions(+), 127 deletions(-) create mode 100644 math/boxkde/BoxGaus.h create mode 100644 math/boxkde/BoxSizes.h create mode 100644 math/boxkde/DataStructures.h create mode 100644 math/boxkde/GausLib.h create mode 100644 math/boxkde/Grid2D.h create mode 100644 math/boxkde/Image2D.h create mode 100644 math/boxkde/benchmark.h create mode 100644 smc/smoothing/FastKDESmoothing.h diff --git a/Assertions.h b/Assertions.h index 4c727b8..be6c370 100644 --- a/Assertions.h +++ b/Assertions.h @@ -52,7 +52,7 @@ namespace Assert { } template static inline void isNotNull(const T& v, const STR err) { - if (v == nullptr) {doThrow(err);} + if (v == nullptr) {doThrow(err);} } template static inline void isNotNaN(const T v, const STR err) { diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h index 653f9e6..e29e3f8 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h +++ b/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); } diff --git a/math/boxkde/BoxGaus.h b/math/boxkde/BoxGaus.h new file mode 100644 index 0000000..1888cb6 --- /dev/null +++ b/math/boxkde/BoxGaus.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include + +#include "BoxSizes.h" + +template +struct BoxGaus +{ + void boxfilter(std::vector& input, size_t w, size_t h, unsigned filterSize) + { + assertMsg((filterSize % 2) == 1, "filterSize must be odd"); + + unsigned radius = filterSize / 2; + + std::vector buffer(input.size()); + boxBlur(input, buffer, w, h, radius); + boxBlur(buffer, input, w, h, radius); + } + + void approxGaus(Image2D& input, T sigmaX, T sigmaY, unsigned nFilt) + { + approxGaus(input.data(), input.width, input.height, sigmaX, sigmaY, nFilt); + } + + void approxGaus(std::vector& input, size_t w, size_t h, T sigmaX, T sigmaY, unsigned nFilt) + { + BoxSizes bsX(sigmaX, nFilt); + BoxSizes bsY(sigmaY, nFilt); + std::vector 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 &src, std::vector &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 + } + + // Überhangbereich links vom Bild + for (size_t j = 0; j <= r; j++) + { + val += src[ri] - fv; + dst[j + i*w] = val * iarr; + + ri += w; + ti += w; + } + + // Bildbereich + for (size_t j = r + 1; j < h - r; j++) + { + val += src[ri] - src[li]; + dst[j + i*w] = val * iarr; + + li += w; + ri += w; + ti += w; + } + + // Überhangbereich rechts vom Bild + for (size_t j = h - r; j < h; j++) + { + val += lv - src[li]; + dst[j + i*w] = val * iarr; + + li += w; + ti += w; + } + } + } +}; + + + + + + + diff --git a/math/boxkde/BoxSizes.h b/math/boxkde/BoxSizes.h new file mode 100644 index 0000000..ca228da --- /dev/null +++ b/math/boxkde/BoxSizes.h @@ -0,0 +1,85 @@ +#pragma once + +#include "DataStructures.h" + +template +struct BoxSizes +{ + static_assert(std::is_floating_point::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 +struct ExBoxSizes +{ + static_assert(std::is_floating_point::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); + } +}; diff --git a/math/boxkde/DataStructures.h b/math/boxkde/DataStructures.h new file mode 100644 index 0000000..64cb419 --- /dev/null +++ b/math/boxkde/DataStructures.h @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include + +template +struct BoundingBox +{ + static_assert(std::is_arithmetic::value, "This class only works with floats or integers."); + + T MinX, MaxX, MinY, MaxY; + + BoundingBox(T MinX = std::numeric_limits::max(), + T MaxX = std::numeric_limits::lowest(), + T MinY = std::numeric_limits::max(), + T MaxY = std::numeric_limits::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 +struct Point2D +{ + static_assert(std::is_arithmetic::value, "This class only works with floats and integers."); + + T X, Y; + + Point2D(T x = 0, T y = 0) + : X(x), Y(y) + { } +}; + +template +struct Size2D +{ + static_assert(std::is_arithmetic::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 +std::ostream& operator<<(std::ostream& os, const Point2D& pt) +{ + return os << "(" << pt.X << "; " << pt.Y << ")"; +} + +template +std::ostream& operator<<(std::ostream& os, const BoundingBox& bb) +{ + return os << "(X: " << bb.MinX << " - " << bb.MaxX << ";" + << " Y: " << bb.MinY << " - " << bb.MaxY << ")"; +} + + + diff --git a/math/boxkde/GausLib.h b/math/boxkde/GausLib.h new file mode 100644 index 0000000..460e3b9 --- /dev/null +++ b/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); + + \ No newline at end of file diff --git a/math/boxkde/Grid2D.h b/math/boxkde/Grid2D.h new file mode 100644 index 0000000..25bd4c9 --- /dev/null +++ b/math/boxkde/Grid2D.h @@ -0,0 +1,211 @@ +#pragma once + +#include + +#include "DataStructures.h" +#include "Image2D.h" + +template +struct Grid2D +{ + static_assert(std::is_floating_point::value, "Grid2D only supports float values"); + + BoundingBox bb; + size_t numBinsX, numBinsY; + T binSizeX, binSizeY; +private: + Image2D data; + +public: + + Grid2D(){ } //TODO: fast hack + + Grid2D(BoundingBox bb, size_t numBins) + : Grid2D(bb, numBins, numBins) + { + } + + Grid2D(BoundingBox 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& image() { return data; } + const Image2D& 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>& 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>& samples, const std::vector& 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 add(T x, T y, T w) + { + if (assertCond(bb.isInside(x,y))) + { + std::stringstream ss; + ss << "Point " << Point2D(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(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 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(bin_x, bin_y); + } + + // Takes a Size2D in input space and converts it to grid space coordiantes + Size2D asGridSpace(Size2D sz) const + { + return Size2D(sz.sX / binSizeX, sz.sY / binSizeY); + } + + // Takes a point in grid space and converts it to input space coordinates + Point2D asInputSpace(Point2D pt) const + { + return asInputSpace(pt.X, pt.Y); + } + + // Takes a point in grid space and converts it to input space coordinates + Point2D asInputSpace(size_t x, size_t y) const + { + return Point2D(x * binSizeX + bb.MinX + T(0.5)*binSizeX, + y * binSizeY + bb.MinY + T(0.5)*binSizeY); + } + + + T maximum(Point2D& pt) const + { + Point2D gridPt; + + T maxValue = image().maximum(gridPt); + pt = asInputSpace(gridPt); + return maxValue; + } + +private: + Point2D 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(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; +template struct Grid2D; diff --git a/math/boxkde/Image2D.h b/math/boxkde/Image2D.h new file mode 100644 index 0000000..714264d --- /dev/null +++ b/math/boxkde/Image2D.h @@ -0,0 +1,188 @@ +#pragma once + +#include +#include +#include +#include + +#include "DataStructures.h" + +enum struct LineDirection { X, Y }; + +template +struct ImageView2D +{ + static_assert(std::is_arithmetic::value, "Image only supports integers or floats"); + + // forward declaration + //enum struct LineDirection; + template struct LineView; + template 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 coordFromIndex(size_t index) const + { + assertMsg(index < size, "Index out of bounds"); + return Point2D(index % width, index / width); + } + + Point2D maximum() const + { + size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); + return coordFromIndex(maxValueIndex); + } + + TValue maximum(Point2D& 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& 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 transpose() const + { + return ImageView2D(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> findLocalMaxima(int radiusX = 1, int radiusY = 1) const + { + std::vector> result; + findLocalMaxima(result, radiusX, radiusY); + return result; + } + + void findLocalMaxima(std::vector>& 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 +struct Image2D : public ImageView2D +{ + static_assert(std::is_arithmetic::value, "Image only supports integers or floats"); + + std::vector values_vec; + + Image2D() + { + } + + Image2D(size_t width, size_t height) + : ImageView2D(width, height), values_vec(width * height) + { + this->values = values_vec.data(); + } + + Image2D(size_t width, size_t height, std::vector& data) + : ImageView2D(width, height), values_vec(data) + { + assertMsg(data.size() == width*height, "Sizes must be the same"); + this->values = values_vec.data(); + } + + + std::vector& data() { return values_vec; } + const std::vector& data() const { return values_vec; } +}; + + +template struct ImageView2D; +template struct ImageView2D; + +template struct Image2D; +template struct Image2D; diff --git a/math/boxkde/benchmark.h b/math/boxkde/benchmark.h new file mode 100644 index 0000000..0b8210f --- /dev/null +++ b/math/boxkde/benchmark.h @@ -0,0 +1,105 @@ +#pragma once +#include +#include +#include + +template +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::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 +static BenchResult benchmarkEx(std::string name, size_t count, F&& lambda) +{ + std::vector 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(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(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(clock::now() - lapTime); + + std::cout << str << (str.empty() ? "" : " ") << "took " << duration.count() << "us (" << duration.count() / 1000.0f << "ms)" << std::endl; + + lapTime = clock::now(); + } +}; \ No newline at end of file diff --git a/smc/filtering/ParticleFilterHistory.h b/smc/filtering/ParticleFilterHistory.h index 15cacb2..1cb2f74 100644 --- a/smc/filtering/ParticleFilterHistory.h +++ b/smc/filtering/ParticleFilterHistory.h @@ -80,10 +80,10 @@ namespace SMC { return particles; } - /** access to all non resample particles */ - const std::vector>& getNonResamplingParticles() { - return particlesNoResampling; - } + /** access to all non resample particles */ + const std::vector>& getNonResamplingParticles() { + return particlesNoResampling; + } /** initialize/re-start the particle filter */ void init() { @@ -91,11 +91,11 @@ namespace SMC { initializer->initialize(particles); } - void setAngle(const double angle){ - for(SMC::Particle& p : particles){ - p.state.walkState.heading = angle; - } - } + void setAngle(const double angle){ + for(SMC::Particle& p : particles){ + p.state.walkState.heading = angle; + } + } /** set the resampling method to use */ @@ -148,7 +148,7 @@ namespace SMC { Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); // perform the transition step - transition->transition(particles, control); + transition->transition(particles, control); // perform the evaluation step and calculate the sum of all particle weights const double weightSum = evaluation->evaluation(particles, observation); diff --git a/smc/smoothing/BackwardFilter.h b/smc/smoothing/BackwardFilter.h index 969c360..b83f548 100644 --- a/smc/smoothing/BackwardFilter.h +++ b/smc/smoothing/BackwardFilter.h @@ -6,6 +6,7 @@ #include #include "BackwardFilterTransition.h" +#include "ForwardFilterHistory.h" #include "../sampling/ParticleTrajectorieSampler.h" @@ -16,6 +17,7 @@ #include "../filtering/ParticleFilterEvaluation.h" #include "../filtering/ParticleFilterInitializer.h" + #include "../../Assertions.h" @@ -25,7 +27,19 @@ namespace SMC { class BackwardFilter { public: - virtual State update(std::vector> const& forwardParticles) = 0; + + /** + * @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& forwardFilter, int lag = 0) = 0; + + /** + * @brief getEstimations + * @return vector with all estimations running backward in time. T -> 0 + */ + virtual std::vector getEstimations() = 0; /** access to all backward / smoothed particles */ virtual const std::vector>>& getbackwardParticles() = 0; @@ -48,9 +62,6 @@ namespace SMC { /** get the used transition method */ virtual BackwardFilterTransition* getTransition() = 0; - /** reset */ - virtual void reset() {}; - }; } diff --git a/smc/smoothing/BackwardSimulation.h b/smc/smoothing/BackwardSimulation.h index 5f3729b..271790f 100644 --- a/smc/smoothing/BackwardSimulation.h +++ b/smc/smoothing/BackwardSimulation.h @@ -46,8 +46,8 @@ namespace SMC { /** all smoothed particles T -> 1*/ std::vector>> backwardParticles; - /** container for particles */ - std::vector> smoothedParticles; + /** all estimations calculated */ + std::vector estimatedStates; /** the estimation function to use */ std::unique_ptr> estimation; @@ -77,26 +77,13 @@ namespace SMC { BackwardSimulation(int numRealizations) { this->numRealizations = numRealizations; backwardParticles.reserve(numRealizations); - smoothedParticles.reserve(numRealizations); firstFunctionCall = true; } /** dtor */ ~BackwardSimulation() { - ; - } - - /** reset **/ - void reset(){ - this->numRealizations = numRealizations; - backwardParticles.clear(); - backwardParticles.reserve(numRealizations); - - smoothedParticles.clear(); - smoothedParticles.reserve(numRealizations); - - firstFunctionCall = true; + estimatedStates.clear(); } /** access to all backward / smoothed particles */ @@ -140,118 +127,154 @@ namespace SMC { } /** - * perform update: transition -> correction -> approximation - * gets the weighted sample set of a standard condensation - * particle filter in REVERSED order! + * @brief update + * @param forwardHistory + * @return */ - State update(std::vector> const& forwardParticles) { + State update(ForwardFilterHistory& 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!"); - //storage for single trajectories / smoothed particles - smoothedParticles.clear(); + //init for backward filtering + std::vector> smoothedParticles; + smoothedParticles.reserve(numRealizations); + firstFunctionCall = true; - // Choose \tilde x_T = x^(i)_T with probability w^(i)_T - // Therefore sample independently from the categorical distribution of weights. - if(firstFunctionCall){ - - smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); - - firstFunctionCall = false; - backwardParticles.push_back(smoothedParticles); - - const State es = estimation->estimate(smoothedParticles); - return es; + if(lag == 666){ + lag = forwardHistory.size() - 1; } - // compute weights using the transition model - // transitionWeigths[numRealizations][numParticles] - std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); + //check if we have enough data for lag + if(forwardHistory.size() <= lag){ + lag = forwardHistory.size() - 1; + } - //get the next trajectorie for a realisation - for(int j = 0; j < numRealizations; ++j){ + //iterate through all forward filtering steps + for(int i = 0; i <= lag; ++i){ + std::vector> forwardParticles = forwardHistory.getParticleSet(i); - //vector for the current smoothedWeights at time t - std::vector> smoothedWeights; - smoothedWeights.resize(forwardParticles.size()); - smoothedWeights = forwardParticles; + //storage for single trajectories / smoothed particles + smoothedParticles.clear(); - //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"); + // Choose \tilde x_T = x^(i)_T with probability w^(i)_T + // Therefore sample independently from the categorical distribution of weights. + if(firstFunctionCall){ - int i = 0; - for (auto& w : transitionWeights.at(j)) { + smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); - // multiply the weight of the particles at time t and normalize - smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w); - if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";} + firstFunctionCall = false; + backwardParticles.push_back(smoothedParticles); - // iter - ++i; + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); } - //get the sum of all weights - auto lambda = [](double current, const Particle& a){return current + a.weight; }; - double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + // compute weights using the transition model + // transitionWeigths[numRealizations][numParticles] + std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); - //normalize the weights - if(weightSumSmoothed != 0.0){ - for (int i = 0; i < smoothedWeights.size(); ++i){ - smoothedWeights.at(i).weight /= weightSumSmoothed; + //get the next trajectorie for a realisation + for(int j = 0; j < numRealizations; ++j){ + + //vector for the current smoothedWeights at time t + std::vector> smoothedWeights; + smoothedWeights.resize(forwardParticles.size()); + smoothedWeights = forwardParticles; + + //check if all transitionWeights are zero + double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0); + Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero"); + + int 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; } - //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"); + //get the sum of all weights + auto lambda = [](double current, const Particle& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + + //normalize the weights + if(weightSumSmoothed != 0.0){ + for (int 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"); } - //draw the next trajectorie at time t for a realization and save them - smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights)); + 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); + } - //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"); } + //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. ) - if(resampler) - { + // würde es sinn machen, die estimations auch mit zu speichern? - //TODO - does this even make sense? - std::cout << "Warning - Resampling is not yet implemented!" << std::endl; -// //resampling if necessery -// double sum = 0.0; -// double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0); -// for (auto& p : smoothedParticles) { -// p.weight /= weightSum; -// sum += (p.weight * p.weight); -// } - -// const double neff = 1.0/sum; -// if (neff != neff) {throw "detected NaN";} - -// // if the number of efficient particles is too low, perform resampling -// if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } - } - - // push_back the smoothedParticles - backwardParticles.push_back(smoothedParticles); - - // estimate the current state - const State est = estimation->estimate(smoothedParticles); - - // done - return est; + return estimatedStates.back(); } + std::vector getEstimations(){ + return estimatedStates; + } + + }; + } diff --git a/smc/smoothing/FastKDESmoothing.h b/smc/smoothing/FastKDESmoothing.h new file mode 100644 index 0000000..0777c91 --- /dev/null +++ b/smc/smoothing/FastKDESmoothing.h @@ -0,0 +1,271 @@ +#ifndef FASTKDESMOOTHING_H +#define FASTKDESMOOTHING_H + +#include +#include +#include + +#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 + class FastKDESmoothing : public BackwardFilter{ + + private: + + /** all smoothed particles T -> 1*/ + std::vector>> backwardParticles; + + /** all estimations calculated */ + std::vector estimatedStates; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the sampler for drawing trajectories */ + std::unique_ptr> sampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** number of realizations to be calculated */ + int numParticles; + + /** is update called the first time? */ + bool firstFunctionCall; + + /** boundingBox for the boxKDE */ + BoundingBox bb; + + /** histogram/grid holding the particles*/ + Grid2D 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); + firstFunctionCall = true; + + const Point3 maxBB = FloorplanHelper::getBBox(map).getMax(); + const Point3 minBB = FloorplanHelper::getBBox(map).getMin(); + bb = BoundingBox(minBB.x, maxBB.x, minBB.y, maxBB.y); + + // Create histogram + size_t nBinsX = static_cast((maxBB.x - minBB.x) / gridsize_cm); + size_t nBinsY = static_cast((maxBB.y - minBB.y) / gridsize_cm); + grid = Grid2D(bb, nBinsX, nBinsY); + + this->bandwith = bandwith; + } + + /** dtor */ + ~FastKDESmoothing() { + backwardParticles.clear(); + estimatedStates.clear(); + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> 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> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the sampler method to use */ + void setSampler(std::unique_ptr> sampler){ + Assert::isNotNull(sampler, "setSampler() MUST not be called with a nullptr!"); + this->sampler = std::move(sampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return nullptr; + } + + /** + * @brief update + * @param forwardHistory + * @return + */ + State update(ForwardFilterHistory& 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> 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> 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); + return est; + } + + // 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 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 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 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& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda); + + if(weightSumSmoothed != 0.0){ + + for (Particle 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 getEstimations(){ + return estimatedStates; + } + + + }; + +} +#endif // FASTKDESMOOTHING_H diff --git a/smc/smoothing/ForwardFilterHistory.h b/smc/smoothing/ForwardFilterHistory.h index b8841c7..66ef0b5 100644 --- a/smc/smoothing/ForwardFilterHistory.h +++ b/smc/smoothing/ForwardFilterHistory.h @@ -32,13 +32,11 @@ namespace SMC { //empty ctor } - void add(Timestamp time, std::vector>> set, Control control, Observation obs){ + void add(Timestamp time, std::vector> set, Control control, Observation obs){ - // Is empty? Null? etc. - Assert::isNotNull(time, "Timestamp is Null"); - Assert::isNotNull(set, "Particle Set is Null"); - Assert::isNotNull(control, "Control is Null"); - Assert::isNotNull(obs, "Observation is Null"); + // 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); @@ -64,39 +62,39 @@ namespace SMC { * @brief Return the particles from [latestFilterUpdate - @param idx] * @return returns vector of particles. note: c11 makes a std::move here */ - std::vector> getParticleSet(idx = 0){ - return particleSets.at(particleSets.end() - idx); + std::vector> getParticleSet(int idx = 0){ + return particleSets.at(particleSets.size() - 1 - idx); } /** * @brief getControl from [latestFilterUpdate - @param idx] * @return const controls object */ - const Control getControl(idx = 0){ - return controls.at(controls.end() - idx); + 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 (idx = 0){ - return observations.at(observations.end() - idx); + 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 */ - std::vector> getTimestamp(idx = 0){ - return timestamps.at(particleSets.end() - idx); + const Timestamp getTimestamp(int idx = 0){ + return timestamps.at(timestamps.size() - 1 - idx); } /** - * @brief getLatestFilterUpdateNum + * @brief size * @return num of particleSets size */ - const int getLatestFilterUpdateNum(){ + int size(){ return particleSets.size(); } @@ -133,6 +131,14 @@ namespace SMC { return timestamps.back(); } + /** + * @brief getFirstimestamp + * @return const Timestamp object + */ + const Timestamp getFirstTimestamp(){ + return timestamps.front(); + } + }; From 3591106b38e327f835773cfe30e8815c3b10cd86 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 17 Jan 2018 10:27:01 +0100 Subject: [PATCH 4/4] added boxkde von bulli --- math/boxkde/GausLib.cpp | 83 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 math/boxkde/GausLib.cpp diff --git a/math/boxkde/GausLib.cpp b/math/boxkde/GausLib.cpp new file mode 100644 index 0000000..8c4b98f --- /dev/null +++ b/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 + +// 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 bb(boundingBox->minX, boundingBox->maxX, boundingBox->minY, boundingBox->maxY); + + // Create histogram + Grid2D 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 boxGaus; + boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + break; + } + case ALGO_EXBOX: + { + //ExtendedBox boxGaus; + //boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + break; + } + case ALGO_BOX_SIMD: + { + //BoxGausSIMD 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 maxPos; + *out_maxValue = grid.maximum(maxPos); + *out_maxPosX = static_cast(maxPos.X); + *out_maxPosY = static_cast(maxPos.Y); + + if (out_density) + { + memcpy(out_density, grid.image().data().data(), sizeof(float)*grid.image().data().size()); + } + }); + + *out_runTimeInNS = bench.min; +}