began putting everything together

2016-01-28 21:49:36 +01:00
parent 07d739ebb7
commit 41713a5d47
30 changed files with 1446 additions and 279 deletions
--- a/code/CMakeLists.txt
+++ b/code/CMakeLists.txt
@@ -64,7 +64,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all

 	-g
-	-O2
+	-O0
 	-DWITH_TESTS
 	-DWITH_ASSERTIONS

--- a/code/DijkstraMapper.h
+++ b/code/DijkstraMapper.h
@@ -0,0 +1,29 @@
+#ifndef DIJKSTRAMAPPER_H
+#define DIJKSTRAMAPPER_H
+
+#include "MyGridNode.h"
+
+/**
+ * allows dijkstra calculation on top of our data-structure
+ */
+class DijkstraMapper {
+
+	Grid<MyGridNode>& grid;
+
+public:
+
+	DijkstraMapper(Grid<MyGridNode>& grid) : grid(grid) {;}
+
+	int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
+
+	const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
+
+	float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
+		float d = ((Point3)n1 - (Point3)n2).length(2.0);
+		//if (d > 20) {d*= 1.30;}
+		return d / std::pow(n2.imp, 3);
+	}
+
+};
+
+#endif // DIJKSTRAMAPPER_H
--- a/code/Helper.h
+++ b/code/Helper.h
@@ -0,0 +1,147 @@
+#ifndef HELPER_H
+#define HELPER_H
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/grid/factory/GridFactory.h>
+#include <Indoor/grid/factory/GridImportance.h>
+
+#include <Indoor/floorplan/FloorplanFactorySVG.h>
+
+#include "Settings.h"
+#include "MyGridNode.h"
+
+class Helper {
+
+private:
+
+
+
+public:
+
+	/** convert height (in cm) to floor-numbers */
+	static int getFloorNr(float z_cm) {
+//		if (z_cm < 360) {return 0;}
+//		if (z_cm < 360+340) {return 1;}
+//		if (z_cm < 360+340+340) {return 2;}
+//		return 3;
+		if (z_cm < 180) {return 0;}
+		if (z_cm < 360+180) {return 1;}
+		if (z_cm < 360+340+180) {return 2;}
+		return 3;
+	}
+
+	/** convert height (in cm) to floor-numbers */
+	static int getFloorNrFloat(float z_cm) {
+		return z_cm / 340.0f;
+	}
+
+	static int getHeight(const int floorNr) {
+		switch(floorNr) {
+			case 0: return 0;
+			case 1: return 360;
+			case 2: return 360+340;
+			case 3: return 360+340+340;
+			default: throw "error";
+		}
+	}
+
+	/** align the given value onto the grid */
+	static int align(const int val) {
+		return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm;
+	}
+
+	/** all floors within the building */
+	struct FHWSFloors {
+
+		Floor f0, f1, f2, f3;
+
+		Stairs s01, s12, s23;
+
+		const LengthF h0 = LengthF::cm(align(getHeight(0)));
+		const LengthF h1 = LengthF::cm(align(getHeight(1)));
+		const LengthF h2 = LengthF::cm(align(getHeight(2)));
+		const LengthF h3 = LengthF::cm(align(getHeight(3)));
+
+		FHWSFloors() {;}
+
+	};
+
+	/** load the entire floorplan */
+	static FHWSFloors getFloors() {
+
+		FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
+		FHWSFloors f;
+
+		f.f0 = fpFac.getFloor("floor_0");
+		f.f1 = fpFac.getFloor("floor_1");
+		f.f2 = fpFac.getFloor("floor_2");
+		f.f3 = fpFac.getFloor("floor_3");
+
+		f.s01 = fpFac.getStairs("staircase_0_1");
+		f.s12 = fpFac.getStairs("staircase_1_2");
+		f.s23 = fpFac.getStairs("staircase_2_3");
+
+		return f;
+
+	}
+
+	template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {
+
+		GridFactory<MyGridNode> gridFac(grid);
+
+		gridFac.addFloor(floors.f0, floors.h0.cm());
+		gridFac.addFloor(floors.f1, floors.h1.cm());
+		gridFac.addFloor(floors.f2, floors.h2.cm());
+		gridFac.addFloor(floors.f3, floors.h3.cm());
+
+		gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
+		gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
+		gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
+
+		// maybe the two sides are wrong?
+		PlatformStair psUpperLeft;
+		psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
+		psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
+		psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
+		gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
+		gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
+		gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
+	//	vis.gp << "set xrange [1100:1800]\n";
+	//	vis.gp << "set yrange [4500:5200]\n";
+
+		PlatformStair psUpperRight;
+		psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
+		psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
+		psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
+		gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
+		gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
+		gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
+	//	vis.gp << "set xrange [6100:6900]\n";
+	//	vis.gp << "set yrange [4500:5200]\n";
+
+		PlatformStair psLowerLeft;
+		psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
+		psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
+		psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
+		gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
+		gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
+		gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
+	//	vis.gp << "set xrange [1300:2100]\n";
+	//	vis.gp << "set yrange [400:1400]\n";
+
+		// remove all isolated nodes not attached to 300,300,floor0
+		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );
+
+		// stamp importance information onto the grid-nodes
+		GridImportance gridImp;
+		gridImp.addImportance(grid, floors.h0.cm());
+		gridImp.addImportance(grid, floors.h1.cm());
+		gridImp.addImportance(grid, floors.h2.cm());
+		gridImp.addImportance(grid, floors.h3.cm());
+
+
+	}
+
+};
+
+#endif // HELPER_H
--- a/code/MyGridNode.h
+++ b/code/MyGridNode.h
@@ -0,0 +1,26 @@
+#ifndef MYGRIDNODE_H
+#define MYGRIDNODE_H
+
+#include <Indoor/grid/GridNode.h>
+#include <Indoor/grid/GridPoint.h>
+
+/**
+ * the nodes we add to our grid
+ */
+struct MyGridNode : public GridNode, public GridPoint {
+
+	/** distance to the desired target */
+	float distToTarget = 1.0;
+
+	/** node importance based on surroundings */
+	float imp = 1.0;
+
+public:
+
+	/** needed ctor */
+	MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
+
+};
+
+
+#endif // MYGRIDNODE_H
--- a/code/Settings.h
+++ b/code/Settings.h
@@ -0,0 +1,26 @@
+#ifndef OTHER_SETTINGS_H
+#define OTHER_SETTINGS_H
+
+#define USE_STATIC_CIRCULAR_BUFFERING false
+#define USE_BAROMETER_SMOOTHING_RC_LOWPASS false
+#define USE_BAROMETER_SMOOTHING_HEAD_TAIL false
+#define USE_BAROMETRIC_FORMULAR false
+
+#include <string>
+
+namespace MiscSettings {
+
+	const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
+
+	const int gridSize_cm = 40;
+
+	const int timeSteps = 500;
+
+	const int numParticles = 1000;
+
+}
+
+
+#endif // OTHER_SETTINGS_H
+
+
--- a/code/Vis.h
+++ b/code/Vis.h
@@ -26,10 +26,15 @@ public:
 	Vis() {

 		gp << "set hidden3d front\n";
-		gp << "set view equal xy\n";
+		//gp << "set view equal xy\n";
 		gp << "set ticslevel 0\n";
 		gp << "set cbrange[0.8:2.0]\n";

+		gp << "unset xtics\n";
+		gp << "unset ytics\n";
+		gp << "unset ztics\n";
+		gp << "unset border\n";
+
 		// attach all layers
 		splot.add(&floors);
 		splot.add(&gridNodes);
@@ -80,6 +85,14 @@ public:
 		gridNodes.clear();;
 	}

