added tex template

code/eval/FixedLagEvalBase.h

@@ -19,6 +19,7 @@
 #include "../particles/MyInitializer.h"
 #include "../particles/smoothing/MySmoothingTransition.h"
 #include "../particles/smoothing/MySmoothingTransitionSimple.h"
+#include "../particles/smoothing/MySmoothingTransitionExperimental.h"
 
 #include "../reader/SensorReader.h"
 #include "../reader/SensorReaderStep.h"
@@ -68,6 +69,8 @@ protected:
 	std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
 	std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};	// duplicate 1st waypoint!
 
+    float stepSize = 0.71;
+
 public:
 
     FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
@@ -84,7 +87,7 @@ public:
 		vis.floors.setColorHex("#666666");
 		vis.groundTruth.setCustomAttr("dashtype 3");
 		vis.groundTruth.setColorHex("#009900");
-		vis.gp << "unset cbrange\n";
+        vis.gp << "unset cbrange\n";
 
 
 	}
@@ -236,7 +239,7 @@ public:
             while (!step_observations.empty() && current_time > step_observations.front().ts) {
                 const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
                 obs.step->steps++;
-                ctrl.walked_m = obs.step->steps * 0.71;
+                ctrl.walked_m = obs.step->steps * stepSize;
             }
 
             // process all occurred steps
@@ -273,12 +276,14 @@ public:
 
                 // skip the first 24 scans due to uniform distribution start
                 if (++cnt > 24) {
-                statsFiltering.add(err);
-                std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
+                    statsFiltering.add(err);
+                    std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
+                }
 
-                //save the current estimation for later smoothing.
-                pfHistory.push_back(pf->getNonResamplingParticles());
-                tsHistory.push_back(se.ts);
+                if(cnt > 19){
+                    //save the current estimation for later smoothing.
+                    pfHistory.push_back(pf->getNonResamplingParticles());
+                    tsHistory.push_back(se.ts);
                 }
 
 
@@ -291,7 +296,7 @@ public:
                     //use every n-th (std = 1) particle set of the history within a given lag (std = 5)
                     for(int i = 0; i < MiscSettings::lag; i += MiscSettings::fixedLagGap){
 
-                        ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
+                        ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
                         estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);
 
                     }
@@ -309,18 +314,20 @@ public:
                     statsSmoothing.add(errSmoothed);
                     std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl;
 
-
-                    // plot
+                    //plot
                     vis.clearStates();
                     for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
                         const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
                         vis.addState(p.state.walkState);
                     }
-                    vis.setTimestamp(se.ts);
+                    //vis.setTimestamp(se.ts);
+                    vis.setFilteringMedian(statsFiltering.getMedian());
+                    vis.setSmoothingMedian(statsSmoothing.getMedian());
                     vis.addGroundTruth(gtw);
                     vis.addEstPath(pathEst);
                     vis.addSmoothPath(smoothedEst);
-                    vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
+                    vis.setEstAndShould(curEst, curGT);
+                    vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
 
                     if (obs.barometer != nullptr) {
                         vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
@@ -343,6 +350,42 @@ public:
 
         }
         statsout.close();
+
+        // plot
+        //vis.clearStates();
+        vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
+        vis.gp.setOutput("/tmp/" + runName + ".png");
+        vis.gp << "set view 60," << 40 << "\n"; //For fixed position
+
+        for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
+            const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
+            vis.addState(p.state.walkState);
+        }
+        //vis.setTimestamp(se.ts);
+        vis.setFilteringMedian(statsFiltering.getMedian());
+        vis.setSmoothingMedian(statsSmoothing.getMedian());
+        vis.addGroundTruth(gtw);
+        vis.addEstPath(pathEst);
+        vis.addSmoothPath(smoothedEst);
+        //vis.setEstAndShould(curEst, curGT);
+        //vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
+
+        if (obs.barometer != nullptr) {
+            vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
+        }
+        vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
+
+        //vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
+
+        Point2 p1(0.1, 0.1);
+        Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
+        //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
+        vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
+
+        vis.show();
+
+        // prevent gnuplot errors
+        usleep(1000*33);
     }
 
 };

code/eval/SmoothingEval1.h

@@ -21,7 +21,7 @@
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
 
