added sampling function for NormalN and approximation of normalN using samples + testcases for both

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*";
 

math/distribution/NormalN.h

@@ -1,8 +1,14 @@
 #ifndef NORMALN_H
 #define NORMALN_H
 
+#include <cmath>
+#include <random>
+
 #include <eigen3/Eigen/Dense>
 
+#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<Eigen::MatrixXd> 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);
+        }
+
     };
 
 }

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<MACAddress, APEntry>(beacon, params) );

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 <random>
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+
+
+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<Eigen::VectorXd> 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
+