+	void clearStates() {
+		particles.clear();
+	}
+
+	template <typename T> void addState(const GridWalkState<T>& n) {
+		particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
+	}
+
 	template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
 		particles.clear();;
 		for (const GridWalkState<T>& n : states) {
--- a/code/eval/Eval.h
+++ b/code/eval/Eval.h
@@ -0,0 +1,66 @@
+#ifndef EVAL_H
+#define EVAL_H
+
+#include "EvalBase.h"
+#include "../DijkstraMapper.h"
+#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
+#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
+#include <Indoor/grid/walk/GridWalkPushForward.h>
+#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+
+class Eval : public EvalBase {
+
+public:
+
+
+
+	Eval() {
+
+
+		pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
+
+		MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
+		MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
+
+		//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
+		//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
+		//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
+		GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
+
+		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
+
+		sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
+		srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
+		srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
+		//gtw = getGroundTruthWay(*sr, gtwp, way2);
+
+	}
+
+
+
+	void setEval1() {
+
+
+		runName = "TODO";
+
+		// the particle filter's evaluation method
+		std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
+		eval.get()->setUsage(true, false, false, false, false);
+		pf->setEvaluation( std::move(eval) );
+
+		// resampling step?
+		pf->setNEffThreshold(1.0);
+		pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
+
+		// state estimation step
+		pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+
+	}
+
+
+
+};
+
+#endif // EVAL_H
--- a/code/eval/EvalBase.h
+++ b/code/eval/EvalBase.h
@@ -0,0 +1,262 @@
+#ifndef EVALBASE_H
+#define EVALBASE_H
+
+#include "../Settings.h"
+#include "../Helper.h"
+#include "../Vis.h"
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <KLib/math/statistics/Statistics.h>
+
+#include "GroundTruthWay.h"
+
+#include "../particles/P3.h"
+#include "../particles/MyState.h"
+#include "../particles/MyObservation.h"
+#include "../particles/MyEvaluation.h"
+#include "../particles/MyTransition.h"
+#include "../particles/MyInitializer.h"
+
+#include "../reader/SensorReader.h"
+#include "../reader/SensorReaderStep.h"
+#include "../reader/SensorReaderTurn.h"
+
+#include "../lukas/TurnObservation.h"
+#include "../lukas/StepObservation.h"
+
+#include "../toni/BarometerSensorReader.h"
+
+#include "../frank/WiFiSensorReader.h"
+#include "../frank/BeaconSensorReader.h"
+
+class EvalBase {
+
+protected:
+
+	Grid<MyGridNode> grid;
+	Helper::FHWSFloors floors;
+	Vis vis;
+
+	K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
+
+	SensorReader* sr;
+	SensorReaderTurn* srt;
+	SensorReaderStep* srs;
+
+	GroundTruthWay gtw;
+
+	std::string runName;
+
+public:
+
+	EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
+
+		// build the grid
+		Helper::buildTheGrid(grid, floors);
+
+		// setup the visualisation
+		vis.addFloor(floors.f0, floors.h0);
+		vis.addFloor(floors.f1, floors.h1);
+		vis.addFloor(floors.f2, floors.h2);
+		vis.addFloor(floors.f3, floors.h3);
+
+	}
+
+	void run() {
+
+			// read CSV input
+			// const int s_wifi = 0;
+			//SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv");
+			const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
+			const int s_linearAcceleration = 2;
+
+			std::list<TurnObservation> turn_observations;
+			std::list<StepObservation> step_observations;
+
+			//Create an BarometerSensorReader
+			BarometerSensorReader baroSensorReader;
+
+
+			//Read all turn Observations
+			while(srt->hasNext()) {
+
+				SensorEntryTurn set = srt->getNext();
+				TurnObservation to;
+
+				to.ts = set.ts;
+				to.delta_heading = set.delta_heading;
+				to.delta_motion = set.delta_motion;
+
+				turn_observations.push_back(to);
+			}
+
+
+			//Step Observations
+			while(srs->hasNext()) {
+
+				SensorEntryStep ses = srs->getNext();
+				StepObservation so;
+
+				so.ts = ses.ts;
+
+				step_observations.push_back(so);
+			}
+
+
+
+			// the to-be-evaluated observation
+			MyObservation obs;
+
+
+			std::vector<Point3> pathEst;
+
+			uint64_t lastTransitionTS = 0;
+			bool firstReading = true;
+			int64_t start_time = -1;
+
+			K::Statistics<double> stats;
+
+			// process each sensor reading
+			while(sr->hasNext()) {
+
+				// get the next sensor reading from the CSV
+				const SensorEntry se = sr->getNext();
+
+				//start_time needed for time calculation of steps and turns
+				obs.latestSensorDataTS = se.ts;
+				if (start_time == -1) {start_time = se.ts;}
+				int64_t current_time = se.ts - start_time;
+
+				// ensure the graph timestamp starts with the first reading
+				if (firstReading) {
+					//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
+					firstReading = false;
+				}
+
+				switch(se.idx) {
+
+					case s_wifi: {
+						obs.wifi = WiFiSensorReader::readWifi(se);
+						break;
+					}
+
+					case s_beacons: {
+						BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
+						if (!boe.mac.empty()) {
+							obs.beacons.entries.push_back(boe);
+						}			// add the observed beacon
+						obs.beacons.removeOld(obs.latestSensorDataTS);
+						break;
+					}
+
+					case s_barometer: {
+						obs.barometer = baroSensorReader.readBarometer(se);
+						break;
+					}
+
+					case s_linearAcceleration:{
+							baroSensorReader.readVerticalAcceleration(se);
+							break;
+					}
+
+				}
+
+
+				// scheduled transition every 500 ms
+				if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
+				for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
+
+					//Steps are sorted in the list by timestamp.
+					//If the current observation timestamp is bigger/equal
+					//to the current step timestamp, use this step as observation
+					//and remove it from the list.
+					//The new first timestamp in the list will be then be the next one (timestamp-wise)
+					StepObservation so;
+					if(current_time >= step_observations.front().ts && !step_observations.empty()) {
+						so.step = true;
+						so.ts = current_time;
+
+						obs.step = &so;
+						step_observations.pop_front();
+
+					}
+					else {
+						so.step = false;
+						so.ts = current_time;
+
+						obs.step = &so;
+					}
+
+					TurnObservation to;
+					//same principal as for steps is applied for turns
+					if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
+						to = turn_observations.front();
+						obs.turn = &to;
+
+						turn_observations.pop_front();
+					}
+					else {
+						to.delta_heading = 0.0;
+						to.delta_motion = 0.0;
+
+						obs.turn = &to;
+					}
+
+
+					// let the transition know the current timestamp to determine the time since the last transition
+					//if (!useSimpleTrans) {
+						((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
+					//} else {
+					//	((MyTransitionSimple*)pf->getTransition())->setCurrentTime(lastTransitionTS);
+					//}
+
+					// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
+					const MyState est = pf->update(nullptr, obs);
+					const Point3 curEst = est.pCur;
+					pathEst.push_back(curEst);
+
+					// debug print current particle set.
+					//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
+
+					// error calculation. compare ground-truth to estimation
+//					const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
+
+//					// TODO
+//					const Point3 diff = curEst - curGT;
+
+//					//if (std::abs(diff.z) < 0.1) {
+//						const float err = diff.length();
+//						std::cout << err << std::endl;
+//						stats.add(err);
+//						std::cout << stats.asString() << std::endl;
+//					//}
+
+					vis.clearStates();
+					for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
+
+					vis.show();;
+
+				}
+
+
+			}
+
+			{
+//				vis.setShowParticles(false);
+//				vis.setShowTime(false);
+//				vis.setShowCurPos(false);
+//				vis.debugProcess(0, pathEst, gtw, pf, layers);
+//				std::ofstream out("/tmp/" + runName + ".data");
+//				out << vis.getDataset();
+//				out.close();
+			}
+
+			sleep(1000);
+
+		}
+
+
+
+};
+
+#endif // EVALBASE_H
--- a/code/eval/GroundTruthWay.h
+++ b/code/eval/GroundTruthWay.h
@@ -0,0 +1,24 @@
+#ifndef GROUNDTRUTHWAY_H
+#define GROUNDTRUTHWAY_H
+
+#include <Indoor/math/Interpolator.h>
+#include <Indoor/geo/Point3.h>
+
+/**
+ * interpolated ground-trouth based on timed check-points
+ */
+class GroundTruthWay : public Interpolator<uint64_t, Point3> {
+
+public:
+
+	Point3 getPosAtTime(const uint64_t ts) const {
+		return get(ts);
+	}
+
+	/** get the ground truth way */
+	const std::vector<Entry>& getWay() const {return entries;}
+
+};
+
+
+#endif // GROUNDTRUTHWAY_H
--- a/code/frank/BeaconEvaluation.h
+++ b/code/frank/BeaconEvaluation.h
@@ -39,10 +39,12 @@ public:
 			if (!beacon) {continue;}

 			// distance (in meter) between particle and AP