-class SmoothingEval1 : public SmoothingEvalBase {
+class SmoothingEval1 : public FixedLagEvalBase {
 
 public:
 
@@ -59,17 +59,17 @@ public:
 
     void fixedIntervallSimpleTransPath1() {
 
-        runName = "fixedIntervallSimpleTrans";
+        runName = "fixedIntervallSimpleTransPath1";
         bool smoothing_resample = false;
         smoothing_time_delay = 1;
 
-        BarometerEvaluation::barometerSigma = 0.05;
+        BarometerEvaluation::barometerSigma = 0.10;
         sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward
         srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt");
         srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt");
 		gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
 
-        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
         GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
         pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
 
@@ -83,27 +83,37 @@ public:
 
     void fixedIntervallSimpleTransPath4(){
 
-        runName = "fixedIntervallSimpleTrans";
-        bool smoothing_resample = false;
-        smoothing_time_delay = 1;
+        runName = "fixedIntervallSimpleTransPath4";
 
-        BarometerEvaluation::barometerSigma = 0.05;
-        sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
-        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
-        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/Steps2.txt");
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
         gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
 
         MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
         GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
         pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
 
+
+        //Smoothing Variables
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 15.0;
+        smoothing_baro_sigma = 0.05;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
         //Smoothing using Simple Trans
         bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
         if(smoothing_resample)
             bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
-        bf->setTransition(std::unique_ptr<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
     }
 
+    // ============================================================ Dijkstra ============================================== //
+
     // Hier dauert sehr laaaaaaaannnnnnnggge @Frank
     void fixedIntervallDijkstraTransPath4(){
 
@@ -129,6 +139,493 @@ public:
     }
 
 
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+    /* ----------------------------------------------- Bergwerk ------ Fusion 2016 --------------------------------------------------- */
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path1_nexus() {
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
+
+        //Smoothing Variables
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 15.0;
+        smoothing_baro_sigma = 0.05;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path1_nexus_simple() {
+        runName = "bergwerk_path1_nexus_simple_fixedlag";
+        bergwerk_path1_nexus();
+
+        for (auto& n : grid) {n.imp = 1;}
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+
+    void bergwerk_path1_nexus_imp() {
+
+        runName = "bergwerk_path1_nexus_importance_fixedlag";
+        bergwerk_path1_nexus();
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path1_nexus_multi() {
+
+        runName = "bergwerk_path1_nexus_multi_fixedlag";
+        bergwerk_path1_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path1_nexus_shortest() {
+
+        runName = "bergwerk_path1_nexus_shortest_fixedlag";
+        bergwerk_path1_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path2_nexus() {
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path2/nexus/vor/1454779863041.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path2/nexus/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path2/nexus/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.05;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path2_nexus_simple() {
+
+        runName = "bergwerk_path2_nexus_simple_fixedlag";
+        bergwerk_path2_nexus();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+
+    void bergwerk_path2_nexus_imp() {
+
+        runName = "bergwerk_path2_nexus_importance_fixedlag";
+        bergwerk_path2_nexus();
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path2_nexus_multi() {
+
+        runName = "bergwerk_path2_nexus_multi_fixedlag";
+        bergwerk_path2_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path2_nexus_shortest() {
+
+        runName = "bergwerk_path2_nexus_shortest_fixedlag";
+        bergwerk_path2_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        walk->times = 4;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path3_nexus() {
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path3/nexus/vor/1454782562231.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path3/nexus/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path3/nexus/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.05;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path3_nexus_simple() {
+
+        runName = "bergwerk_path3_nexus_simple_fixedlag";
+        bergwerk_path3_nexus();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+
+    void bergwerk_path3_nexus_imp() {
+
+        runName = "bergwerk_path3_nexus_importance_fixedlag";
+        bergwerk_path3_nexus();
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path3_nexus_multi() {
+
+        runName = "bergwerk_path3_nexus_multi_fixedlag";
+        bergwerk_path3_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path3_nexus_shortest() {
+
+        runName = "bergwerk_path3_nexus_shortest_fixedlag";
+        bergwerk_path3_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path4_nexus() {
+        BarometerEvaluation::barometerSigma = 0.10;
+        sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.05;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path4_nexus_simple() {
+
+        runName = "bergwerk_path4_nexus_simple_fixedlag";
+        bergwerk_path4_nexus();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+
+    void bergwerk_path4_nexus_imp() {
+
+        runName = "bergwerk_path4_nexus_importance_fixedlag";
+        bergwerk_path4_nexus();
+
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path4_nexus_multi() {
+
+        runName = "bergwerk_path4_nexus_multi_fixedlag";
+        bergwerk_path4_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    void bergwerk_path4_nexus_shortest() {
+
+        runName = "bergwerk_path4_nexus_shortest_fixedlag";
+        bergwerk_path4_nexus();
+
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path1_galaxy() {
+        BarometerEvaluation::barometerSigma = 0.20;
+        stepSize = 0.9;
+        sr = new SensorReader("./measurements/bergwerk/path1/galaxy/vor/1454776168794.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path1/galaxy/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path1/galaxy/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.15;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path1_galaxy_simple() {
+        runName = "bergwerk_path1_galaxy_simple_fixedlag";
+        bergwerk_path1_galaxy();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path1_galaxy_multi() {
+        runName = "bergwerk_path1_galaxy_multi_fixedlag";
+        bergwerk_path1_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path1_galaxy_shortest() {
+        runName = "bergwerk_path1_galaxy_shortest_fixedlag";
+        bergwerk_path1_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path2_galaxy() {
+        BarometerEvaluation::barometerSigma = 0.20;
+        stepSize = 0.9;
+        sr = new SensorReader("./measurements/bergwerk/path2/galaxy/vor/1454780113404.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path2/galaxy/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path2/galaxy/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.15;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path2_galaxy_simple() {
+        runName = "bergwerk_path2_galaxy_simple_fixedlag";
+        bergwerk_path2_galaxy();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path2_galaxy_multi() {
+        runName = "bergwerk_path2_galaxy_multi_fixedlag";
+        bergwerk_path2_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path2_galaxy_shortest() {
+        runName = "bergwerk_path2_galaxy_shortest_fixedlag";
+        bergwerk_path2_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+    void bergwerk_path3_galaxy() {
+        BarometerEvaluation::barometerSigma = 0.20;
+        stepSize = 0.9;
+        sr = new SensorReader("./measurements/bergwerk/path3/galaxy/vor/1454782896548.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path3/galaxy/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path3/galaxy/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.15;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path3_galaxy_simple() {
+        runName = "bergwerk_path3_galaxy_simple_fixedlag";
+        bergwerk_path3_galaxy();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path3_galaxy_multi() {
+        runName = "bergwerk_path3_galaxy_multi_fixedlag";
+        bergwerk_path3_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path3_galaxy_shortest() {
+        runName = "bergwerk_path3_galaxy_shortest_fixedlag";
+        bergwerk_path3_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+
+    /* ------------------------------------------------------------------------------------------------------------------------------- */
+
+
+    void bergwerk_path4_galaxy() {
+        BarometerEvaluation::barometerSigma = 0.20;
+        stepSize = 0.9;
+        sr = new SensorReader("./measurements/bergwerk/path4/galaxy/vor/1454779020844.csv"); // forward
+        srt = new SensorReaderTurn("./measurements/bergwerk/path4/galaxy/vor/Turns.txt");
+        srs = new SensorReaderStep("./measurements/bergwerk/path4/galaxy/vor/Steps2.txt");
+        gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);
+
+        smoothing_walk_mu = 0.7;
+        smoothing_walk_sigma = 0.5;
+        smoothing_heading_sigma = 5.0;
+        smoothing_baro_sigma = 0.15;
+
+        bool smoothing_resample = false;
+        smoothing_time_delay = 1;
+
+        //Smoothing using Simple Trans
+        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+        if(smoothing_resample)
+            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
+    }
+
+    void bergwerk_path4_galaxy_simple() {
+        runName = "bergwerk_path4_galaxy_simple_fixedlag";
+        bergwerk_path4_galaxy();
+        for (auto& n : grid) {n.imp = 1;}	// remove importance
+        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path4_galaxy_multi() {
+        runName = "bergwerk_path4_galaxy_multi_fixedlag";
+        bergwerk_path4_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+    }
+
+    void bergwerk_path4_galaxy_shortest() {
+
+        runName = "bergwerk_path4_galaxy_shortest_fixedlag";
+        bergwerk_path4_galaxy();
+        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
+        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
+        //walk->times = 2;
+        walk->pOther = 0.15;
+        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+    }
+
+
 };
 
 #endif // EVAL1_H