parameter for normal distirbuation approximation are okay

code/Settings.h

@@ -14,16 +14,16 @@ namespace Settings {
     const int numParticles = 5000;
 
     namespace Mode1 {
-        const double modeProbability = 0.5;
+        const double modeProbability = 0.99;
     }
 
     namespace Mode2 {
-        const double modeProbability = 0.5;
+        const double modeProbability = 0.01;
     }
 
     namespace Mixing {
         //Eigen::Matrix2d transitionProbabilityMatrix(1,0,0,1);
-        const double lambda = 0.01;
+        const double lambda = 0.03;
     }
 
 	namespace IMU {

code/filter/KLB.h

@@ -23,6 +23,7 @@
 #include <Indoor/sensors/imu/MotionDetection.h>
 #include <Indoor/sensors/pressure/RelativePressure.h>
 #include <Indoor/sensors/radio/WiFiGridEstimator.h>
+#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
 #include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
 
 #include <Indoor/math/MovingAVG.h>
@@ -55,6 +56,7 @@
 #include "../Settings.h"
 
 double __KLD = 0.0;
+double __QUALITY = 0.0;
 
 //todo function return the transition prob matrix for markov chain!
 //getKernelDensityProbability should work fine for first shot! nevertheless we need to do 2 kernel density estimations for both filters :( :( :(
@@ -67,7 +69,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
 
     ModeProbabilityTransition(Grid<MyNode>& grid, double lambda) : grid(grid), lambda(lambda) {;}
 
-    virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
+    virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
 
         std::vector<double> probsWifiV;
         std::vector<double> probsParticleV;
@@ -121,6 +123,7 @@ struct ModeProbabilityTransition : public K::MarkovTransitionProbability<MyState
 struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<MyState, MyControl, MyObs>{
 
     const double lambda;
+    WiFiQualityAnalyzer analyzer;
 
     //this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
     //no inverse matrix is possible
@@ -129,7 +132,7 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
     /** ctor */
     ModeProbabilityTransitionNormal(double lambda) : lambda(lambda) {;}
 
-    virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes) override {
+    virtual Eigen::MatrixXd update(std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>>& modes, const MyObs& obs) override {
 
         Assert::equal(modes[0].getParticles().size(), modes[1].getParticles().size(), "Particle.size() differs!");
 
@@ -159,25 +162,37 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
         meanWifi << estWifi.x, estWifi.y, estWifi.z;
         Distribution::NormalDistributionN normWifi = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(mWifi, meanWifi);
 
-        // get kld
+        //calc wi-fi metrik
-        double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
+        const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
-
+        if(!wifiObs.entries.empty()){
-        if(kld > 20){
+            analyzer.add(wifiObs);
-            std::cout << "STTTTTOOOOOOP" << std::endl;
+        }
+        float qualityWifi = analyzer.getQuality();
+        if(std::isnan(qualityWifi)){
+            qualityWifi = 1.0;
+        } else if(qualityWifi == 0) {
+            qualityWifi = 0.00000001;
         }
 
+        // debugging global variable
+        __QUALITY = qualityWifi;
+
+        // get kld
+        double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
 
         // debugging global variable
         __KLD = kld;
 
         //exp. distribution
-        double expKld = std::exp(-lambda * kld);
+        double expKld = std::exp(-lambda * (kld * qualityWifi));
 
         Assert::isTrue(expKld < 1.0, "exp. distribution greater 1!");
 
+
+
         //create the matrix
         Eigen::MatrixXd m(2,2);
-        m << expKld, 1- expKld, 0, 1;
+        m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
 
         return m;
 

code/filter/Logic.h

@@ -100,9 +100,10 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
     Grid<MyNode>& grid;
     GridPoint startPos;
     float headingDeg;
+    int mode;
 
-    PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
+    PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg, int mode) :
-        grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
+        grid(grid), startPos(startPos), headingDeg(headingDeg), mode(mode) {;}
 
     virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
 
@@ -118,6 +119,9 @@ struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {
             p.state.heading.error = 0;
             p.state.relativePressure = 0;                               // start with a relative pressure of 0
             p.weight = 1.0 / particles.size();						// equal weight
+
+            //for debugging
+            p.state.curMode = mode;
         }
     }
 
@@ -130,7 +134,7 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
 
     // define the noise
     Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
-    Distribution::Normal<float> height = Distribution::Normal<float>(0.0, 600.0);
+    Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
 
     // draw randomly from a vector
     random_selector<> rand;
@@ -149,55 +153,35 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
         for (int i = 0; i < Settings::numParticles; ++i) {
             K::Particle<MyState>& p = particles[i];
 
-//            // if neighboring node is a staircase, we have a 0.8 chance to walk them.
+            double diffHeight = p.state.position.inMeter().z + height_m.draw();
-//            GridPoint tmp = grid.getNodeFor(p.state.position);
+            double newHeight_cm = p.state.position.z_cm;
-//            MyNode tmpNode(tmp);
+            if(diffHeight > 9.1){
-//            int numNeigbors = grid.getNumNeighbors(tmpNode);
+                newHeight_cm = 10.8 * 100.0;
+            } else if (diffHeight < 9.1 && diffHeight > 5.7){
+                newHeight_cm = 7.4 * 100.0;
+            } else if (diffHeight < 5.7 && diffHeight > 2.0) {
+                newHeight_cm = 4.0 * 100.0;
+            } else {
+                newHeight_cm = 0.0;
+            }
 
-//            std::vector<MyNode> zNodes;
+            GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
-//            for(int i = 0; i < numNeigbors; ++i){
-
-//                //if neighbor is stair (1) or elevator (2)
-//                MyNode curNode = grid.getNeighbor(tmpNode, i);
-//                if(curNode.getType() == 1 || curNode.getType() == 2){
-//                    zNodes.push_back(curNode);
-//                }
-//            }
-
-//            float height = 0.0;
-//            if(!zNodes.empty()){
-
-//                if(uniRand.draw() > 0.3){
-//                    //get a random height from all the neighbors on stairs or elevators
-//                    height = rand(zNodes).z_cm - p.state.position.z_cm;
-//                }else{
-//                    //do nothin
-//                }
-
-//            }
-
-
-            double diffHeight = p.state.position.z_cm + height.draw();
-
-            if()
-
-
-            GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), height.draw());
             GridPoint newPosition = p.state.position + noisePt;
+            newPosition.z_cm = newHeight_cm;
 
-            p.state.position = grid.getNearestNode(newPosition);
+//            p.state.position = grid.getNearestNode(newPosition);
 
-//            if(grid.hasNodeFor(newPosition)){
+            if(grid.hasNodeFor(newPosition)){
-//                p.state.position = newPosition;
+                p.state.position = newPosition;
-//            }else{
+            }else{
-//                //no new position!
+                //no new position!
-//                #pragma omp atomic
+                #pragma omp atomic
-//                noNewPositionCounter++;
+                noNewPositionCounter++;
-//            }
+            }
 
         }
 
-//        std::cout << noNewPositionCounter << std::endl;
+        std::cout << noNewPositionCounter << std::endl;
     }
 };
 