-			const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
+			//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
+			const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0;

 			// floor difference?
-			const double floorDist = std::abs(beacon->zNr - state.z_nr);
+			//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
+			const float floorDist = std::round(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));

 			// estimate the rssi depending on above distance
 			const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
--- a/code/frank/BeaconSensorReader.h
+++ b/code/frank/BeaconSensorReader.h
@@ -1,7 +1,7 @@
 #ifndef BEACONSENSORREADER_H
 #define BEACONSENSORREADER_H

-#include "../SensorReader.h"
+#include "../reader/SensorReader.h"
 #include "BeaconObservation.h"
 #include "Settings.h"
 #include <cassert>
--- a/code/frank/PositionedBeacon.h
+++ b/code/frank/PositionedBeacon.h
@@ -2,19 +2,26 @@
 #define POSITIONEDBEACON_H

 #include "WiFiAP.h"
-#include "Position3D.h"
+//#include "Position3D.h"
+#include <Indoor/geo/Point3.h>

-class PositionedBeacon : public Position3D  {
+class PositionedBeacon : public Point3  {

 public:

 	MACAddress mac;
-	double tx;
-	double pl;
+	float tx;
+	float pl;
+
+//	/** ctor */
+//	PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
+//		mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
+//		;
+//	}

 	/** ctor */
-	PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
-		mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
+	PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
+		mac(mac), tx(tx), pl(pl), Point3(x_cm, y_cm, z_cm) {
 		;
 	}

--- a/code/frank/PositionedWiFiAP.h
+++ b/code/frank/PositionedWiFiAP.h
@@ -2,16 +2,22 @@
 #define POSITIONEDWIFIAP_H

 #include "WiFiAP.h"
-#include "Position3D.h"
+//#include "Position3D.h"
+#include <Indoor/geo/Point3.h>

-
-class PositionedWifiAP : public WiFiAP, public Position3D  {
+class PositionedWifiAP : public WiFiAP, public Point3  {

 public:

+//	/** ctor */
+//	PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
+//		WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
+//		;
+//	}
+
 	/** ctor */
-	PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
-		WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
+	PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
+		WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) {
 		;
 	}

--- a/code/frank/Settings.h
+++ b/code/frank/Settings.h
@@ -7,6 +7,8 @@

 #include <unordered_map>

+#include "../Helper.h"
+
 class Settings {

 private:
@@ -21,49 +23,49 @@ public:
 		const double pl = 2.7;
 		const double tx = -46;

-		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  3, tx, pl-0.5);
-		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  3, tx, pl-0.5);
-		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5);
-		addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl);
-		addAP(("00:04:96:77:ed:69"), "i.3.3",  6460, 3400, 3, tx, pl);
+		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
+		addAP(("00:04:96:77:ed:69"), "i.3.3",  6460, 3400, Helper::getHeight(3), tx, pl);

 		// 2nd floor (vague AP position)
-		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, 2, tx, pl-0.5);
-		addAP(("00:04:96:6b:bf:f9"), "I.2.9",  3000, 3350, 2, tx, pl);
-		addAP(("00:04:96:77:ec:a9"), "I.2.15",  290,  750, 2, tx, pl);
-		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, 2, tx, pl-0.5);
-		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 2, tx, pl-0.5);
+		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, Helper::getHeight(2), tx, pl-0.5);
+		addAP(("00:04:96:6b:bf:f9"), "I.2.9",  3000, 3350, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:77:ec:a9"), "I.2.15",  290,  750, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, Helper::getHeight(2), tx, pl-0.5);
+		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5);

 		// 1st floor (vague AP position)
-		addAP(("00:04:96:6c:cf:19"), "I.1.2",  6150, 3420, 1, tx, pl);
-		addAP(("00:04:96:7d:07:79"), "I.1.9",  1800, 3300, 1, tx, pl);
-		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, 1, tx, pl-0.25);
-		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, 1, tx, pl-0.25);
-		addAP(("00:04:96:6b:45:59"), "I.1.30",  800, 4800, 1, tx, pl);
-		addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl);
+		addAP(("00:04:96:6c:cf:19"), "I.1.2",  6150, 3420, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:7d:07:79"), "I.1.9",  1800, 3300, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, Helper::getHeight(1), tx, pl-0.25);
+		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, Helper::getHeight(1), tx, pl-0.25);
+		addAP(("00:04:96:6b:45:59"), "I.1.30",  800, 4800, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);

 		// 0th floor (exact AP position)
-		addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530,  4970, 0, tx, pl);
-		addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030,  270, 0, tx, pl);
-		addAP(("00:04:96:6C:A4:A9"), "I.0.9",  1660, 2780, 0, tx, pl);
-		addAP(("00:04:96:77:EE:69"), "I.0.7",  3560, 3380, 0, tx, pl);
-		addAP(("00:04:96:6B:46:09"), "I.0.xx",  6860, 3690, 0, tx, pl);
-		addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 0, tx, pl);		// vague!!
+		addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530,  4970, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030,  270, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:A4:A9"), "I.0.9",  1660, 2780, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:77:EE:69"), "I.0.7",  3560, 3380, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6B:46:09"), "I.0.xx",  6860, 3690, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl);		// vague!!

 		const int ibOff = +2;
 		const float ibPLE = 1.9;
