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

CMakeLists.txt

@@ -19,9 +19,9 @@ ENDIF()
 
 INCLUDE_DIRECTORIES(
 	../
-	../../
+        ../../
-	../../../
+        ../../../
-	../../../../
+        ../../../../
 )
 
 
@@ -42,7 +42,7 @@ FILE(GLOB SOURCES
 	./*/*/*.cpp
 	./*/*/*/*.cpp
 	../../Indoor/lib/tinyxml/tinyxml2.cpp
-	../../Indoor/lib/Recast/*.cpp
+        ../../Indoor/lib/Recast/*.cpp
 )
 
 
@@ -71,8 +71,9 @@ else()
 		-fstack-protector-all
 
 		-g3
-			#-O2
+                -O2
 		-march=native
+
 	)	
 endif()
 
@@ -102,6 +103,7 @@ ADD_EXECUTABLE(
 # needed external libraries
 TARGET_LINK_LIBRARIES(
 	${PROJECT_NAME}
+        stdc++fs
 #	gtest
 #	pthread
 )

Plotty.h

@@ -167,6 +167,8 @@ public:
 		cpoints.setPointSize(2);
 		cpoints.setPointType(7);
 
+        splot.getView().setEnabled(false);
+
 	}
 
 	void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {
@@ -313,6 +315,22 @@ public:
 		splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
 	}
 
+    void setActivity(const int act) {
+
+        std::string activity = "Unkown";
+        if(act == 0){
+            activity = "Standing";
+        } else if(act == 1) {
+            activity = "Walking";
+        } else if(act == 2) {
+            activity = "Up";
+        } else if(act == 3) {
+            activity = "Down";
+        }
+
+        gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\n";
+    }
+
 	void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) {
 		std::vector<Point3> points = {
 			Point3(p1.x, p1.y, p1.z),
@@ -512,10 +530,17 @@ public:
         gp.draw(splot);
         stream << "set terminal x11 size 2000,1500\n";
         stream << gp.getBuffer();
-        stream << "pause -1\n";
+        stream << "pause -1\n"; 
         gp.flush();
     }
 
+    void printOverview(const std::string& path) {
+        gp << "set terminal png size 2000,1500\n";
+        gp << "set output '" << path << "_overview" << ".png'\n";
+        gp << "set view 75,60\n";
+        gp << "set autoscale xy\n";
+        gp << "set autoscale z\n";
+    }
 
 	void buildFloorplan() {
 

Settings.h

@@ -62,13 +62,14 @@ namespace Settings {
         constexpr float WAF = -11.0;
 
         const bool optimize = true;
+		const bool useRegionalOpt = true;
 
         // how to perform VAP grouping. see
         // - calibration in Controller.cpp
         // - eval in Filter.h
         // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
-        const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1);
+		const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1);	// Frank: WAS MAXIMUM
-        const VAPGrouper vg_eval  = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1);
+		const VAPGrouper vg_eval  = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1);	// Frank: WAS MAXIMUM
 	}
 
     namespace BeaconModel {
@@ -89,9 +90,13 @@ namespace Settings {
 		constexpr bool useMainThread = false;						// perform filtering in the main thread
 	}
 
-    const std::string mapDir = "../map/";
+//    const std::string mapDir = "../map/";
-    const std::string dataDir = "../measurements/";
+//    const std::string dataDir = "../measurements/";
-    const std::string errorDir = dataDir  + "results/";
+//    const std::string errorDir = dataDir  + "results/";
+
+	const std::string mapDir = "/apps/museum/maps/";
+	const std::string dataDir = "/apps/";
+	const std::string errorDir = dataDir  + "museum/results/";
 
     /** describes one dataset (map, training, parameter-estimation, ...) */
     struct DataSetup {
@@ -171,7 +176,7 @@ namespace Settings {
                 dataDir + "museum/Nexus/Path3_4519.csv",
                 dataDir + "museum/Nexus/Path3_5929.csv",
                 dataDir + "museum/Pixel/Path3_6307.csv",
-                dataDir + "museum/Pixel/Path3_5451.csv",
+                //dataDir + "museum/Pixel/Path3_5451.csv", //geht nur bis gt-punkt 31
                 dataDir + "museum/Pixel/Path3_9243.csv",
                 //dataDir + "museum/Samsung/Path3_7610.csv",
                 //dataDir + "museum/Samsung/Path3_7819.csv"

main.cpp

@@ -20,11 +20,16 @@
 #include <Indoor/sensors/imu/MotionDetection.h>
 #include <Indoor/sensors/pressure/RelativePressure.h>
 #include <Indoor/data/Timestamp.h>
+#include <Indoor/sensors/radio/model/WiFiModels.h>
 #include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
+#include <Indoor/sensors/radio/setup/WiFiOptimizerPerFloor.h>
+
+
 #include <Indoor/math/stats/Statistics.h>
 
 #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
 
+#include <sys/stat.h>
 
 Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
 
@@ -54,32 +59,67 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
 
 
     // wifi
-    WiFiModelLogDistCeiling WiFiModel(map);
+//    WiFiModelLogDistCeiling WiFiModel(map);
+//	WiFiModelPerFloor WiFiModelPerFloor(map);
+//	WiFiModelPerBBox WiFiModelPerBBox(map);
+
+	WiFiModel* WiFiModel = nullptr;
 
     // with optimization
     if(Settings::WiFiModel::optimize){
 
         if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
-            Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
-            WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
-            for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
-                opt.addFingerprint(fp);
-            }
-            const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
-            for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
-                const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
-                WiFiModel.addAP(ap.mac, entry);
-            }
 
-            WiFiModel.saveXML(setup.wifiModel);
+            Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
+
+			if (Settings::WiFiModel::useRegionalOpt) {
+
+				// use a regional optimization scheme (one per floor)
+
+				WiFiOptimizerPerFloor opt(map);
+
+				// add all fingerprints to the optimizer (optimizer will add them to the correct floor/model)
+				for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
+					opt.addFingerprint(fp);
+				}
+
+				WiFiModel = opt.optimizeAll();
+
+				WiFiModel->saveXML(setup.wifiModel);
+
+			} else {
+
+				// use one model per AP for the whole map
+
+				WiFiModel = new WiFiModelLogDistCeiling(map);
+				WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
+				for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
+					opt.addFingerprint(fp);
+				}
+				const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
+				for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
+					const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
+					((WiFiModelLogDistCeiling*)WiFiModel)->addAP(ap.mac, entry);
+				}
+
+				WiFiModel->saveXML(setup.wifiModel);
+
+			}
+
         } else {
-            WiFiModel.loadXML(setup.wifiModel);
+
+			// load WiFiModel from file. The factory will create the correct instance
+			//WiFiModel->loadXML(setup.wifiModel);
+			WiFiModelFactory fac(map);
+			WiFiModel = fac.loadXML(setup.wifiModel);
+
         }
 
     } else {
         // without optimization
-        WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
+		WiFiModel = new WiFiModelLogDistCeiling(map);
-        Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
+		((WiFiModelLogDistCeiling*)WiFiModel)->loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
+		Assert::isFalse(WiFiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml");
     }
 
 
@@ -92,8 +132,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
 
     const Point3 srcPath0(26, 43, 7.5);
     const Point3 srcPath1(62, 38, 1.7);
-    //const Point3 srcPath2(62, 38, 1.8);
+    const Point3 srcPath2(62, 38, 1.8);
-    //const Point3 srcPath3(62, 38, 1.8);
+    const Point3 srcPath3(62, 38, 1.8);
 
     // add shortest-path to destination
     //const Point3 dst(51, 45, 1.7);
@@ -114,13 +154,14 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
 
     // particle-filter
     const int numParticles = 5000;
-    auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1);	// known position
+    //auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1);	// known position
-    //auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
+    auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
-    auto eval = std::make_unique<MyPFEval>(WiFiModel);
+	auto eval = std::make_unique<MyPFEval>(*WiFiModel);
-    auto trans = std::make_unique<MyPFTrans>(mesh);
+	auto trans = std::make_unique<MyPFTrans>(mesh, *WiFiModel);
 
     //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