@@ -339,7 +323,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
             // Point3 posOld_m = p.state.positionOld.inMeter();
 
             const double pWifi = getWIFI(observation, wifiObs, p.state.position);
-            //const double pBaroPressure = getStairProb(p, observation.activity);
+            const double pBaroPressure = getStairProb(p, observation.activity);
             //const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
             //const double pBeacon = getBEACON(observation, p.state.position);
 
@@ -347,7 +331,7 @@ struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
             _assertNotNAN(pWifi, "Wifi prob is nan");
             //_assertNot0(pBaroPressure,"pBaroPressure is null");
 
-            const double prob = pWifi;
+            const double prob = pWifi * pBaroPressure;
 
             p.weight = prob;
 

code/main.cpp

@@ -107,6 +107,7 @@ Floorplan::IndoorMap* MyState::map;
 void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
 
     std::vector<double> kld_data;
+    std::vector<double> quality_data;
 
     // load the floorplan
     Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
@@ -169,7 +170,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     //std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
 
     // mode 1
-    std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
+    //std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
+    std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1));
+
     K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability);
     mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
     mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -180,7 +183,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     modes.push_back(mode1);
 
     // mode 2
-    std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
+    //std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
+    std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2));
     K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability);
     mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
     mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -281,6 +285,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             } else{
                 kld_data.push_back(__KLD);
             }
+            quality_data.push_back(__QUALITY);
 
             Point3 estPos = est.position.inMeter();
 
@@ -313,6 +318,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
             plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n";
             plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" <<  obs.activity << "'\n";
 
+            plot.gp << "set label 1011 at screen 0.90, 0.10 'Wifi Quality:" <<  __QUALITY << "'\n";
             plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" <<  IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n";
             plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" <<  IMMAPF.getModes()[1].getModePosteriorProbability()  << "'\n";
 
@@ -390,7 +396,7 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     //save as screenshot for klb
     std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
     gp << "set terminal png size 1280,720\n";
-    gp << "set output '" << path << "_shennendistance.png'\n";
+    gp << "set output '" << path << "klddistance.png'\n";
 
     for(int i=0; i < kld_data.size()-1; ++i){
 
@@ -404,6 +410,28 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
     gp.draw(plotkld);
     gp.flush();
 
+    //draw wifi quality
+    K::Gnuplot gpQuality;
+    K::GnuplotPlot plotquality;
+    K::GnuplotPlotElementLines linesQuality;
+
+    //save as screenshot for wifiquality
+    std::string pathWifi = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
+    gpQuality << "set terminal png size 1280,720\n";
+    gpQuality << "set output '" << pathWifi << "wifiquality.png'\n";
+
+    for(int i=0; i < quality_data.size()-1; ++i){
+
+        K::GnuplotPoint2 p1(i, quality_data[i]);
+        K::GnuplotPoint2 p2(i+1, quality_data[i+1]);
+
+        linesQuality.addSegment(p1, p2);
+    }
+
+    plotquality.add(&linesQuality);
+    gpQuality.draw(plotquality);
+    gpQuality.flush();
+
     std::cout << "finished" << std::endl;
     sleep(1);
 
@@ -424,10 +452,12 @@ int main(int argc, char** argv) {
 //        //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
 
         //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
-        run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
+
-        run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
-        //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
         run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
+        //run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
+        //run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
+        //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
+
         //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
     //}