-		addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, 3);		// id:16
-        addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2);		// id:18
-		addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1);		// id:11
-        addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0);		// id:20
+		addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3));		// id:16
+		addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2));		// id:18
+		addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1));		// id:11
+		addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0));		// id:20

-		addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, 2);	// idis switchboard
-		addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2);		// idis outside
-		addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2);		// idis toni
+		addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2));	// idis switchboard
+		addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2));		// idis outside
+		addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2));		// idis toni

-		addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, 1);		// i.1.47
-		addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1);		// fachschaft
-		addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 1);		// neben fachschaft
+		addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1));		// i.1.47
+		addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1));		// fachschaft
+		addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1));		// neben fachschaft

 //		addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
 //		addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
--- a/code/frank/WiFiEvaluation.h
+++ b/code/frank/WiFiEvaluation.h
@@ -37,14 +37,14 @@ public:

 		//const double tx = -48;		// tablet
 		//const double pl = 3.15;
-		const double waf = 7;//10.0;
-		const double floor_height_cm = 350;
+		const float waf = 7;//10.0;		// was 7 before?! has something todo with the floor heights / levels
+	//	const int floor_height_cm = 350;

 		// get the ap the client had the strongest measurement for
 		const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
-		const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
-		const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
-		const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
+		//const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
+		//const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
+		//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);

 		// process each detected AP
 		for (const WiFiObservationEntry& entry : obs.entries) {
@@ -53,20 +53,23 @@ public:
 			const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);

 			// distance (in meter) between particle and AP
-			const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
+			//const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
+			const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0;

 			// floor difference?
-			const double floorDist = std::abs(ap->zNr - state.z_nr);
+			const float floorDiff = std::round(
+						std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z))
+		);

 			// estimate the rssi depending on above distance
-			const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDist * waf);
+			const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);

 			// the measured rssi
 			const double realRSSI = entry.rssi;

 			// the measured relative rssi
-			const double realRelRSSI = strongest.rssi - realRSSI;
-			const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
+			//const double realRelRSSI = strongest.rssi - realRSSI;
+			//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;

 			// probability? (sigma grows with measurement's age)
 			const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
--- a/code/frank/WiFiSensorReader.h
+++ b/code/frank/WiFiSensorReader.h
@@ -1,7 +1,7 @@
 #ifndef WIFISENSORREADER_H
 #define WIFISENSORREADER_H

-#include "../SensorReader.h"
+#include "../reader/SensorReader.h"
 #include "WiFiObservation.h"

 #include <cassert>
--- a/code/lukas/StepEvaluation.h
+++ b/code/lukas/StepEvaluation.h
@@ -8,10 +8,10 @@
 #include "StepObservation.h"
 #include <math.h>

