From d065015f7d1c430f09efb5c53db77b21840192c3 Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 13 Mar 2017 12:55:31 +0100 Subject: [PATCH] added sampling function for NormalN and approximation of normalN using samples + testcases for both --- main.cpp | 2 +- math/distribution/NormalN.h | 39 +++++++++- .../beacon/model/BeaconModelLogDistCeiling.h | 2 +- tests/math/distribution/TestNormalN.cpp | 77 +++++++++++++++++++ 4 files changed, 114 insertions(+), 6 deletions(-) create mode 100644 tests/math/distribution/TestNormalN.cpp diff --git a/main.cpp b/main.cpp index b103b91..07e9163 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; + ::testing::GTEST_FLAG(filter) = "*NormalN*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index c628828..ad2f04e 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -1,8 +1,14 @@ #ifndef NORMALN_H #define NORMALN_H +#include +#include + #include +#include "../../Assertions.h" +#include "../Random.h" + namespace Distribution { class NormalDistributionN { @@ -15,19 +21,30 @@ namespace Distribution { const double _a; const Eigen::MatrixXd _sigmaInv; + const Eigen::SelfAdjointEigenSolver eigenSolver; + Eigen::MatrixXd transform; //can i make this const? + + RandomGenerator gen; + std::normal_distribution<> dist; + public: /** ctor */ NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : - mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()), eigenSolver(sigma) { + transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal(); } - /** get probability for the given value */ double getProbability(const Eigen::VectorXd val) const { - const double b = ((val-mu).transpose() * _sigmaInv * (val-mu)); - return _a * std::exp(-b/2.0); + const double b = ((val-this->mu).transpose() * this->_sigmaInv * (val-this->mu)); + return this->_a * std::exp(-b/2.0); + } + + /** get a randomly drawn sample from the given normalN distribution*/ + Eigen::VectorXd draw() { + return this->mu + this->transform * Eigen::VectorXd{ this->mu.size() }.unaryExpr([&](double x) { return dist(gen); }); } /** get the mean vector */ @@ -44,6 +61,20 @@ namespace Distribution { return this->_sigmaInv; } + /** return a NormalN based on given data */ + static NormalDistributionN getNormalNFromSamples(const Eigen::MatrixXd& data) { + + const int numElements = data.rows(); + Assert::notEqual(numElements, 1, "data is just 1 value, thats not enough for getting the distribution!"); + Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!"); + + const Eigen::VectorXd mean = data.colwise().mean(); + const Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); + const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); + + return NormalDistributionN(mean, cov); + } + }; } diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h index 8e794a6..649d19e 100644 --- a/sensors/beacon/model/BeaconModelLogDistCeiling.h +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -94,7 +94,7 @@ public: Assert::isBetween(params.txp, -90.0f, -30.0f, "TXP out of bounds [-50:-30]"); Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]"); - Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); + //Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); // add beacons.insert( std::pair(beacon, params) ); diff --git a/tests/math/distribution/TestNormalN.cpp b/tests/math/distribution/TestNormalN.cpp new file mode 100644 index 0000000..39f79e6 --- /dev/null +++ b/tests/math/distribution/TestNormalN.cpp @@ -0,0 +1,77 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/KullbackLeibler.h" +#include "../../../math/Distributions.h" + +#include + +#include +#include +#include +#include + + +TEST(NormalN, multivariateGaussSampleData) { + + Eigen::VectorXd mu(2); + mu[0] = 0.0; + mu[1] = 0.0; + + Eigen::MatrixXd cov(2,2); + cov(0,0) = 1.0; + cov(0,1) = 0.0; + cov(1,0) = 0.0; + cov(1,1) = 1.0; + + Distribution::NormalDistributionN normTest(mu, cov); + + // draw + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementPoints pParticles; + + gp << "set terminal qt size 1200,1200 \n"; + gp << "set xrange [-5:5] \n set yrange [-5:5] \n"; + + // gen + std::vector vals; + for(int i = 0; i < 100000; ++i){ + Eigen::VectorXd vec = normTest.draw(); + vals.push_back(vec); + + K::GnuplotPoint2 pos(vec[0], vec[1]); + pParticles.add(pos); + } + + plot.add(&pParticles); + + gp.draw(plot); + gp.flush(); + + sleep(5); + + //create matrix out of the vector + Eigen::MatrixXd m(vals.size(), vals[0].rows()); + + for(int i = 0; i < vals.size(); ++i){ + m(i,0) = vals[i][0]; + m(i,1) = vals[i][1]; + } + + Distribution::NormalDistributionN norm = Distribution::NormalDistributionN::getNormalNFromSamples(m); + + //get distance between original and approximated mu and sigma + Eigen::MatrixXd diffM = cov - norm.getSigma(); + double distM = diffM.norm(); + + Eigen::VectorXd diffV = mu - norm.getMu(); + double distV = diffV.norm(); + + ASSERT_NEAR(0.0, distM, 0.01); + ASSERT_NEAR(0.0, distV, 0.01); + +} + +#endif +