-    auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
+    //auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
+    auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
 
     //auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, Settings::KDE::gridSize, Settings::KDE::bandwidth);
     //auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
@@ -154,6 +195,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
 
         if (e.type == Offline::Sensor::WIFI) {
             obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
+            ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
 
         } else if (e.type == Offline::Sensor::ACC) {
             if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
@@ -184,6 +226,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
         if (ctrl.numStepsSinceLastEval > 0) {
 
             obs.currentTime = ts;
+            ctrl.currentTime = ts;
+
 //            if(ctrl.numStepsSinceLastEval > 0){
 //                pf.updateTransitionOnly(&ctrl);
 //            }
@@ -192,7 +236,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
             Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
             lastTimestamp = ts;
 
-
+            ctrl.lastEstimate = est.pos.pos;
 
             //plot
             //dbg.showParticles(pf.getParticles());
@@ -207,22 +251,17 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
             plot.setCurEst(est.pos.pos);
             plot.setGroundTruth(gtPos);
             plot.addEstimationNode(est.pos.pos);
-            plot.plot();
+            plot.setActivity((int) act.get());
+
+			plot.plot();
 
             // error calc
             float err_m = gtPos.getDistance(est.pos.pos);
             errorStats.add(err_m);
-            errorFile << err_m << "\n";
+            errorFile << ts.ms() << " " << err_m << "\n";
 
             //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
             //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
-
-            if(ts.ms() == 13410 || ts.ms() == 20802){
-                std::ofstream plotFile;
-                plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp");
-                plot.saveToFile(plotFile);
-                plotFile.close();
-            }
         }
     }
 