-static double mu_walk = 40;
-static double sigma_walk = 15;
-static double mu_stop = 0;
-static double sigma_stop = 5;
+static constexpr double mu_walk = 40;
+static constexpr double sigma_walk = 15;
+static constexpr double mu_stop = 0;
+static constexpr double sigma_stop = 5;

 class StepEvaluation {

@@ -19,29 +19,31 @@ public:

    double getProbability(const MyState& state, const StepObservation* obs) const {

-        double distance = state.distanceWalkedCM;
+		return 1;

-        double a = 1.0;
-	   double mu_distance = 0; //cm
-		double sigma_distance = 10.0; //cm
+//        double distance = state.distanceWalkedCM;

-        if(obs->step) {
-            a = 1.0;
-			mu_distance = mu_walk;//80.0; //cm
-			sigma_distance = sigma_walk;//40.0; //cm
-        }
+//        double a = 1.0;
+//	   double mu_distance = 0; //cm
+//		double sigma_distance = 10.0; //cm

-        else {
-            a = 0.0;
-			mu_distance = mu_stop; //cm
-			sigma_distance = sigma_stop; //cm
-        }
+//        if(obs->step) {
+//            a = 1.0;
+//			mu_distance = mu_walk;//80.0; //cm
+//			sigma_distance = sigma_walk;//40.0; //cm
+//        }

-        //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
-        const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
-                        (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
+//        else {
+//            a = 0.0;
+//			mu_distance = mu_stop; //cm
+//			sigma_distance = sigma_stop; //cm
+//        }

-        return p;
+//        //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
+//        const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
+//                        (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
+
+//        return p;
    }

 };
--- a/code/lukas/TurnEvaluation.h
+++ b/code/lukas/TurnEvaluation.h
@@ -4,10 +4,10 @@

 #include "../particles/MyState.h"
 #include "TurnObservation.h"
-#include <boost/math/special_functions/bessel.hpp>
+//#include <boost/math/special_functions/bessel.hpp>
 #include <math.h>

-static double sigma_heading = 35;
+static constexpr double sigma_heading = 35;

 class TurnEvaluation {

@@ -17,55 +17,33 @@ public:

    double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {

-        //Particle's heading change
-        double delta_heading_particle = state.heading - state.heading_old;
+		return 1;
+
+//        //Particle's heading change
+//        double delta_heading_particle = state.heading - state.heading_old;


-        //Correct offset of the heading change
-        if (delta_heading_particle < -180) {
-            delta_heading_particle += 360;
-        }
-        else if (delta_heading_particle > 180) {
-            delta_heading_particle -= 360;
-        }
+//        //Correct offset of the heading change
+//        if (delta_heading_particle < -180) {
+//            delta_heading_particle += 360;
+//        }
+//        else if (delta_heading_particle > 180) {
+//            delta_heading_particle -= 360;
+//        }


-        //Switch between simple and improved evaluation
-        //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
-        //normal distribution
-        if(simple) {
+//        //Switch between simple and improved evaluation
+//        //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
+//        //normal distribution
+//		//if(simple) {

-			double sigma_delta_heading = sigma_heading;
+//			double sigma_delta_heading = sigma_heading;

-            const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
+//            const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);


-            return p;
-        }
-
-        //use the von Mises distribution
-        else {
-            //Here some calculations must be done in rad
-
-            double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
-            double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
-
-            //Equation for estimating kappa value of von Mises distribution
-            //empirically estimated
-            double kappa = 0.0;
-
-            kappa = 5.0 / exp(2 * delta_motion_rad);
-
-            double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
-
-
-
-            //pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
-            const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
-
-            return p;
-
-        }
+//            return p;
+//	   // }

    }

--- a/code/lukas/TurnReader.h
+++ b/code/lukas/TurnReader.h
@@ -1,7 +1,7 @@
 #ifndef TURNREADER_H
 #define TURNREADER_H

-#include "../SensorReaderTurn.h"
+#include "../reader/SensorReaderTurn.h"
 #include "TurnObservation.h"

 class TurnReader {
--- a/code/main.cpp
+++ b/code/main.cpp
@@ -1,146 +1,62 @@
-#include <Indoor/grid/factory/GridFactory.h>
-#include <Indoor/floorplan/FloorplanFactorySVG.h>
+
 #include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
 #include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
 #include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
 #include <Indoor/grid/walk/GridWalkPushForward.h>
-#include <Indoor/grid/factory/GridImportance.h>

 #include "Vis.h"
+#include "Helper.h"
+#include "MyGridNode.h"
+#include "Helper.h"
+#include "DijkstraMapper.h"

-namespace Settings {
-	const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
-	const int gridSize_cm = 20;
-}
+#include "eval/Eval.h"
+#include "eval/EvalBase.h"

-struct MyNode : public GridNode, public GridPoint {
-	float distToTarget = 1.0;
-	float imp = 1.0;
-public:
-	MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
-};
+Settings settings;

+void testModelWalk() {

-int align(const int val) {
-	return val / Settings::gridSize_cm * Settings::gridSize_cm;
-}
+	Grid<MyGridNode> grid(MiscSettings::gridSize_cm);

-// dijkstra mapper
-class DijkstraMapper {
-	Grid<MyNode>& grid;
-public:
-	DijkstraMapper(Grid<MyNode>& grid) : grid(grid) {;}
-	int getNumNeighbors(const MyNode& node) const {return node.getNumNeighbors();}
-	const MyNode* getNeighbor(const MyNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
-	float getWeightBetween(const MyNode& n1, const MyNode& n2) const {
-		float d = ((Point3)n1 - (Point3)n2).length(2.0);
-		//if (d > 20) {d*= 1.30;}
-		return d / std::pow(n2.imp, 3);
-	}
-};
+	Helper::FHWSFloors floors = Helper::getFloors();
+	Helper::buildTheGrid(grid, floors);

-int main(void) {
-
-	Grid<MyNode> grid(Settings::gridSize_cm);
-	GridFactory<MyNode> gridFac(grid);
-
-
-	FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);
-
-	Floor f0 = fpFac.getFloor("floor_0");
-	Floor f1 = fpFac.getFloor("floor_1");
-	Floor f2 = fpFac.getFloor("floor_2");
-	Floor f3 = fpFac.getFloor("floor_3");
-
-	Stairs s01 = fpFac.getStairs("staircase_0_1");
-	Stairs s12 = fpFac.getStairs("staircase_1_2");
-	Stairs s23 = fpFac.getStairs("staircase_2_3");
-
-	const LengthF h0 = LengthF::cm(align(0));
-	const LengthF h1 = LengthF::cm(align(360));
-	const LengthF h2 = LengthF::cm(align(360+340));
-	const LengthF h3 = LengthF::cm(align(360+340+340));
-
-	gridFac.addFloor(f0, h0.cm());
-	gridFac.addFloor(f1, h1.cm());
-	gridFac.addFloor(f2, h2.cm());
-	gridFac.addFloor(f3, h3.cm());
-
-	gridFac.addStairs(s01, h0.cm(), h1.cm());
-	gridFac.addStairs(s12, h1.cm(), h2.cm());
-	gridFac.addStairs(s23, h2.cm(), h3.cm());
-
-	// maybe the two sides are wrong?
-	PlatformStair psUpperLeft;
-	psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
-	psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
-	psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
-	gridFac.buildPlatformStair(psUpperLeft, h0.cm(), h1.cm());
-	gridFac.buildPlatformStair(psUpperLeft, h1.cm(), h2.cm());
-	gridFac.buildPlatformStair(psUpperLeft, h2.cm(), h3.cm());
-//	vis.gp << "set xrange [1100:1800]\n";
-//	vis.gp << "set yrange [4500:5200]\n";
-
-	PlatformStair psUpperRight;
-	psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
-	psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
-	psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
-	gridFac.buildPlatformStair(psUpperRight, h0.cm(), h1.cm());
-	gridFac.buildPlatformStair(psUpperRight, h1.cm(), h2.cm());
-	gridFac.buildPlatformStair(psUpperRight, h2.cm(), h3.cm());
-//	vis.gp << "set xrange [6100:6900]\n";
-//	vis.gp << "set yrange [4500:5200]\n";
-
-	PlatformStair psLowerLeft;
-	psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
-	psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
-	psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
-	gridFac.buildPlatformStair(psLowerLeft, h0.cm(), h1.cm());
-	gridFac.buildPlatformStair(psLowerLeft, h1.cm(), h2.cm());
-	gridFac.buildPlatformStair(psLowerLeft, h2.cm(), h3.cm());
-//	vis.gp << "set xrange [1300:2100]\n";
-//	vis.gp << "set yrange [400:1400]\n";
-
-	gridFac.removeIsolated( (MyNode&)grid.getNodeFor(GridPoint(300,300,h0.cm())) );
-
-	GridImportance gridImp;
-	gridImp.addImportance(grid, h0.cm());
-	gridImp.addImportance(grid, h1.cm());
-	gridImp.addImportance(grid, h2.cm());
-	gridImp.addImportance(grid, h3.cm());
-
-	MyNode& start = (MyNode&)grid.getNodeFor(GridPoint(500,300,h0.cm()));
-	MyNode& end = (MyNode&)grid.getNodeFor(GridPoint(7000,5000,h3.cm()));
+	MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
+	MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));

 	Vis vis;
-	vis.addFloor(f0, h0);
-	vis.addFloor(f1, h1);
-	vis.addFloor(f2, h2);
-	vis.addFloor(f3, h3);
+	vis.addFloor(floors.f0, floors.h0);
+	vis.addFloor(floors.f1, floors.h1);
+	vis.addFloor(floors.f2, floors.h2);
+	vis.addFloor(floors.f3, floors.h3);
+
+//	vis.gp << "set xrange [1000:4000]\n";
+//	vis.gp << "set yrange [1000:4000]\n";
+//	vis.gp << "set zrange [0:600]\n";
+
+	// switch between different grid-walkers
+	GridWalkRandomHeadingUpdate<MyGridNode> walk;
+	//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
+	//GridWalkPushForward<MyGridNode> walk;
+	//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);


+	std::vector<GridWalkState<MyGridNode>> states;
+	for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }

+//	track the number-of-visits for each node to draw something like a particle-heat-map?

-	//GridWalkRandomHeadingUpdate<MyNode> walk;
-	//GridWalkRandomHeadingUpdateAdv<MyNode> walk;
-	//GridWalkPushForward<MyNode> walk;
-	GridWalkLightAtTheEndOfTheTunnel<MyNode> walk(grid, DijkstraMapper(grid), end);
-
-
-	std::vector<GridWalkState<MyNode>> states;
-	for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState<MyNode>(&start, Heading::rnd())); }
-
+	// show the importance factors
 //	vis.addGrid(grid);
 //	vis.show();
 //	sleep(100);
 //	vis.removeGrid();

 	Distribution::Normal<float> wDist(0.3, 0.3);
-	//std::minstd_rand gen(1234);
-	//std::normal_distribution<float> dist(0.6, 0.3);

 	while(true) {
-		for (GridWalkState<MyNode>& state : states) {
+		for (GridWalkState<MyGridNode>& state : states) {
 			state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
 		}
 		usleep(1000*80);
@@ -152,6 +68,20 @@ int main(void) {

 	sleep(1000);

+}
+
+
+
+int main(void) {
+
+	//testModelWalk();
+
+	Eval eval;
+	eval.setEval1();
+	eval.run();
+
 	return 0;

 }
+
+
--- a/code/particles/MyControl.h
+++ b/code/particles/MyControl.h
@@ -0,0 +1,8 @@
+#ifndef MYCONTROL_H
+#define MYCONTROL_H
+
+struct MyControl {
+
+};
+
+#endif // MYCONTROL_H
--- a/code/particles/MyEvaluation.h
+++ b/code/particles/MyEvaluation.h
@@ -0,0 +1,85 @@
+#ifndef MYEVALUATION_H
+#define MYEVALUATION_H
+
+#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
+#include "MyObservation.h"
+#include "MyState.h"
+
+#include "../frank/WiFiEvaluation.h"
+#include "../frank/BeaconEvaluation.h"
+#include "../toni/BarometerEvaluation.h"
+#include "../lukas/StepEvaluation.h"
+#include "../lukas/TurnEvaluation.h"
+
+class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+private:
+
+	WiFiEvaluation wifiEval;
+	BeaconEvaluation beaconEval;
+	BarometerEvaluation barometerEval;
+	StepEvaluation stepEval;
+	TurnEvaluation turnEval;
+
+	bool useWifi = true;
+	bool useStep = true;
+	bool useTurn = true;
+	bool useBaro = true;
+	bool useIB = true;
+
+public:
+
+	void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
+		this->useWifi = useWifi;
+		this->useStep = useStep;
+		this->useTurn = useTurn;
+		this->useBaro = useBaro;
+		this->useIB = useIB;
+	}
+
+	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
+
+		//if (observation.wifi) {
+		wifiEval.nextObservation(observation.wifi);
+		//}
+
+		// evalulate each particle
+		double sum = 0;
+		for (K::Particle<MyState>& p : particles) {
+
+			double weight = 1.0;
+
+			if (useWifi) {
+				weight *= wifiEval.getProbability(p.state, observation);
+			}
+
+//			if (useBaro && observation.barometer) {
+//				weight *= barometerEval.getProbability(p.state, observation.barometer);
+//            }
+
+//			if (useIB) {
+//				weight *= beaconEval.getProbability(p.state, observation);
+//			}
+
+//			if (useStep) {
+//				weight *= stepEval.getProbability(p.state, observation.step);
+//				p.state.distanceWalkedCM = 0.0;
+//			}
+
+//			if (useTurn) {
+//				weight *= turnEval.getProbability(p.state, observation.turn, true);
+//			}
+
+			// set and accumulate
+            p.weight = weight;
+			sum += p.weight;
+
+		}
+
+		return sum;
+
+	}
+
+};
+
+#endif // MYEVALUATION_H
--- a/code/particles/MyInitializer.h
+++ b/code/particles/MyInitializer.h
@@ -0,0 +1,59 @@
+#ifndef MYINITIALIZER3_H
+#define MYINITIALIZER3_H
+
+#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
+#include "MyState.h"
+
+#include <Indoor/grid/Grid.h>
+
+class MyInitializer : public K::ParticleFilterInitializer<MyState> {
+
+private:
+	int x_cm;
+	int y_cm;
+	int z_cm;
+	int heading;
+
+	Grid<MyGridNode>& grid;
+
+public:
+
+	/** q0 = random */
+	MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
+
+	}
+
+	/** q0 = given */
+	MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
+		grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
+
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		std::minstd_rand gen;
+		std::uniform_int_distribution<> dist(0, grid.getNumNodes());
+
+		for (K::Particle<MyState>& p : particles) {
+
+			MyGridNode& n = grid[dist(gen)];
+
+			//p.state.pCur = Point3(x_cm, y_cm, z_cm);
+			//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
+			//p.state.walkState.node = &grid.getNodeFor(gp);
+
+			p.state.pCur = (Point3) n;
+			p.state.walkState.node = &n;
+
+			p.state.pOld = p.state.pCur;
+			p.state.walkState.heading = Heading::rnd();
+			p.state.distanceWalkedCM = 0;
+			p.state.hPa = 0;
+
+		}
+
+	}
+
+};
+
+#endif // MYINITIALIZER_H
--- a/code/particles/MyObservation.h
+++ b/code/particles/MyObservation.h
@@ -0,0 +1,49 @@
+#ifndef MYOBSERVATION_H
+#define MYOBSERVATION_H
+
+#include "../frank/WiFiObservation.h"
+#include "../frank/BeaconObservation.h"
+#include "../toni/BarometerObservation.h"
+#include "../lukas/StepObservation.h"
+#include "../lukas/TurnObservation.h"
+
+/**
+ * all available sensor readings
+ */
+struct MyObservation {
+
+	/** wifi observation */
+	WiFiObservation wifi;
+
+	/** barometer observation data (if any) */
+	BarometerObservation* barometer = nullptr;
+
+	/** beacon observation data */
+	BeaconObservation beacons;
+
+    /** step observation data (if any) */
+    StepObservation* step = nullptr;
+
+    /** turn observation data (if any) */
+    TurnObservation* turn = nullptr;
+
+	/** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */
+	uint64_t latestSensorDataTS = 0;
+
+	/** ctor */
+	MyObservation() {
+//		reset();
+	}
+
+//	/** set all observations to null */
+//	void reset() {
+//		//delete wifi; wifi = nullptr;
+//		delete barometer; barometer = nullptr;
+//		delete beacons; beacons = nullptr;
+//        //delete step; step = nullptr;
+//        //delete turn; turn = nullptr;
+//	}
+
+};
+
+#endif // MYOBSERVATION_H
--- a/code/particles/MyState.h
+++ b/code/particles/MyState.h
@@ -0,0 +1,136 @@
+#ifndef MYSTATE_H
+#define MYSTATE_H
+
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/optimization/NumOptVector.h>
+
+#include <Indoor/grid/walk/GridWalkState.h>
+
+#include "../MyGridNode.h"
+
+/**
+ * one possible state for the pedestrian
+ * 3D position (x, y, floor-nr)
+ */
+struct MyState {
+
+	// current position
+	Point3 pCur;
+
+	// previous position
+	Point3 pOld;
+
+	// the grid-walk state
+	GridWalkState<MyGridNode> walkState;
+
+	int distanceWalkedCM;
+
+
+//    double heading_old;
+
+//	//double transHeading;
+//	float numZChanges;
+
+//	// cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
+//	double distanceWalkedCM;
+
+	double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h
+
+//    double vertical_acc; //vertical acceleration
+
+//	/** the pedestrian's current heading */
+//	double heading;
+
+	/** empty ctor */
+	MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
+		;
+	}
+
+//	/** get the 2D distance between this state and the given x,y (in centimter) */
+//	double getDistance2D(const double x_cm, const double y_cm) const {
+//		const double dx = (x_cm - this->x_cm);
+//		const double dy = (y_cm - this->y_cm);
+//		return std::sqrt( (dx*dx) + (dy*dy) );
+//	}
+
+//	/** get the 3D distance between this state and the given x,y,floor (in centimter) */
+//	double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
+//		const double dx = (x_cm - this->x_cm);
+//		const double dy = (y_cm - this->y_cm);
+//		const double dz = (z_nr - this->z_nr) * floor_height_cm;
+//		return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
+//	}
+
+	/** -------- METHODS FOR THE PARTICLE FILTER -------- */
+
+	MyState& operator += (const MyState& o) {
+		pCur += o.pCur;
+		//hPa += o.hPa;
+		//distanceWalked += o.distanceWalked;
+		return *this;
+	}
+
+	MyState& operator /= (const double d) {
+		pCur /= d;
+		//hPa /= d;
+		//distanceWalked /= d;
+		return *this;
+	}
+
+	MyState operator * (const double d) const {
+		MyState s = MyState(*this);
+		s.pCur *= d;
+		//s.hPa *= d;
+		//distanceWalked *= d;
+		return s;
+	}
+
+
+	// use the default one
+//	MyState& operator = (const MyState& o) {
+//		x_cm = o.x_cm;
+//		y_cm = o.y_cm;
+//		z_nr = o.z_nr;
+//		x_cm_old = o.x_cm_old;
+//		y_cm_old = o.y_cm_old;
+//		z_nr_old = o.z_nr_old;
+//		hPa = o.hPa;
+//		heading_old = o.heading_old;
+//		heading = o.heading;
+//		distanceWalkedCM = o.distanceWalkedCM;
+//		return *this;
+//	}
+
+//	/** rejection for the regional estimator. reject after 150cm distance */
+//	bool belongsToRegion(const MyState& o) const {
+
+////		// do NOT group particles in distinct floors!
+////		if (z_nr != o.z_nr) {return false;}
+
+////		// get the 2D distance
+////		double d =	(x_cm - o.x_cm)*(x_cm - o.x_cm) +
+////					(y_cm - o.y_cm)*(y_cm - o.y_cm);
+////		d = std::sqrt(d);
+
+////		// 2D distance below grouping threshold?
+////		return d < 350.0;
+
+//		const double dx = (x_cm - o.x_cm);
+//		const double dy = (y_cm - o.y_cm);
+//		const double dz = (z_nr - o.z_nr) * 3000;
+
+//		// get the 2D distance
+//		double d = dx*dx + dy*dy + dz*dz;
+//		d = std::sqrt(d);
+//		return d < 350.0;
+
+//	}
+
+
+
+//	MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}
+
+
+};
+
+#endif // MYSTATE_H
--- a/code/particles/MyTransition.h
+++ b/code/particles/MyTransition.h
@@ -0,0 +1,189 @@
+#ifndef MYTRANSITION_H
+#define MYTRANSITION_H
+
+#include <KLib/math/filter/particles/ParticleFilterTransition.h>
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Uniform.h>
+
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/grid/walk/GridWalk.h>
+
+#include "MyState.h"
+#include "MyControl.h"
+//#include "Helper.h"
+
+#include "../toni/barometric.h"
+
+#include "../MyGridNode.h"
+
+inline double sgn(double x){
+    return ((x>0)?1 : ((x<0)?-1 : 1));
+}
+
+class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
+
+private:
+
+	Grid<MyGridNode>& grid;
+	GridWalk<MyGridNode>& walker;
+
+
+	/** a simple normal distribution */
+	K::UniformDistribution distWalkStop;
+	K::NormalDistribution distWalk;
+	K::NormalDistribution distStop;
+
+
+    /** normal distribution for barometer */
+	K::NormalDistribution distBaro;
+
+
+public:
+
+	/**
+	 * ctor
+	 * @param choice the choice to use for randomly drawing nodes
+	 * @param fp the underlying floorplan
+	 */
+	MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
+		grid(grid), walker(walker),
+		distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
+
+		distWalkStop.setSeed(1234);
+		distWalk.setSeed(1234);
+		distStop.setSeed(1234);
+
+        distBaro.setSeed(5678);
+
+	}
+
+public:
+
+	uint64_t ts = 0;
+	uint64_t deltaMS = 0;
+
+	/** set the current time in millisconds */
+	void setCurrentTime(const uint64_t ts) {
+		if (this->ts == 0) {
+			this->ts = ts;
+			deltaMS = 0;
+		} else {
+			deltaMS = ts - this->ts;
+			this->ts = ts;
+		}
+	}
+
+	virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		for (K::Particle<MyState>& p : particles) {
+
+			// TODO: depending on the time since the last update
+			// random distance to move
+//			const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
+//			double dist_m = distance * deltaMS / 1000.0;
+
+//			if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
+
+			// update the old heading and the other old values
+			//p.state.walkState.heading = p.state.heading;
+			p.state.pOld = p.state.pCur;
+
+
+			// 10% stand still, 90% walk
+			double dist_m;
+			if (distWalkStop.draw() > 0.9) {
+				dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
+			} else {
+				dist_m = std::abs(distWalk.draw() * deltaMS / 1000.0);
+			}
+
+			// update cumulative distance
+			p.state.distanceWalkedCM += std::abs(dist_m * 100.0);
+
+			// find the node (square) the particle is within
+			// just to be safe, we round z to the nearest floor
+			//Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr));
+			const MyGridNode* src = p.state.walkState.node;
+
+			// might happen during initialization:
+			// the particle is nowhere near the grid.. replace it with a random on on the grid
+			// alternative: just ignore.. resampling will fix this issue quickly ;)
+//			if (!src) {
+//				auto it = graph->getNodes().begin();
+//				std::advance(it, rand() % graph->getNodes().size());
+//				src = it->second;
+//			}
+
+			// get new destination
+			//const Node3* dst = choice->getTarget(src, p.state, dist_m);
+			p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );
+
+			// randomly move the particle within its target grid (box)
+			// (z remains unchanged!)
+			const int grid_size_cm = grid.getGridSize_cm();
+
+			// new position (x,y) is randomly distributed within the target node
+			Point3 noise = Point3(0,0,0);		// TODO
+			p.state.pCur = (Point3) *p.state.walkState.node + noise;
+
+
+            // --- ATTENTION HORRIBLE CODE INCOMING. ---
+
+//            //how many floors are changed? and in what direction (given by the sign)
+//            double numFloorChanged = p.state.z_nr_old - p.state.z_nr;
+
+//            //The height of the single floor levels.
+//            const static double floor_height[3] = {4.1, 3.4, 3.4};
+
+//            //update barometer
+//            if(USE_BAROMETRIC_FORMULAR){
+//                //height the particle has climbed.
+//                double h_1 = 0.0;
+//                double mu = 0.0;
+
+//                //we need only the sign of the floors changed, since the pressure change between the floors
+//                //is calculated within s_getAtmosphericPressure
+//                numFloorChanged = sgn(numFloorChanged);
+
+//                for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
+//                   h_1 += floor_height[i];
+//                }
+
+//                {
+//                    // use the barometric formular to calculate the relative pressure
+//                    // the calculation is done assuming sea level height at every floor.
+//                    double mslp = BarometricFormular::s_getSeaLevelPressure();
+//                    double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
+//                    mu = std::abs(mslp - pressure);
+//                }
+
+//                if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
+//					p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
+//                else
+//                    p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
+//            }
+//            else{
+//                if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
+//                    p.state.hPa += numFloorChanged * distBaro.draw();
+//                else
+//                    p.state.hPa = numFloorChanged * distBaro.draw();
+//            }
+
+//			// sanity check
+//			if (p.state.heading != p.state.heading) {throw "detected NaN";}
+//            if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
+//			if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
+//			if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
+
+//			// ensure p.state.z_nr IS discreet
+//            if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}
+
+		}
+
+	}
+
+
+};
+
+#endif // MYTRANSITION_H
--- a/code/particles/MyTransitionSimple.h
+++ b/code/particles/MyTransitionSimple.h
@@ -0,0 +1,83 @@
+#ifndef MYTRANSITIONSIMPLE_H
+#define MYTRANSITIONSIMPLE_H
+
+#include <KLib/math/filter/particles/ParticleFilterTransition.h>
+#include <KLib/math/distribution/Normal.h>
+#include "MyState.h"
+#include "MyControl.h"
+
+class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
+
+private:
+
+	/** a simple normal distribution */
+	K::NormalDistribution distX;
+	K::NormalDistribution distY;
+	K::NormalDistribution distZ;
+	K::NormalDistribution distBaro;
+
+public:
+
+	/** ctor */
+	MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
+		distX.setSeed(1234);
+		distY.setSeed(1235);
+		distZ.setSeed(1236);
+		distBaro.setSeed(5678);
+	}
+
+public:
+
+	uint64_t ts = 0;
+	uint64_t deltaMS = 0;
+
+	/** set the current time in millisconds */
+	void setCurrentTime(const uint64_t ts) {
+		if (this->ts == 0) {
+			this->ts = ts;
+			deltaMS = 0;
+		} else {
+			deltaMS = ts - this->ts;
+			this->ts = ts;
+		}
+	}
+
+	virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		for (K::Particle<MyState>& p : particles) {
+
+            p.state.heading_old = p.state.heading;
+            p.state.x_cm_old = p.state.x_cm;
+            p.state.y_cm_old = p.state.y_cm;
+            p.state.z_nr_old = p.state.z_nr;
+
+			p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
+			p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
+			p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
+			p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old,  p.state.x_cm, p.state.y_cm);
+
+//            if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
+//            if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
+//            if (p.state.x_cm < 0) {p.state.x_cm = 0;}
+//            if (p.state.y_cm < 0) {p.state.y_cm = 0;}
+
+
+
+            //update barometer
+			p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
+
+			// update walked distance (2D)
+			const double dx = p.state.x_cm_old - p.state.x_cm;
+			const double dy = p.state.y_cm_old - p.state.y_cm;
+			p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
+
+
+		}
+
+	}
+
+
+};
+
+
+#endif // MYTRANSITIONSIMPLE_H
--- a/code/particles/P3.h
+++ b/code/particles/P3.h
@@ -0,0 +1,31 @@
+#ifndef P3_H
+#define P3_H
+
+struct P3 {
+
+	double x;
+	double y;
+	double z;
+
+	P3() : x(0), y(0), z(0) {;}
+	P3(const double x, const double y, const double z) : x(x), y(y), z(z) {;}
+
+	P3 operator - (const P3& o) const {
+		return P3(x-o.x, y-o.y, z-o.z);
+	}
+
+	P3 operator + (const P3& o) const {
+		return P3(x+o.x, y+o.y, z+o.z);
+	}
+
+	P3 operator * (const double v) const {
+		return P3(x*v, y*v, z*v);
+	}
+
+	double getLength(const double floorHeight_cm) const {
+		 return std::sqrt(x*x + y*y + z*floorHeight_cm*z*floorHeight_cm);
+	}
+
+};
+
+#endif // P3_H
--- a/code/toni/BarometerEvaluation.h
+++ b/code/toni/BarometerEvaluation.h
@@ -2,10 +2,10 @@

 #include "../particles/MyState.h"
 #include "BarometerObservation.h"