@@ -241,14 +280,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
     errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
     errorFile.close();
 
-    //save the .gp buffer into a file
+    /* plot in gp file */
-//    std::ofstream plotFile;
+    std::ofstream plotFile;
-//    plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
+    plotFile.open(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
-//    dbg.saveToFile(plotFile);
+    plot.saveToFile(plotFile);
-//    plotFile.close();
+    plotFile.close();
 
     //save also a png image, just for a better overview
-//    dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
+    plot.printOverview(evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t));
+    plot.plot();
 
     return errorStats;
 }
@@ -261,13 +301,38 @@ int main(int argc, char** argv) {
     Stats::Statistics<float> statsQuantil;
     Stats::Statistics<float> tmp;
 
-    Settings::DataSetup set = Settings::data.Path1;
+    std::string evaluationName = "museum/tmp";
-    std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
 
     for(int i = 0; i < 1; ++i){
 
-        for(int j = 0; j < 1; ++j){
+        //TODO: in transition die distance über KLD noch einkommentieren als Test
-            tmp = run(set, j, evaluationName);
+
+//        for(int j = 0; j < Settings::data.Path0.training.size(); ++j){
+//            tmp = run(Settings::data.Path0, j, evaluationName);
+//            statsMedian.add(tmp.getMedian());
+//            statsAVG.add(tmp.getAvg());
+//            statsSTD.add(tmp.getStdDev());
+//            statsQuantil.add(tmp.getQuantile(0.75));
+//        }
+
+//        for(int j = 0; j < Settings::data.Path1.training.size(); ++j){
+//            tmp = run(Settings::data.Path1, j, evaluationName);
+//            statsMedian.add(tmp.getMedian());
+//            statsAVG.add(tmp.getAvg());
+//            statsSTD.add(tmp.getStdDev());
+//            statsQuantil.add(tmp.getQuantile(0.75));
+//        }
+
+//        for(int j = 0; j < Settings::data.Path2.training.size(); ++j){
+//            tmp = run(Settings::data.Path2, j, evaluationName);
+//            statsMedian.add(tmp.getMedian());
+//            statsAVG.add(tmp.getAvg());
+//            statsSTD.add(tmp.getStdDev());
+//            statsQuantil.add(tmp.getQuantile(0.75));
+//        }
+
+        for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
+            tmp = run(Settings::data.Path3, j, evaluationName);
             statsMedian.add(tmp.getMedian());
             statsAVG.add(tmp.getAvg());
             statsSTD.add(tmp.getStdDev());
@@ -299,6 +364,6 @@ int main(int argc, char** argv) {
 
     finalStatisticFile.close();
 
-    std::this_thread::sleep_for(std::chrono::seconds(60));
+    //std::this_thread::sleep_for(std::chrono::seconds(60));
 
 }

main.h

@@ -0,0 +1,6 @@
+#ifndef NAV_MESH_MAIN_H
+#define NAV_MESH_MAIN_H
+
+
+
+#endif

navMesh/filter.h

@@ -17,13 +17,23 @@
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h>
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
 #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
+
 #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
 #include <Indoor/navMesh/walk/NavMeshWalkEval.h>
+#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
+#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
+#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
+#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
+#include <Indoor/navMesh/NavMeshRandom.h>
+
 #include <Indoor/sensors/radio/WiFiMeasurements.h>
 #include <Indoor/data/Timestamp.h>
 #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
 #include <Indoor/sensors/activity/ActivityDetector.h>
 
+#include <Indoor/math/divergence/KullbackLeibler.h>
+#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
+
 struct MyState {
 
 	/** the state's position (within the mesh) */
@@ -89,6 +99,14 @@ struct MyControl {
 		headingChangeSinceLastEval = 0;
 	}
 
+    //wifi
+    WiFiMeasurements wifi;
+
+    //time
+    Timestamp currentTime;
+
+    //last estimation
+    Point3 lastEstimate = Point3(26, 43, 7.5);
 
 };
 
@@ -160,26 +178,75 @@ public:
 
 class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
 
-	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+	//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+    //using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
+    //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
+	using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
 	MyNavMeshWalk walker;
 
+    WiFiQualityAnalyzer analyzer;
+    WiFiObserverFree wifiProbability;
+    const double lambda =  0.0003;
+
+    SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
+
 public:
 
-	MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
+    MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
+        walker(mesh),
+        wifiProbability(Settings::WiFiModel::sigma, wifiModel){
 
 		// how to evaluate drawn points
-        walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
+        //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
-        walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
+        //walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
         //walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target
 
-	}
+        estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
+
+    }
 
     void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
 
+        // walking and heading random
         Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
 		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
         Distribution::Normal<float> dHeading(0.0, 0.1);
 
+        // wifi and quality of wifi
+        const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
+        if(!wifiObs.entries.empty()){
+            analyzer.add(wifiObs);
+        }
+        float qualityWifi = analyzer.getQuality();
+        if(std::isnan(qualityWifi)){
+            qualityWifi = 1.0;
+        } else if(qualityWifi == 0) {
+            qualityWifi = 0.00000001;
+        }
+
+        // divergence between eval and transition
+		std::vector<SMC::Particle<MyState>> wifiParticles;
+		NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
+		for(int i = 0; i < 10000; ++i){
+
+			NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
+			double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
+			SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
+			wifiParticles.push_back(tmpParticle);
+		}
+
+		MyState wifiEstimate = estimator.estimate(wifiParticles);
+
+		// fake kld
+		const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
+        //const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
+
+        //std::cout << "KLD: " << kld << std::endl;
+        //std::cout << "Quality: " << qualityWifi << std::endl;
+
+        //update wifi
+        //walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
+
         #pragma omp parallel for num_threads(3)
         for (int i = 0; i < particles.size(); ++i) {
             SMC::Particle<MyState>& p = particles[i];
@@ -198,8 +265,11 @@ public:
                 params.stepSizes.stepSizeStair_m  = 0.1;
             }
 
+            double deltaUnblockable = 0.01;
+
 			// walk
-            MyNavMeshWalk::ResultEntry res = walker.getOne(params);
+			//MyNavMeshWalk::ResultEntry res = walker.getOne(params);
+			MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
 
 			// assign back to particle's state
             p.weight *= res.probability;
@@ -246,7 +316,10 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
 
 public:
 
-    MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
+	// FRANK
+	//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
+	//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
+	MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
 
     virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
 
@@ -254,8 +327,8 @@ public:
         const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
 
 
-        #pragma omp parallel for num_threads(3)
+		#pragma omp parallel for num_threads(3)
-        for (int i = 0; i < particles.size(); ++i) {
+		for (size_t i = 0; i < particles.size(); ++i) {
             SMC::Particle<MyState>& p = particles[i];
 
             double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
@@ -263,9 +336,7 @@ public:
 
             //HACK HACK HACK HACK
             double prob = 1.0;
-            if(observation.currentTime.ms() > 20801){
+            prob = pWifi * pStair;
-                prob = pWifi * pStair;
-            }
 
             p.weight *= prob;
 

navMesh/mesh.h

@@ -18,7 +18,6 @@ class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangle
 public:
 
 	MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) {
-		;
 	}
 
 };