-#include "barometric.h"
-#include <KLib/math/distribution/Normal.h>
+//#include "barometric.h"
+//#include <KLib/math/distribution/Normal.h>

-double g_BarometerObservation = 0.0;
+static constexpr double g_BarometerObservation = 0.0;

 class BarometerEvaluation {

@@ -13,46 +13,50 @@ public:

 	double getProbability(const MyState& state, const BarometerObservation* obs) const {

-        //rho_z
-        double barometerSigma = 0.3;
+		return 1;

-        //The height of the single floor levels.
-        const static double floor_height[3] = {4.1, 3.4, 3.4};
+//        //rho_z
+//        double barometerSigma = 0.3;

-        if(USE_BAROMETRIC_FORMULAR){
-            //height the particle has climbed.
-            double h_1 = 0.0;
-            for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
-               h_1 += floor_height[i];
-            }
+//        //The height of the single floor levels.
+//        const static double floor_height[3] = {4.1, 3.4, 3.4};

-            if(h_1 != 0.0){
-                // use the barometric formular to calculate the relative pressure
-                // the calculation is done assuming sea level height at every floor.
-                double mslp = BarometricFormular::s_getSeaLevelPressure();
-                double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
-                barometerSigma = std::abs(mslp - pressure);
-            }
+//        if(USE_BAROMETRIC_FORMULAR){
+//            //height the particle has climbed.
+//            double h_1 = 0.0;
+//            for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
+//               h_1 += floor_height[i];
+//            }

-        }
-        else {
-            // constant value for sigma if we assume all floors are same in height
-            barometerSigma = 0.30 / 1.0; //hPa
-        }
+//            if(h_1 != 0.0){
+//                // use the barometric formular to calculate the relative pressure
+//                // the calculation is done assuming sea level height at every floor.
+//                double mslp = BarometricFormular::s_getSeaLevelPressure();
+//                double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
+//                barometerSigma = std::abs(mslp - pressure);
+//            }

-        // evaluate the current particle with a normal distribution
-		const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
+//        }
+//        else {
+//            // constant value for sigma if we assume all floors are same in height
+//            barometerSigma = 0.30 / 1.0; //hPa
+//        }

-        //Just for the visualization. i'm a lazy bastard
-        g_BarometerObservation = obs->hpa;
+//        // evaluate the current particle with a normal distribution
+//		const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);

-        assert(barometerProbability == barometerProbability);
-        assert(state.hPa == state.hPa);
-        assert(obs->hpa == obs->hpa);
+//        //Just for the visualization. i'm a lazy bastard
+//        g_BarometerObservation = obs->hpa;

-        //std::cout << barometerProbability << std::endl;
+//        assert(barometerProbability == barometerProbability);
+//        assert(state.hPa == state.hPa);
+//        assert(obs->hpa == obs->hpa);
+
+//        //std::cout << barometerProbability << std::endl;
+
+//        return  pow(2.0, barometerProbability);
+//        //return  barometerProbability;
+
+	}

-        return  pow(2.0, barometerProbability);
-        //return  barometerProbability;
-    }
 };
--- a/code/toni/BarometerSensorReader.h
+++ b/code/toni/BarometerSensorReader.h
@@ -2,7 +2,7 @@

 #include "circular.h"
 #include "BarometerObservation.h"
-#include "../SensorReader.h"
+#include "../reader/SensorReader.h"
 #include <sstream>

 //circular_buffer<double> measurementHistory(1000);