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

2017-11-08 18:11:01 +01:00
parent ce558d0c01 08d8292976
commit 72932ad90f
55 changed files with 4010 additions and 520 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -65,7 +65,7 @@ else()
 # system specific compiler flags
 ADD_DEFINITIONS(

-    -std=gnu++11
+	-std=gnu++11

 	-Wall
 	-Werror=return-type
@@ -95,11 +95,16 @@ ADD_EXECUTABLE(
 	${SOURCES}
 )

+SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3)
+INCLUDE_DIRECTORIES(/usr/include/libnl3/)
+SET(EXTRA_LIBS ${EXTRA_LIBS} iw)
+
 # needed external libraries
 TARGET_LINK_LIBRARIES(
 	${PROJECT_NAME}
 	gtest
 	pthread
+	${EXTRA_LIBS}
 )

 SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER})
--- a/data/Timestamp.h
+++ b/data/Timestamp.h
@@ -21,6 +21,7 @@ public:
 	/** empty ctor */
 	explicit Timestamp() : _ms(0) {;}

+
 	/** get timestamp from the given value which represents milliesconds */
 	static inline Timestamp fromMS(const int64_t ms)	{return Timestamp(ms);}

@@ -75,11 +76,14 @@ public:


 	Timestamp operator - (const Timestamp& o) const {return Timestamp(_ms - o._ms);}
+	Timestamp& operator -= (const Timestamp& o) {_ms += o._ms; return *this;}

 	Timestamp operator + (const Timestamp& o) const {return Timestamp(_ms + o._ms);}
+	Timestamp& operator += (const Timestamp& o) {_ms += o._ms; return *this;}

-	Timestamp operator * (const float val) const {return Timestamp(_ms * val);}
+	template <typename T> Timestamp operator * (const T val) const {return Timestamp(_ms * val);}

+	template <typename T> Timestamp operator / (const T val) const {return Timestamp(_ms / val);}

 //	/** cast to float */
 //	operator float () const {return sec();}
@@ -87,4 +91,22 @@ public:

 };

+namespace std {
+	template<> class numeric_limits<Timestamp> {
+	public:
+		static Timestamp min() {
+			const int64_t minVal = std::numeric_limits<int64_t>::min();
+			return Timestamp::fromMS(minVal);
+		}
+		static Timestamp lowest() {
+			const int64_t minVal = std::numeric_limits<int64_t>::min();
+			return Timestamp::fromMS(minVal);
+		}
+		static Timestamp max() {
+			const int64_t maxVal = std::numeric_limits<int64_t>::max();
+			return Timestamp::fromMS(maxVal);
+		}
+	};
+}
+
 #endif // TIMESTAMP_H
--- a/geo/Circle2.h
+++ b/geo/Circle2.h
@@ -5,6 +5,7 @@

 #include "Point2.h"
 #include "Ray2.h"
+#include "Line2.h"

 #include "../Assertions.h"

@@ -65,6 +66,24 @@ public:

 	}

+	/** does this circle intersect with the given ray? */
+	bool intersects(const Line2 line) const {
+
+		// https://math.stackexchange.com/questions/311921/get-location-of-vector-circle-intersection
+		Point2 dir = line.p2 - line.p1;
+		Point2 start = line.p1;
+		const float a = dir.x*dir.x + dir.y*dir.y;
+		const float b = 2 * dir.x * (start.x-center.x) + 2 * dir.y * (start.y - center.y);
+		const float c = (start.x-center.x) * (start.x-center.x) + (start.y - center.y)*(start.y - center.y) - radius*radius;
+		const float discr = b*b - 4*a*c;
+
+		if (discr < 0) {return false;}
+
+		const float t = (2*c) / (-b + std::sqrt(discr));
+		return (t <= 1) && (t >= 0);
+
+	}
+

 	/** configure this sphere to contain the given point-set */
 	void adjustToPointSet(const std::vector<Point2>& lst) {
--- a/geo/Heading.h
+++ b/geo/Heading.h
@@ -27,6 +27,11 @@ public:
 		Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds");
 	}

+	/** ctor from(x,y) -> to(x,y) */
+	Heading(const Point2 from, const Point2 to) : Heading(from.x, from.y, to.x, to.y) {
+		;
+	}
+
 	/** POSITIVE angular difference [0:PI] */
 	float getDiffHalfRAD(const Heading other) const {
 		return Angle::getDiffRAD_2PI_PI(rad, other.rad);
@@ -77,10 +82,10 @@ public:
 	}


-        Heading& operator = (const float _rad) {
-            rad = _rad;
-            return *this;
-        }
+		Heading& operator = (const float _rad) {
+			rad = _rad;
+			return *this;
+		}

 	/** compare two headings */
 	bool operator == (const Heading other) const {return rad == other.rad;}
@@ -95,6 +100,11 @@ public:

 	float getRAD() const {return rad;}

+	/** convert heading into a direction-vector */
+	Point2 asVector() const {
+		return Point2(std::cos(rad), std::sin(rad));
+	}
+
 //	/** get a random heading */
 //	static Heading rnd() {
 //		static std::minstd_rand gen(1234);
--- a/grid/GridNode.h
+++ b/grid/GridNode.h
@@ -64,6 +64,9 @@ public:
 	/** get the node's index within its grid */
 	int getIdx() const {return _idx;}

+	/** get the x-th neighbor's index within its grid */
+	int getNeighborIdx(const int x) const {return _neighbors[x];}
+
 	/** get the number of neighbors for this node */
 	int getNumNeighbors() const {return _numNeighbors;}

--- a/grid/factory/v2/GridFactory.h
+++ b/grid/factory/v2/GridFactory.h
@@ -12,6 +12,7 @@
 #include "Elevators.h"

 #include "../../../geo/Units.h"
+#include "../../../geo/Circle2.h"
 #include "../../GridNodeBBox.h"
 #include "../../Grid.h"

@@ -583,7 +584,10 @@ private:
 				if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;}

 			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
-				throw Exception("should not happen");
+				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
+				Circle2 circ(circle->center, circle->radius);
+				if (circ.intersects(lineNodes)) {return true;}
+				//throw Exception("should not happen");

 			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
 				// DOORS ARE NOT AN OBSTACLE
--- a/grid/walk/v2/modules/WalkModuleActivityControl.h
+++ b/grid/walk/v2/modules/WalkModuleActivityControl.h
@@ -6,7 +6,7 @@
 #include "../../../../geo/Heading.h"
 #include "../../../../math/Distributions.h"

-#include "../../../../sensors/pressure/ActivityButterPressure.h"
+#include "../../../../sensors/activity/ActivityButterPressure.h"


 /**
@@ -74,20 +74,20 @@ public:
 		const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;

 		// TODO: general activity enum and activity-detector based on barometer and accelerometer?
-		const ActivityButterPressure::Activity activity = ctrl->activity;
+        const Activity activity = ctrl->activity;

 //		const float kappa = 0.75;

 		switch (activity) {
-			case ActivityButterPressure::Activity::DOWN:
+            case Activity::WALKING_DOWN:
 			    if (deltaZ_cm < 0)	{return 0.60;}
 				if (deltaZ_cm == 0)	{return 0.25;}
 		                            {return 0.15;}
-			case ActivityButterPressure::Activity::UP:
+            case Activity::WALKING_UP:
 			    if (deltaZ_cm > 0)	{return 0.60;}
 				if (deltaZ_cm == 0)	{return 0.25;}
 		                            {return 0.15;}
-			case ActivityButterPressure::Activity::STAY:
+            case Activity::WALKING:
 			    if (deltaZ_cm == 0) {return 0.60;}
 		                            {return 0.40;}
 //			case ActivityButterPressure::Activity::DOWN:
--- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
+++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
@@ -48,6 +48,7 @@ public:
        if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}

        // adjust the state's heading using the control-data
+        //TODO: if motionaxis detects landscaping, do not do this!
        state.heading.direction += ctrl->turnSinceLastTransition_rad + var;

        //set kappa of mises
--- a/grid/walk/v3/Helper.h
+++ b/grid/walk/v3/Helper.h
@@ -0,0 +1,295 @@
+#ifndef INDOOR_GW3_HELPER_H
+#define INDOOR_GW3_HELPER_H
+
+#include "../../../nav/dijkstra/Dijkstra.h"
+#include "../../Grid.h"
+
+#include "Structs.h"
+
+#include <vector>
+#include <set>
+
+#include <KLib/math/random/RandomIterator.h>
+
+//#define SHOW_DEBUG_PLOT
+
+#ifdef SHOW_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
+#endif
+
+namespace GW3 {
+
+
+	/** get an iterator over all nodes reachable from the given start */
+	template <typename Node> class ReachableIteratorSorted {
+
+	const Grid<Node>& grid;
+	const Node& start;
+
+	struct Next {
+
+		uint32_t idx;
+		float distToStart;
+
+		Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;}
+
+		/** compare by weight. same weight? : compare by pointer */
+		bool operator < (const Next& o) const	{
+			return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx);
+		}
+
+	};
+
+	Node* curNode = nullptr;
+	std::unordered_set<uint32_t> visited;
+	std::set<Next> toVisit;
+
+	public:
+
+		ReachableIteratorSorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
+			toVisit.insert(Next(start.getIdx(),0));
+		}
+
+		bool hasNext() const {
+			return !toVisit.empty();
+		}
+
+		const Node& next() {
+
+			const Next cur = *(toVisit.begin());				// visit from inside out (needed for correct distance)
+			toVisit.erase(toVisit.begin());
+			visited.insert(cur.idx);
+
+			const Node& curNode = grid[cur.idx];
+
+			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+				const int neighborIdx = curNode.getNeighborIdx(i);
+				const Node& neighbor = grid[neighborIdx];
+				const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor);
+
+				// not yet reached -> store distance
+				if (visited.find(neighborIdx) == visited.end()) {
+					toVisit.insert(Next(neighborIdx, newDist));
+				}
+
+			}
+
+			// done
+			return curNode;
+
+		}
+
+	};
+
+	/** get an iterator over all nodes reachable from the given start */
+	template <typename Node> class ReachableIteratorUnsorted {
+
+	const Grid<Node>& grid;
+	const Node& start;
+
+	Node* curNode = nullptr;
+	std::unordered_set<uint32_t> visited;
+	std::vector<uint32_t> toVisit;
+
+	public:
+
+		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
+			toVisit.push_back(start.getIdx());
+		}
+
+		bool hasNext() const {
+			return !toVisit.empty();
+		}
+
+		const Node& next(const std::function<bool(const Node&)>& skip) {
+
+			const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance)
+			toVisit.erase(toVisit.begin());
+			visited.insert(curIdx);
+
+			const Node& curNode = grid[curIdx];
+
+			for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+				const int neighborIdx = curNode.getNeighborIdx(i);
+				const Node& neighbor = grid[neighborIdx];
+
+				// not yet reached -> store distance
+				if (!skip(neighbor)) {
+					if (visited.find(neighborIdx) == visited.end()) {
+						toVisit.push_back(neighborIdx);
+					}
+				}
+
+			}
+
+			// done
+			return curNode;
+
+		}
+
+	};
+
+
+	struct ReachableSettings {
+
+		float dist_m;
+		bool limitDistance = true;
+
+		Heading heading = Heading(0);
+		float maxHeadingDiff_rad;
+		bool limitHeading = false;
+
+	};
+
+
+	template <typename Node> class Helper {
+
+	public:
+
+		static GridPoint p3ToGp(const Point3 p) {
+			const Point3 p100 = p*100;
+			return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
+		}
+
+		static Point3 gpToP3(const GridPoint gp) {
+			return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
+		}
+
+		/** does the given grid-node contain the provided point-in-question? */
+		static bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
+			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
+			const float d = gridSize_m / 2.0f;
+			const Point2 pMin = n->inMeter().xy() - Point2(d, d);
+			const Point2 pMax = n->inMeter().xy() + Point2(d, d);
+			const BBox2 bbox(pMin, pMax);
+			return bbox.contains(pt);
+		}
+
+		/** does one of the given grid-nodes contains the provided point-in-question? */
+		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
+			for (const Node* n : nodes) {
+				if (contains(grid, n, pt)) {return n;}
+			}
+			return nullptr;
+		}
+
+		/** get all possible walks from start within a given region */
+		static Walks<Node> getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {
+
+			struct Access {
+				Grid<Node>& grid;
+				Access(Grid<Node>& grid) : grid(grid) {;}
+				int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
+				Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
+				float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
+			} acc(grid);
+
+			const float addDist_m = grid.getGridSize_cm() / 100.0f;
+			const float maxDist_m = dist_m * 1.1 + addDist_m;
+			Dijkstra<Node> dijkstra;
+			dijkstra.build(start, nullptr, acc, maxDist_m);
+
+			const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();
+
+			Walks<Node> walks;
+
+			for (const auto& it : nodes) {
+				Walk<Node> walk;
+				DijkstraNode<Node>* node = it.second;
+				do {
+					const Node* gridNode = node->element;
+					walk.insert(walk.begin(), gridNode);		// push_front() as dijkstra is inverted
+					node = node->previous;
+				} while (node);
+				walks.push_back(walk);
+			}
+
+			return walks;
+
+		}		
+
+		/** get all reachable nodes that are within a given range */
+		static Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const ReachableSettings& set ) {
+
+			//auto tStart = std::chrono::system_clock::now();
+
+			Nodes<Node> res;
+			std::unordered_map<uint32_t, float> distances;
+			std::vector<uint32_t> toVisit;						// std::queue was only barely faster: 900 vs 880 microseconds
+
+			toVisit.push_back(start->getIdx());
+			distances[start->getIdx()] = 0.0f;
+
+			#ifdef SHOW_DEBUG_PLOT
+				static K::Gnuplot gp;
+				K::GnuplotPlot plot;
+				K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1);
+				plot.add(&pts1);
+			#endif
+
+			while (!toVisit.empty()) {
+
+				const int curIdx = toVisit.front();				// visit from inside out (needed for correct distance)
+				toVisit.erase(toVisit.begin());
+
+				const Node& curNode = grid[curIdx];
+				const float curDistance = distances[curIdx];
+				res.push_back(&curNode);						// remember for output
+
+				#ifdef SHOW_DEBUG_PLOT
+					pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
+					gp.draw(plot);
+					gp.flush();
+				#endif
+
+				for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
+
+					const int neighborIdx = curNode.getNeighborIdx(i);
+					const Node& neighbor = grid[neighborIdx];
+					const float addDist = neighbor.getDistanceInMeter(curNode);
+					const float totalDist = curDistance + addDist;
+
+					// this is like in dijkstra. keep the smallest distance to reach a node:
+
+					// not yet reached -> store distance
+					if (distances.find(neighborIdx) == distances.end()) {
+						distances[neighborIdx] = totalDist;
+
+						if (set.limitDistance) {
+							if (totalDist > set.dist_m) {continue;}
+						}
+						if (set.limitHeading) {
+							const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm);		// angle between start and current node
+							const float diff = head.getDiffHalfRAD(set.heading);							// difference between above angle and requested angle
+							if (diff > set.maxHeadingDiff_rad) {continue;}									// more than 90 degree difference? -> ignore
+						}
+						toVisit.push_back(neighborIdx);	// needs a visit? (still some distance left)
+
+
+					// reached earlier but found shorter way
+					} else if (distances[neighborIdx] > totalDist) {
+						distances[neighborIdx] = totalDist;
+
+					}
+
+				}
+
+			}
+
+			//auto tEnd = std::chrono::system_clock::now();
+			//auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tEnd - tStart);
+			//std::cout << elapsed.count() << std::endl;
+
+			return res;
+
+		}
+
+	};
+
+}
+
+#endif // INDOOR_GW3_HELPER_H
--- a/grid/walk/v3/Structs.h
+++ b/grid/walk/v3/Structs.h
@@ -0,0 +1,47 @@
+#ifndef INDOOR_GW3_STRUCTS_H
+#define INDOOR_GW3_STRUCTS_H
+
+#include "../../../geo/Heading.h"
+#include "../../../geo/Point3.h"
+#include <vector>
+
+namespace GW3 {
+
+	/** paremters for the walk */
+	struct WalkParams {
+
+		Point3 start;
+		float distance_m;
+		Heading heading = Heading(0);
+
+		float lookFurther_m = 1.5;
+
+	};
+
+	/** result of the random walk */
+	struct WalkResult {
+		Point3 position;
+		Heading heading = Heading(0);
+		double probability = 1.0;
+	};
+
+	/** several nodes */
+	template <typename Node> struct Nodes : public std::vector<const Node*> {
+
+	};
+
+	/** one walk along several nodes */
+	template <typename Node> struct Walk : public std::vector<const Node*> {
+
+	};
+
+	/** several walks */
+	template <typename Node> struct Walks : public std::vector<Walk<Node>> {
+
+	};
+
+
+
+}
+
+#endif // INDOOR_GW3_STRUCTS_H
--- a/grid/walk/v3/WalkEvaluator.h
+++ b/grid/walk/v3/WalkEvaluator.h
@@ -0,0 +1,97 @@
+#ifndef INDOOR_GW3_WALKEVALUATOR_H
+#define INDOOR_GW3_WALKEVALUATOR_H
+
+#include "Structs.h"
+#include "Helper.h"
+
+#include "../../../math/Distributions.h"
+#include "../../../grid/Grid.h"
+
+namespace GW3 {
+
+	/** interface for all evaluators that return a probability for a given walk */
+	template <typename Node> class WalkEvaluator {
+
+	public:
+
+		/** get the probability for the given walk */
+		//virtual double getProbability(const Walk<Node>& walk) const = 0;
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
+
+	};
+
+
+	/** evaluate the grid-node-importance importance(end) */
+	template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
+
+		Grid<Node>* grid;
+
+	public:
+
+		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			(void) params;
+			(void) pStart;
+
+			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
+			const Node& node = grid->getNodeFor(gp);
+			const double p = node.getWalkImportance();
+			return std::pow(p,10);
+
+		}
+
+	};
+
+
+
+	/** evaluate the difference between head(start,end) and the requested heading */
+	template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
+
+		const double sigma;
+
+	public:
+
+		WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			(void) params;
+
+			if (pStart == pEnd) {
+				std::cout << "warn! start-position == end-positon" << std::endl;
+				return 0;
+			}
+
+			const Heading head(pStart.xy(), pEnd.xy());
+			const float diff = head.getDiffHalfRAD(params.heading);
+			//const float diff = Heading::getSignedDiff(params.heading, head);
+			return Distribution::Normal<double>::getProbability(0, sigma, diff);
+
+		}
+
+	};
+
+	/** evaluate the difference between distance(start, end) and the requested distance */
+	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
+
+		const double sigma;
+
+	public:
+
+		WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;}
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			const float walkedDistance_m = pStart.getDistance(pEnd);
+			return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+
+		}
+
+	};
+
+}
+
+#endif // INDOOR_GW3_WALKEVALUATOR_H
--- a/grid/walk/v3/WalkGenerator.h
+++ b/grid/walk/v3/WalkGenerator.h
@@ -0,0 +1,20 @@
+#ifndef INDOOR_GRIDWALKER3GENERATOR_H
+#define INDOOR_GRIDWALKER3GENERATOR_H
+
+#include "Structs.h"
+
+
+namespace GW3 {
+
+	class GridWalker3Generator {
+
+		virtual Walk getPossibleWalks() = 0;
+
+	};
+
+
+
+
+}
+
+#endif // INDOOR_GRIDWALKER3GENERATOR_H
--- a/grid/walk/v3/Walker.h
+++ b/grid/walk/v3/Walker.h
@@ -0,0 +1,422 @@
+#ifndef INDOOR_GW3_WALKER_H
+#define INDOOR_GW3_WALKER_H
+
+#include "../../../data/Timestamp.h"
+#include "../../../math/DrawList.h"
+#include "../../../math/Distributions.h"
+#include "../../../math/Stats.h"
+#include "../../../geo/Heading.h"
+#include "../../../math/stats/Variance.h"
+#include "../../../geo/BBox2.h"
+
+#include "../../Grid.h"
+
+#include "Helper.h"
+#include "Structs.h"
+#include "WalkEvaluator.h"
+
+namespace GW3 {
+
+	/**
+	 * modular grid-walker that takes various sub-components to determine
+	 * p(e) and thus randomly pick edges
+	 */
+	template <typename Node> class WalkerBase {
+
+	public:
+
+		/** get a new destination for the given start */
+		virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
+
+	};
+
+	template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
+
+		//RandomGenerator rnd;
+
+		std::vector<WalkEvaluator<Node>*> evals;
+
+	public:
+
+		/** make the code a little more readable */
+		using Helper = GW3::Helper<Node>;
+		using Walk = typename GW3::Walk<Node>;
+		using Walks = typename GW3::Walks<Node>;
+		using Nodes = typename GW3::Nodes<Node>;
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(WalkEvaluator<Node>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		/** perform the walk based on the configured setup */
+		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+
+			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+
+			static std::mt19937 rndGen;
+
+			const GridPoint gpStart = Helper::p3ToGp(params.start);
+			const Node* startNode = grid.getNodePtrFor(gpStart);
+
+			// include one additional grid-cell (increased distance)
+			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
+			ReachableSettings set;
+			set.limitDistance = true;
+			set.dist_m = params.distance_m + secBuffer_m;
+			set.limitHeading = false;
+			set.heading = params.heading;
+			set.maxHeadingDiff_rad = M_PI/2;
+			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+
+			WalkResult res;
+			res.heading = params.heading;
+			res.position = params.start;
+
+
+			const Point2 dir = res.heading.asVector();
+			const Point2 dst = params.start.xy() + (dir * params.distance_m);
+
+			// is dst reachable?
+			const Node* n = Helper::contains(grid, reachableNodes, dst);
+			if (n) {
+
+				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+				const GridPoint gp = Helper::p3ToGp(p3);
+
+				if (grid.hasNodeFor(gp)) {
+					res.position = p3;					// update position
+					//res.heading;						// keep as-is
+					//res.probability;					// keep as-is
+					return res;							// done
+
+				} else {
+					std::cout << "WARN dst not found" << std::endl;
+					//throw "should not happen";
+
+				}
+
+			}
+
+			// not found -> try random pick among all reachable nodes
+			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
+			std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
+
+			const Node* dstNode = reachableNodes[dNode(rndGen)];
+
+			// random position within destination-node
+			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
+			const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+
+			const Point3 start = params.start;
+			const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
+
+			double p = 1;
+			for (const WalkEvaluator<Node>* eval : evals) {
+				const double p1 = eval->getProbability(start, end, params);
+				p *= p1;
+			}
+
+			if (start == end) {
+				res.probability = 0;
+				return res;
+			}
+
+			res.heading = Heading(start.xy(), end.xy());
+			res.probability = p;
+			res.position = end;
+			return res;
+
+		}
+
+	};
+
+
+	/**
+	 * 1) get all reachable nodes from "start" (within a certain distance)
+	 * 2) randomly pick one of em
+	 * 3) scatter positon within the node's square -> end position
+	 * 4) evaluate the probability of walking from start -> end
+	 */
+	template <typename Node> class WalkerWeightedRandom : public WalkerBase<Node> {
+
+		std::vector<WalkEvaluator<Node>*> evals;
+
+	public:
+
+		/** make the code a little more readable */
+		using Helper = GW3::Helper<Node>;
+		using Walk = typename GW3::Walk<Node>;
+		using Walks = typename GW3::Walks<Node>;
+		using Nodes = typename GW3::Nodes<Node>;
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(WalkEvaluator<Node>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		/** perform the walk based on the configured setup */
+		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
+
+			Assert::isNot0(params.distance_m, "walking distance must be > 0");
+
+			static std::minstd_rand rndGen;
+
+			const GridPoint gpStart = Helper::p3ToGp(params.start);
+			const Node* startNode = grid.getNodePtrFor(gpStart);
+
+//			// include one additional grid-cell (increased distance)
+//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
+
+//			ReachableSettings set;
+//			set.limitDistance = true;
+//			set.limitHeading = true;
+//			set.dist_m = params.distance_m + secBuffer_m;
+//			set.heading = params.heading;
+//			set.maxHeadingDiff_rad = M_PI/2;
+//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
+
+			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
+			//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
+
+
+			Point3 best;
+			double bestP = 0;
+//			DrawList<Point3> drawer;
+
+			const Point3 start = params.start;
+
+			// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
+			//for (int i = 0; i < 500; ++i) {
+
+			//	const Node* dstNode = reachableNodes[dNode(rndGen)];
+
+			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
+
+
+
+			ReachableIteratorUnsorted<Node> ri(grid, *startNode);
+			const float maxDist = params.distance_m * 1.25 + gridSize_m;
+
+			auto skip = [&] (const Node& n) {
+				const float dist_m = n.getDistanceInMeter(gpStart);
+				return dist_m > maxDist;
+			};
+
+			//for (const Node* dstNode : reachableNodes) {
+			while(ri.hasNext()) {
+
+				const Node* dstNode = &ri.next(skip);
+//				const float dist_m = dstNode->getDistanceInMeter(gpStart);
+
+//				if (dist_m > maxDist) {
+//					break;
+//				}
+
+				for (int i = 0; i < 25; ++i) {
+
+					// random position within destination-node
+					const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
+
+					// destination = node-center + offset (within the node's bbox)
+					const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
+
+					// sanity check
+					if (start == end) {continue;}
+					if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
+						std::cout << "random destination is not part of the grid" << std::endl;
+						continue;
+					}
+
+					//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
+
+					double p = 1;
+					for (const WalkEvaluator<Node>* eval : evals) {
+						const double p1 = eval->getProbability(start, end, params);
+						p *= p1;
+					}
+
+					if (p > bestP) {bestP = p; best = end;}
+					//drawer.add(end, p);
+
+				}
+
+			}
+
+			//const Point3 end = drawer.get();
+			const Point3 end = best;
+			WalkResult res;
+			if (start == end) {
+				res.probability = 0;
+			} else {
+				res.heading = Heading(start.xy(), end.xy());
+				res.probability = bestP;
+			}
+			res.position = end;
+			return res;
+
+		}
+
+	};
+
+
+
+
+		//const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
+
+
+
+
+	//		Distribution::Normal<float> dDist(1, 0.02);
+	//		Distribution::Normal<float> dHead(0, 0.01);
+
+//			int cnt = 0;
+//			while(true) {
+
+//				const Point2 dir = res.heading.asVector();
+//				const Point2 dst = params.start.xy() + (dir * realDist_m);
+
+//				// is dst reachable?
+//				const Node* n = Helper::contains(grid, reachableNodes, dst);
+//				if (n) {
+
+//					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+//					const GridPoint gp = Helper::p3ToGp(p3);
+
+//					if (grid.hasNodeFor(gp)) {
+//						res.position = p3;					// update position
+//						res.heading;						// keep as-is
+//						res.probability;					// keep as-is
+//						return res;							// done
+//					} else {
+//						std::cout << "WARN dst not found" << std::endl;
+//						//throw "should not happen";
+//					}
+
+//				}
+
+//				// reduce probability with every new run
+//				++cnt;
+//				res.probability /= 2;
+
+//				// before trying again, modify distance and angle
+//				//if (1 == 1) {
+//					res.heading = params.heading + dHead.draw() * cnt;
+//					realDist_m = params.distance_m * dDist.draw();// * cnt;
+//				//}
+
+//				// reached max retries?
+//				if (cnt > 8) {
+//					res.position = params.start;	// reset
+//					res.heading = params.heading;	// reset
+//					res.probability = 1e-50;			// unlikely
+//					return res;
+//				}	// did not work out....
+
+//			}
+
+		//	throw "error";
+
+		//}
+
+
+	//	const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {
+
+	//		const Node* startNode = grid.getNodePtrFor(start);
+
+	//		Heading desiredHeading = ctrl.heading;
+
+	//		DrawList<Walk> weightedWalks;
+
+	//		const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
+	//		for (const Walk& walk : walks) {
+	//			const double prob = eval(walk, desiredHeading, dist_m);
+	//			weightedWalks.add(walk, prob);
+	//		}
+
+
+	//		Walk res = weightedWalks.get();
+	//		const Node* dst = res.back();
+	//		return dst;
+
+	//	}
+
+
+
+
+
+//		double evalDistance(const Walk& w, const float desiredDist) const {
+//			const Node* nStart = w.front();
+//			const Node* nEnd = w.back();
+//			const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
+//			return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
+//		}
+
+//		double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
+//			const Node* nStart = w.front();
+//			const Node* nEnd = w.back();
+//			if (nStart == nEnd) {
+//				std::cout << "warn! start == end" << std::endl;
+//				return 0;
+//			}
+//			Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
+//			const float diff = head.getDiffHalfRAD(desiredHeading);
+//			return Distribution::Normal<double>::getProbability(0, 0.3, diff);
+//		}
+
+//		double evalHeadingChanges(const Walk& w) const {
+//			Stats::Variance<float> var;
+//			for (int i = 0; i < w.size()-2; ++i) {
+//				const Node* n0 = w[i+0];
+//				const Node* n1 = w[i+1];
+//				const Node* n2 = w[i+2];
+//				Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
+//				Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
+//				const float diff = h1.getDiffHalfRAD(h2);
+//				var.add(diff);
+//			}
+//			const float totalVar = var.get();
+//			return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
+//		}
+
+//		double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const {
+
+//			return 1.0
+//					* evalHeadingStartEnd(w, desiredHeading)
+//					* evalDistance(w, desiredDistance)
+//					// * evalHeadingChanges(w);
+//			;
+
+//		}
+
+
+
+
+	//	Walk getRandomWalk2(Grid<Node>& grid, const Node* start, const float dist_m) const {
+
+	//		Walk walk;
+
+	//		float dist = 0;
+
+	//		const Node* cur = start;
+	//		while(true) {
+
+	//			walk.push_back(cur);
+	//			if (dist > dist_m) {break;}
+
+	//			const int numNeighbors = cur->getNumNeighbors();
+	//			//std::cout << "neighbors: " << numNeighbors << std::endl;
+	//			int idx = rand() % numNeighbors;
+	//			const Node* next = &grid.getNeighbor(*cur, idx);
+	//			dist += next->inMeter().getDistance(cur->inMeter());
+	//			cur = next;
+
+	//		}
+
+	//		return walk;
+
+	//	}
+
+}
+
+#endif // INDOOR_GW3_WALKER_H
--- a/main.cpp
+++ b/main.cpp
@@ -10,8 +10,81 @@ class Test : public GridPoint {

 #include "tests/Tests.h"

+#include "sensors/radio/scan/WiFiScanLinux.h"
+#include "sensors/radio/VAPGrouper.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+
+void wifi() {
+
+	K::Gnuplot gp;
+	K::GnuplotPlot plot;
+	std::vector<K::GnuplotPlotElementLines> lines; lines.resize(5);
+	std::vector<K::GnuplotPlotElementPoints> points; points.resize(5);
+
+	for (int i = 0; i < points.size(); ++i) {
+		plot.add(&points[i]);
+		plot.add(&lines[i]);
+		points[i].setPointSize(1);
+		points[i].setPointType(7);
+		lines[i].getStroke().setWidth(2);
+	}
+	points[0].getColor().setHexStr("#ff6666");	lines[0].getStroke().getColor().setHexStr("#ff0000");
+	points[1].getColor().setHexStr("#cccc66");	lines[1].getStroke().getColor().setHexStr("#cccc00");
+	points[2].getColor().setHexStr("#66cccc");	lines[2].getStroke().getColor().setHexStr("#00cccc");
+	points[3].getColor().setHexStr("#6666ff");	lines[3].getStroke().getColor().setHexStr("#0000ff");
+	points[4].getColor().setHexStr("#cc66cc");	lines[4].getStroke().getColor().setHexStr("#cc00cc");
+
+	VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::AVERAGE, 1);
+
+	const std::string dev = "wlp0s20u2u4";
+	WiFiScanLinux scanner(dev);
+
+	Timestamp start = Timestamp::fromUnixTime();
+
+	for (int i = 0; i < 500; ++i) {
+
+		WiFiMeasurements mes = scanner.scan();
+		for (const WiFiMeasurement& m : mes.entries) {
+
+			K::GnuplotPoint2 gp((m.getTimestamp()-start).ms(), m.getRSSI());
+			std::string mac = m.getAP().getMAC().asString().substr(0,14);
+
+			if (mac == "5C:CF:7F:C3:F9") {points[0].add(gp);}
+			if (mac == "18:FE:34:E1:2B") {points[1].add(gp);}
+			if (mac == "60:01:94:03:16") {points[2].add(gp);}
+
+		}
+
+		WiFiMeasurements mes2 = vap.group(mes);
+		for (const WiFiMeasurement& m : mes2.entries) {
+
+			K::GnuplotPoint2 gp((m.getTimestamp()-start).ms(), m.getRSSI());
+			std::string mac = m.getAP().getMAC().asString().substr(0,14);
+
+			if (mac == "5C:CF:7F:C3:F9") {lines[0].add(gp);}
+			if (mac == "18:FE:34:E1:2B") {lines[1].add(gp);}
+			if (mac == "60:01:94:03:16") {lines[2].add(gp);}
+
+		}
+
+		gp.draw(plot);
+		gp.flush();
+
+	}
+
+
+
+
+}
+
 int main(int argc, char** argv) {

+	wifi(); return 0;
+

 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
@@ -25,8 +98,8 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
 	//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";

-    //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
-    //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
+	//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
+	//::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";


 	//::testing::GTEST_FLAG(filter) = "*Offline.readWrite*";
@@ -36,12 +109,14 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Matrix4*";
 	//::testing::GTEST_FLAG(filter) = "*Sphere3*";

-	::testing::GTEST_FLAG(filter) = "Geo_*";
+    ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*";
+	//::testing::GTEST_FLAG(filter) = "Timestamp*";
+
 	//::testing::GTEST_FLAG(filter) = "*RayTrace3*";
 	//::testing::GTEST_FLAG(filter) = "*BVH*";


-    //::testing::GTEST_FLAG(filter) = "*Barometer*";
+	//::testing::GTEST_FLAG(filter) = "*Barometer*";
 	//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";

 	//::testing::GTEST_FLAG(filter) = "Heading*";
--- a/math/Interpolator.h
+++ b/math/Interpolator.h
@@ -56,6 +56,11 @@ public:

 	}

+	/** get a list of all raw entries */
+	const std::vector<InterpolatorEntry>& getEntries() const {
+		return entries;
+	}
+
 protected:

 	/** special interpolation for the timestamp class */
--- a/math/Matrix3.h
+++ b/math/Matrix3.h
@@ -0,0 +1,175 @@
+#ifndef INDOOR_MATH_MATRIX3_H
+#define INDOOR_MATH_MATRIX3_H
+
+
+#include <initializer_list>
+#include <cmath>
+
+class Matrix3 {
+
+private:
+
+    float data[9];
+
+public:
+
+    Matrix3(std::initializer_list<float> lst) {
+        int idx = 0;
+        for (float f : lst) {
+            data[idx] = f; ++idx;
+        }
+    }
+
+    static Matrix3 identity() {
+		return Matrix3( {1,0,0, 0,1,0, 0,0,1} );
+    }
+
+//    static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) {
+//        return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI);
+//    }
+
+//    static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) {
+
+//        const float g = radX; const float b = radY; const float a = radZ;
+//        const float a11 = std::cos(a)*std::cos(b);
+//        const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
+//        const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
+//        const float a21 = std::sin(a)*std::cos(b);
+//        const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
+//        const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
+//        const float a31 = -std::sin(b);
+//        const float a32 = std::cos(b)*std::sin(g);
+//        const float a33 = std::cos(b)*std::cos(g);
+//        return Matrix4({
+//            a11,	a12,	a13,	0,
+//            a21,	a22,	a23,	0,
+//            a31,	a32,	a33,	0,
+//            0,		0,		0,		1
+//        });
+
+//    }
+
+    static Matrix3 getTranslation(const float x, const float y) {
+        return Matrix3({
+            1, 0, x,
+            0, 1, y,
+            0, 0, 1,
+        });
+    }
+
+    static Matrix3 getScale(const float x, const float y) {
+        return Matrix3({
+            x, 0, 0,
+            0, y, 0,
+            0, 0, 1,
+        });
+    }
+
+    float operator [] (const int idx) const {return data[idx];}
+
+    bool operator == (const Matrix3& o) const {
+        for (int i = 0; i < 9; ++i) {
+            if (data[i] != o.data[i]) {return false;}
+        }
+        return true;
+    }
+
+	Matrix3 operator * (const float v) const {
+		return Matrix3({
+			data[0]*v, data[1]*v, data[2]*v,
+			data[3]*v, data[4]*v, data[5]*v,
+			data[6]*v, data[7]*v, data[8]*v,
+		});
+	}
+
+	Matrix3 operator + (const Matrix3& m) const {
+		return Matrix3({
+			data[0]+m.data[0], data[1]+m.data[1], data[2]+m.data[2],
+			data[3]+m.data[3], data[4]+m.data[4], data[5]+m.data[5],
+			data[6]+m.data[6], data[7]+m.data[7], data[8]+m.data[8],
+		});
+	}
+
+	Matrix3 operator * (const Matrix3& m) const {
+		return Matrix3({
+			data[0]*m.data[0] + data[1]*m.data[3] + data[2]*m.data[6],		data[0]*m.data[1] + data[1]*m.data[4] + data[2]*m.data[7],		data[0]*m.data[2] + data[1]*m.data[5] + data[2]*m.data[8],
+			data[3]*m.data[0] + data[4]*m.data[3] + data[5]*m.data[6],		data[3]*m.data[1] + data[4]*m.data[4] + data[5]*m.data[7],		data[3]*m.data[2] + data[4]*m.data[5] + data[5]*m.data[8],
+			data[6]*m.data[0] + data[7]*m.data[3] + data[8]*m.data[6],		data[6]*m.data[1] + data[7]*m.data[4] + data[8]*m.data[7],		data[6]*m.data[2] + data[7]*m.data[5] + data[8]*m.data[8],
+		});
+	}
+
+};
+
+struct Vector3 {
+
+    float x,y,z;
+
+    Vector3() : x(0), y(0), z(0) {;}
+
+    Vector3(float x, float y, float z) : x(x), y(y), z(z) {;}
+
+	Vector3 operator + (const Vector3 o) const {
+		return Vector3(x+o.x, y+o.y, z+o.z);
+	}
+	Vector3 operator - (const Vector3 o) const {
+		return Vector3(x-o.x, y-o.y, z-o.z);
+	}
+	Vector3 operator * (const Vector3 o) const {
+		return Vector3(x*o.x, y*o.y, z*o.z);
+	}
+
+
+	Vector3 operator * (const float v) const {
+		return Vector3(x*v, y*v, z*v);
+	}
+	Vector3 operator / (const float v) const {
+		return Vector3(x/v, y/v, z/v);
+	}
+
+    bool operator == (const Vector3 o) const {
+        return (x==o.x) && (y==o.y) && (z==o.z);
+    }
+
+	float norm() const {
+		return std::sqrt(x*x + y*y + z*z);
+	}
+
+	Vector3 normalized() const {
+		const float n = norm();
+		return Vector3(x/n, y/n, z/n);
+	}
+
+	Vector3 cross(const Vector3 o) const {
+		return Vector3(
+			y*o.z - z*o.y,
+			z*o.x - x*o.z,
+			x*o.y - y*o.x
+		);
+	}
+
+	float dot(const Vector3 o) const {
+		return (x*o.x) + (y*o.y) + (z*o.z);
+	}
+
+};
+
+inline Vector3 operator * (const Matrix3& mat, const Vector3& vec) {
+
+    return Vector3(
+        (mat[ 0]*vec.x + mat[ 1]*vec.y + mat[ 2]*vec.z),
+        (mat[ 3]*vec.x + mat[ 4]*vec.y + mat[ 5]*vec.z),
+        (mat[ 6]*vec.x + mat[ 7]*vec.y + mat[ 8]*vec.z)
+    );
+
+}
+
+//inline Matrix4 operator * (const Matrix4& m1, const Matrix4& m2) {
+//    return Matrix4({
+//        m1[ 0]*m2[ 0] + m1[ 1]*m2[ 4] + m1[ 2]*m2[ 8] + m1[ 3]*m2[12],		m1[ 0]*m2[ 1] + m1[ 1]*m2[ 5] + m1[ 2]*m2[ 9] + m1[ 3]*m2[13],		m1[ 0]*m2[ 2] + m1[ 1]*m2[ 6] + m1[ 2]*m2[10] + m1[ 3]*m2[14],		m1[ 0]*m2[ 3] + m1[ 1]*m2[ 7] + m1[ 2]*m2[11] + m1[ 3]*m2[15],
+//        m1[ 4]*m2[ 0] + m1[ 5]*m2[ 4] + m1[ 6]*m2[ 8] + m1[ 7]*m2[12],		m1[ 4]*m2[ 1] + m1[ 5]*m2[ 5] + m1[ 6]*m2[ 9] + m1[ 7]*m2[13],		m1[ 4]*m2[ 2] + m1[ 5]*m2[ 6] + m1[ 6]*m2[10] + m1[ 7]*m2[14],		m1[ 4]*m2[ 3] + m1[ 5]*m2[ 7] + m1[ 6]*m2[11] + m1[ 7]*m2[15],
+//        m1[ 8]*m2[ 0] + m1[ 9]*m2[ 4] + m1[10]*m2[ 8] + m1[11]*m2[12],		m1[ 8]*m2[ 1] + m1[ 9]*m2[ 5] + m1[10]*m2[ 9] + m1[11]*m2[13],		m1[ 8]*m2[ 2] + m1[ 9]*m2[ 6] + m1[10]*m2[10] + m1[11]*m2[14],		m1[ 8]*m2[ 3] + m1[ 9]*m2[ 7] + m1[10]*m2[11] + m1[11]*m2[15],
+//        m1[12]*m2[ 0] + m1[13]*m2[ 4] + m1[14]*m2[ 8] + m1[15]*m2[12],		m1[12]*m2[ 1] + m1[13]*m2[ 5] + m1[14]*m2[ 9] + m1[15]*m2[13],		m1[12]*m2[ 2] + m1[13]*m2[ 6] + m1[14]*m2[10] + m1[15]*m2[14],		m1[12]*m2[ 3] + m1[13]*m2[ 7] + m1[14]*m2[11] + m1[15]*m2[15]
+//    });
+//}
+
+#endif // INDOOR_MATH_MATRIX3_H
--- a/math/MovingMedian.h
+++ b/math/MovingMedian.h
@@ -41,9 +41,9 @@ public:

 		// get the median
 		if (values.size() % 2 != 0) {
-			return values[(values.size() + 0) / 2];
+			return copy[(copy.size() + 0) / 2];
 		} else {
-			return (values[values.size() / 2 - 1] + values[values.size() / 2 + 0]) / 2;
+			return (copy[copy.size() / 2 - 1] + copy[copy.size() / 2 + 0]) / 2;
 		}

 	}
--- a/math/MovingMedianTS.h
+++ b/math/MovingMedianTS.h
@@ -0,0 +1,74 @@
+#ifndef MOVINGMEDIANTS_H
+#define MOVINGMEDIANTS_H
+
+#include <vector>
+#include <algorithm>
+#include "../data/Timestamp.h"
+
+template <typename T> class MovingMedianTS {
+
+private:
+
+	/** timestamp -> value combination */
+	struct Entry {
+		Timestamp ts;
+		T value;
+		Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;}
+	};
+
+	/** the regional window to use */
+	Timestamp window;
+
+	/** the value history for the window-size */
+	std::vector<Entry> history;
+
+
+
+
+public:
+
+	/** ctor with the window-size to use */
+	MovingMedianTS(const Timestamp window) : window(window) {
+
+	}
+
+	/** add a new value */
+	void add(const Timestamp ts, const T data) {
+
+		// append to history
+		history.push_back(Entry(ts, data));
+
+		// remove too-old history entries
+		const Timestamp oldest = ts - window;
+		while(history.front().ts < oldest) {
+
+			// remove from history
+			history.erase(history.begin());
+
+		}
+
+	}
+
+	/** get the current median */
+	T get() const {
+
+		const auto comp = [] (const Entry& a, const Entry& b) {
+			return a.value < b.value;
+		};
+
+		// create a sorted copy (slow, but works)
+		std::vector<Entry> copy = history;
+		std::sort(copy.begin(), copy.end(), comp);
+
+		// get the median
+		if (copy.size() % 2 != 0) {
+			return copy[(copy.size() + 0) / 2].value;
+		} else {
+			return (copy[copy.size() / 2 - 1].value + copy[copy.size() / 2 + 0].value) / 2;
+		}
+
+	}
+
+};
+
+#endif // MOVINGMEDIANTS_H
--- a/math/stats/Average.h
+++ b/math/stats/Average.h
@@ -15,7 +15,7 @@ namespace Stats {
 	public:

 		/** ctor */
-		Average() : cnt(0), sum(0) {
+		Average() : cnt(0), sum() {
 			;
 		}

@@ -33,7 +33,7 @@ namespace Stats {
 		/** get the current value */
 		Scalar get() const {
 			Assert::isNot0(cnt, "add() values first!");
-			return sum / (Scalar)cnt;
+			return sum / cnt;
 		}

 	};
--- a/math/stats/Maximum.h
+++ b/math/stats/Maximum.h
@@ -2,6 +2,7 @@
 #define STATS_MAXIMUM_H

 #include "../../Assertions.h"
+#include <limits>

 namespace Stats {

@@ -9,7 +10,7 @@ namespace Stats {

 	private:

-		const Scalar START = -99999999;
+		const Scalar START = std::numeric_limits<Scalar>::lowest();
 		Scalar curMax;

 	public:
--- a/math/stats/Minimum.h
+++ b/math/stats/Minimum.h
@@ -1,13 +1,15 @@
 #ifndef STATS_MINIMUM_H
 #define STATS_MINIMUM_H

+#include <limits>
+
 namespace Stats {

 	template <typename Scalar> class Minimum {

 	private:

-		const Scalar START = +999999999;
+		const Scalar START = std::numeric_limits<Scalar>::max();
 		Scalar curMin;

 	public:
--- a/math/stats/Variance.h
+++ b/math/stats/Variance.h
@@ -0,0 +1,48 @@
+#ifndef STATS_VARIANCE_H
+#define STATS_VARIANCE_H
+
+#include "../../Assertions.h"
+#include "Average.h"
+
+namespace Stats {
+
+	template <typename Scalar> class Variance {
+
+		Average<Scalar> avg;
+		Average<Scalar> avg2;
+
+	public:
+
+		/** ctor */
+		Variance() {
+			;
+		}
+
+		/** contains valid data? */
+		bool isValid() const {
+			return avg.isValid();
+		}
+
+		/** add a new value */
+		void add(const Scalar val) {
+			avg.add(val);
+			avg2.add(val*val);
+		}
+
+		/** get the current variance */
+		Scalar get() const {
+			Assert::isTrue(avg.isValid(), "add() values first!");
+			return avg2.get() - (avg.get()*avg.get());
+		}
+
+		/** get the current stadard-deviation */
+		Scalar getStdDev() const {
+			return std::sqrt(get);
+		}
+
+
+	};
+
+}
+
+#endif // STATS_VARIANCE_H
--- a/nav/dijkstra/Dijkstra.h
+++ b/nav/dijkstra/Dijkstra.h
@@ -30,11 +30,16 @@ public:
 	}

 	/** get the dijkstra-pendant for the given user-node. null if none matches */
-	const inline DijkstraNode<T>* getNode(const T& userNode) const {
+	inline const DijkstraNode<T>* getNode(const T& userNode) const {
 		auto it = nodes.find(&userNode);
 		return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
 	}

+	/** get all constructed dijkstra-nodes and their original pendant */
+	inline const std::unordered_map<const T*, DijkstraNode<T>*>& getNodes() const {
+		return nodes;
+	}
+
 	/** calculate all shortest paths from ANY node to the given destination */
 	template <typename Access> void build(const T* end, const Access& acc) {
 		build(end, nullptr, acc, NAN);
@@ -80,7 +85,7 @@ public:
 			if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;}

 			// when a maximum weight is given, stop when current cum-dist > maxWeight
-			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;}
+			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached weight limit: " + std::to_string(maxWeight)); break;}

 			// visit (and maybe update) each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
--- a/sensors/MACAddress.h
+++ b/sensors/MACAddress.h
@@ -47,30 +47,42 @@ public:
 	MACAddress(const std::string& str) {

 		// sanity check
-        if (str.size() == 17 ){
-            mac = 0;	// all zeros
-            fields.h5 = hexWordToInt(str[ 0], str[ 1]);
-            fields.h4 = hexWordToInt(str[ 3], str[ 4]);
-            fields.h3 = hexWordToInt(str[ 6], str[ 7]);
-            fields.h2 = hexWordToInt(str[ 9], str[10]);
-            fields.h1 = hexWordToInt(str[12], str[13]);
-            fields.h0 = hexWordToInt(str[15], str[16]);
-        }
-        else if (str.size() == 12){
-            mac = 0;	// all zeros
-            fields.h5 = hexWordToInt(str[ 0], str[ 1]);
-            fields.h4 = hexWordToInt(str[ 2], str[ 3]);
-            fields.h3 = hexWordToInt(str[ 4], str[ 5]);
+		if (str.size() == 17 ){
+			mac = 0;	// all zeros
+			fields.h5 = hexWordToInt(str[ 0], str[ 1]);
+			fields.h4 = hexWordToInt(str[ 3], str[ 4]);
+			fields.h3 = hexWordToInt(str[ 6], str[ 7]);
+			fields.h2 = hexWordToInt(str[ 9], str[10]);
+			fields.h1 = hexWordToInt(str[12], str[13]);
+			fields.h0 = hexWordToInt(str[15], str[16]);
+		}
+		else if (str.size() == 12){
+			mac = 0;	// all zeros
+			fields.h5 = hexWordToInt(str[ 0], str[ 1]);
+			fields.h4 = hexWordToInt(str[ 2], str[ 3]);
+			fields.h3 = hexWordToInt(str[ 4], str[ 5]);
 			fields.h2 = hexWordToInt(str[ 6], str[ 7]);
 			fields.h1 = hexWordToInt(str[ 8], str[ 9]);
-            fields.h0 = hexWordToInt(str[10], str[11]);
-        }
-        else{
-            throw Exception("invalid hex string length. must be 17 or 12 (without :)");
-        }
+			fields.h0 = hexWordToInt(str[10], str[11]);
+		}
+		else{
+			throw Exception("invalid hex string length. must be 17 or 12 (without :)");
+		}

 	}

+	uint8_t getField(const int idx) const {
+		switch(idx) {
+			case 0: return fields.h0;
+			case 1: return fields.h1;
+			case 2: return fields.h2;
+			case 3: return fields.h3;
+			case 4: return fields.h4;
+			case 5: return fields.h5;
+		}
+		throw Exception("field-idx out of bounds");
+	}
+
 	/** convert to lower-case hex-string ("xx:xx:xx:xx:xx:xx") */
 	std::string asString() const {

--- a/sensors/activity/ActivityButterPressure.h
+++ b/sensors/activity/ActivityButterPressure.h
@@ -6,7 +6,9 @@
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/MovingAVG.h"

-#include "BarometerData.h"
+#include "../pressure/BarometerData.h"
+
+#include "Activity.h"

 /**
 * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
@@ -16,8 +18,6 @@ class ActivityButterPressure {

 public:

-    enum Activity {DOWN, STAY, UP};
-
    struct History {
        Timestamp ts;
        BarometerData data;
@@ -27,6 +27,7 @@ public:
    private:

    std::vector<History> output;
+    std::vector<History> baroInterpHistory;
    Activity currentActivity;
    MovingAVG<float> mvAvg = MovingAVG<float>(20);

@@ -40,9 +41,12 @@ public:
    */
    const bool additionalLowpassFilter = false;
    const unsigned long diffSize = 20; //the number values used for finding the activity.
+    //TODO: some kind of algorithm to calculate the threshold
    const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
+    const float upperBoundary = 0.1; // if this boundary is passed, we should have errors in barometer data
    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.05f,2);
+    bool firstCall = false;

    FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));

@@ -50,29 +54,30 @@ public:


 	/** ctor */
-    ActivityButterPressure() : currentActivity(STAY){
+    ActivityButterPressure() : currentActivity(Activity::STANDING){
        ;
    }


 	/** add new sensor readings that were received at the given timestamp */
-    Activity add(const Timestamp& ts, const BarometerData& baro) {
+    void add(const Timestamp& ts, const BarometerData& baro) {

        //init
-        static bool firstCall = false;
        if(!firstCall){
            butter.stepInitialization(baro.hPa);
            firstCall = true;

            butter2.stepInitialization(0);
-            return STAY;
+            currentActivity = Activity::STANDING;
        }

        bool newInterpolatedValues = false;

        //interpolate & butter
        auto callback = [&] (const Timestamp ts, const float val) {
+            //interp
            float interpValue = val;
+            baroInterpHistory.push_back(History(ts, BarometerData(interpValue)));

            //butter
            float butterValue = butter.process(interpValue);
@@ -92,7 +97,6 @@ public:
                for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){

                    float diffVal = output[i+1].data.hPa - output[i].data.hPa;
-
                    diff.push_back(diffVal);
                }

@@ -111,26 +115,24 @@ public:
                    actValue = sum;
                }

-                if(actValue > threshold){
-                    currentActivity = DOWN;
+                if(actValue > threshold && actValue < upperBoundary){
+                    currentActivity = Activity::WALKING_DOWN;
                }
-                else if (actValue < -threshold){
-                    currentActivity = UP;
+                else if (actValue < -threshold && actValue > -upperBoundary){
+                    currentActivity = Activity::WALKING_UP;
                }
                else{
-                    currentActivity = STAY;
+                    currentActivity = Activity::WALKING;
                }
            }
        }

-        return currentActivity;
-
 	}

    /** get the current Activity */
-    Activity getCurrentActivity() {
+    Activity get() const {
        return currentActivity;
-	}
+    }
 };

 #endif // ACTIVITYBUTTERPRESSURE_H
--- a/sensors/activity/ActivityButterPressurePercent.h
+++ b/sensors/activity/ActivityButterPressurePercent.h
@@ -8,8 +8,9 @@
 #include <KLib/Assertions.h>
 #include <vector>

+#include "../pressure/BarometerData.h"

-#include "BarometerData.h"
+#include "Activity.h"

 /**
 * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
--- a/sensors/activity/ActivityDetector.h
+++ b/sensors/activity/ActivityDetector.h
@@ -41,8 +41,8 @@ private:
 	MovingStdDevTS<float> stdDev;
 	MovingStdDevTS<float> stdDev2;

-	MovingAverageTS<float> baroAvgSlow;
-	MovingAverageTS<float> baroAvgFast;
+    //MovingAverageTS<float> baroAvgSlow;
+    //MovingAverageTS<float> baroAvgFast;

 	MovingAverageTS<float> baroAvg;
 	HistoryTS<float> baroHistory;
@@ -140,7 +140,7 @@ private:
 			++xx;
 		#endif

-		if (std::abs(delta_hPa) < 0.042) {
+        if (std::abs(delta_hPa) < 0.042) {
 			current = Activity::WALKING;
 			return;
 		} else if (delta_hPa > 0) {
--- a/sensors/imu/AccelerometerData.h
+++ b/sensors/imu/AccelerometerData.h
@@ -42,6 +42,14 @@ struct AccelerometerData {
 		return AccelerometerData(x/val, y/val, z/val);
 	}

+	AccelerometerData operator * (const float val) const {
+		return AccelerometerData(x*val, y*val, z*val);
+	}
+
+	AccelerometerData operator + (const AccelerometerData o) const {
+		return AccelerometerData(x+o.x, y+o.y, z+o.z);
+	}
+
 	std::string asString() const {
 		std::stringstream ss;
 		ss << "(" << x << "," << y << "," << z << ")";
--- a/sensors/imu/CompassDetection.h
+++ b/sensors/imu/CompassDetection.h
@@ -0,0 +1,112 @@
+#ifndef INDOOR_IMU_COMPASSDETECTION_H
+#define INDOOR_IMU_COMPASSDETECTION_H
+
+#include "MagnetometerData.h"
+#include "PoseDetection.h"
+
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../math/MovingStdDevTS.h"
+#include "../../geo/Point3.h"
+#include "../../Assertions.h"
+
+#include "CompassDetectionPlot.h"
+
+#include <cmath>
+#include <vector>
+
+
+/**
+ * custom compass implementation, similar to turn-detection
+ */
+class CompassDetection {
+
+private:
+
+	#ifdef WITH_DEBUG_PLOT
+		CompassDetectionPlot plot;
+	#endif
+
+//	struct {
+//		Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
+//		bool isKnown = false;
+//		Timestamp lastEstimation;
+//	} orientation;
+
+	MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
+
+	//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
+	MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
+
+	PoseDetection* pose = nullptr;
+	int numMagReadings = 0;
+
+public:
+
+	/** ctor */
+	CompassDetection(PoseDetection* pose) : pose(pose) {
+		;
+	}
+
+	/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
+	float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {
+
+		++numMagReadings;
+
+		// get the current magnetometer-reading as vector
+		//Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z;
+		const Vector3 vec(mag.x, mag.y, mag.z);
+
+		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
+		//Eigen::Vector3f _curMag = pose->getMatrix() * vec;
+		const Vector3 _curMag = pose->getMatrix() * vec;
+
+		//avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2)));
+		avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z));
+		const MagnetometerData curMag = avgIn.get();
+		//const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z);
+
+		// calculate standard-deviation
+		//stdDev.add(ts, curMag);
+		//const float dev = std::max(stdDev.get().x, stdDev.get().y);
+
+
+		// calculate angle
+		// https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
+		const float mx = curMag.x;
+		const float my = curMag.y;
+		const float tmp = std::atan2(my, mx);
+		//const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my));
+
+		// http://www.magnetic-declination.com/
+		// http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html
+		const float declination = 3.0f / 180.0f * M_PI;		// GERMANY!
+		const float curHeading = - tmp + declination;
+		float resHeading;
+		bool stable = true;
+
+		// calculate standard-deviation within a certain timeframe
+		stdDev.add(ts, curHeading);
+
+		// if the standard-deviation is too high,
+		// the compass is considered unstable
+		if (numMagReadings < 250 || stdDev.get() > 0.30) {
+			resHeading = NAN;
+			stable = false;
+		} else {
+			resHeading = curHeading;
+			stable = true;
+		}
+
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(ts, curHeading, stable, mag, curMag);
+		#endif
+
+		// done
+		return resHeading;
+
+	}
+
+};
+
+#endif // INDOOR_IMU_COMPASSDETECTION_H
--- a/sensors/imu/CompassDetectionPlot.h
+++ b/sensors/imu/CompassDetectionPlot.h
@@ -0,0 +1,123 @@
+#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H
+#define INDOOR_IMU_COMPASSDETECTIONPLOT_H
+
+#ifdef WITH_DEBUG_PLOT
+
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+
+	#include "MagnetometerData.h"
+	#include "../../data/Timestamp.h"
+	#include "../../math/Matrix3.h"
+
+	class CompassDetectionPlot {
+
+		Timestamp plotRef;
+		Timestamp lastPlot;
+
+		K::Gnuplot gp1;
+		K::Gnuplot gp2;
+		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
+
+		K::GnuplotPlot plotMagRaw;
+		K::GnuplotPlotElementLines lineMagRawX;
+		K::GnuplotPlotElementLines lineMagRawY;
+		K::GnuplotPlotElementLines lineMagRawZ;
+		K::GnuplotPlotElementLines lineMagRawMagnitude;
+
+		K::GnuplotPlot plotMagFix;
+		K::GnuplotPlotElementLines lineMagFixX;
+		K::GnuplotPlotElementLines lineMagFixY;
+		K::GnuplotPlotElementLines lineMagFixZ;
+
+		K::GnuplotPlot plotMagScatter;
+		K::GnuplotPlotElementPoints scatterPoints;
+
+
+
+	public:
+
+		CompassDetectionPlot() {
+
+			gp1 << "set autoscale xfix\n";
+			gp2 << "set size ratio -1\n";
+
+			multiplot.add(&plotMagRaw);
+			multiplot.add(&plotMagFix);
+
+			plotMagRaw.setTitle("Magnetometer (raw)");
+			plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x");
+			plotMagRaw.add(&lineMagRawY); lineMagRawY.getStroke().getColor().setHexStr("#00ff00"); lineMagRawY.setTitle("y");
+			plotMagRaw.add(&lineMagRawZ); lineMagRawZ.getStroke().getColor().setHexStr("#0000ff"); lineMagRawZ.setTitle("z");
+			plotMagRaw.add(&lineMagRawMagnitude); lineMagRawMagnitude.getStroke().getColor().setHexStr("#000000"); lineMagRawMagnitude.setTitle("magnitude");
+
+			plotMagFix.setTitle("Magnetometer (re-aligned)");
+			plotMagFix.add(&lineMagFixX); lineMagFixX.getStroke().getColor().setHexStr("#ff0000"); lineMagFixX.setTitle("x");
+			plotMagFix.add(&lineMagFixY); lineMagFixY.getStroke().getColor().setHexStr("#00ff00"); lineMagFixY.setTitle("y");
+			plotMagFix.add(&lineMagFixZ); lineMagFixZ.getStroke().getColor().setHexStr("#0000ff"); lineMagFixZ.setTitle("z");
+
+			plotMagScatter.add(&scatterPoints);
+
+
+		}
+
+		void add(Timestamp ts, float curHeading, bool stable, const MagnetometerData& mag, const MagnetometerData& curMag) {
+
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
+
+			// raw gyro
+			lineMagRawX.add( K::GnuplotPoint2(tsPlot.ms(), mag.x) );
+			lineMagRawY.add( K::GnuplotPoint2(tsPlot.ms(), mag.y) );
+			lineMagRawZ.add( K::GnuplotPoint2(tsPlot.ms(), mag.z) );
+			lineMagRawMagnitude.add( K::GnuplotPoint2(tsPlot.ms(), mag.magnitude()) );
+
+			// adjusted mag
+			lineMagFixX.add( K::GnuplotPoint2(tsPlot.ms(), curMag.x) );
+			lineMagFixY.add( K::GnuplotPoint2(tsPlot.ms(), curMag.y) );
+			lineMagFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curMag.z) );
+
+			const float len = 1;//curMag.magnitude();// std::sqrt(curMag.x*curMag.x + curMag.y*curMag.y);
+			scatterPoints.add(K::GnuplotPoint2(curMag.x/len, curMag.y/len));
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineMagRawX.removeIf(remove);
+				lineMagRawY.removeIf(remove);
+				lineMagRawZ.removeIf(remove);
+				lineMagRawMagnitude.removeIf(remove);
+				lineMagFixX.removeIf(remove);
+				lineMagFixY.removeIf(remove);
+				lineMagFixZ.removeIf(remove);
+
+
+				const float s = stable ? 0.1 : 0.03;
+				const float ax = 0.85 + std::cos(curHeading)*s;
+				const float ay = 0.85 + std::sin(curHeading)*s;
+				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
+				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
+
+				gp1.draw(multiplot);
+				gp1.flush();
+
+				gp2.draw(plotMagScatter);
+				gp2.flush();
+
+			}
+
+		}
+
+	};
+
+#endif
+
+#endif
--- a/sensors/imu/MagnetometerData.h
+++ b/sensors/imu/MagnetometerData.h
@@ -0,0 +1,81 @@
+#ifndef INDOOR_IMU_MAGNETOMETERDATA_H
+#define INDOOR_IMU_MAGNETOMETERDATA_H
+
+
+#include <cmath>
+#include <sstream>
+
+/**
+ * data received from a magnetometer sensor
+ */
+struct MagnetometerData {
+
+	float x;
+	float y;
+	float z;
+
+	MagnetometerData() : x(0), y(0), z(0) {;}
+
+	/** ctor from RADIANS */
+	MagnetometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const GyroscopeData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+	MagnetometerData& operator += (const MagnetometerData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+	MagnetometerData& operator -= (const MagnetometerData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+	MagnetometerData operator * (const MagnetometerData& o) const {
+		return MagnetometerData(x*o.x, y*o.y, z*o.z);
+	}
+
+	MagnetometerData operator - (const MagnetometerData& o) const {
+		return MagnetometerData(x-o.x, y-o.y, z-o.z);
+	}
+
+	MagnetometerData operator / (const float val) const {
+		return MagnetometerData(x/val, y/val, z/val);
+	}
+
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
+};
+
+namespace std {
+	MagnetometerData sqrt(const MagnetometerData& o) {
+		return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
+	}
+}
+
+#endif // INDOOR_IMU_MAGNETOMETERDATA_H
--- a/sensors/imu/PoseDetection.h
+++ b/sensors/imu/PoseDetection.h
@@ -0,0 +1,480 @@
+#ifndef INDOOR_IMU_POSEDETECTION_H
+#define INDOOR_IMU_POSEDETECTION_H
+
+#include "AccelerometerData.h"
+
+#include "../../data/Timestamp.h"
+
+#include "../../math/MovingAverageTS.h"
+#include "../../math/MovingMedianTS.h"
+#include "../../math/Matrix3.h"
+
+#include "../../geo/Point3.h"
+
+#include <eigen3/Eigen/Dense>
+
+#include "PoseDetectionPlot.h"
+
+/**
+ * estimate the smartphones world-pose,
+ * based on the accelerometer's data
+ */
+class PoseDetection {
+
+//	struct LongTermTriggerAverage {
+
+//		Eigen::Vector3f sum;
+//		int cnt;
+
+//		XYZ() {
+//			reset();
+//		}
+
+//		/** add the given accelerometer reading */
+//		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
+
+//			// did NOT improve the result for every smartphone (only some)
+//			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
+//			//if (deltaMag > 5.0) {return;}
+
+//			// adjust sum and count (for average calculation)
+//			Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
+//			sum += vec;
+//			++cnt;
+
+
+//		}
+
+//		AccelerometerData getAvg() const {
+//			return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
+//		}
+
+//		/** get the current rotation matrix estimation */
+//		Eigen::Matrix3f get() const {
+
+//			// get the current acceleromter average
+//			const Eigen::Vector3f avg = sum / cnt;
+
+//			// rotate average accelerometer into (0,0,1)
+//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+//			const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+
+//			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
+//			Eigen::Vector3f aligned = (rotMat * avg).normalized();
+//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+//			return rotMat;
+
+//		}
+
+//		/** reset the current sum etc. */
+//		void reset() {
+//			cnt = 0;
+//			sum = Eigen::Vector3f::Zero();
+//		}
+
+
+//	};
+
+
+	/** live-pose-estimation using moving average of the smartphone's accelerometer */
+	struct EstMovingAverage {
+
+		// average the accelerometer
+		MovingAverageTS<AccelerometerData> avg;
+
+		EstMovingAverage(const Timestamp window) :
+			avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {
+
+			// start approximately
+			addAcc(Timestamp(), AccelerometerData(0,0,9.81));
+
+		}
+
+		/** add the given accelerometer reading */
+		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
+			avg.add(ts, acc);
+		}
+
+		AccelerometerData getBase() const {
+			return avg.get();
+		}
+
+		/** get the current rotation matrix estimation */
+		//Eigen::Matrix3f get() const {
+		Matrix3 get() const {
+
+			// get the current acceleromter average
+			const AccelerometerData avgAcc = avg.get();
+			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
+			const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);
+
+			// rotate average-accelerometer into (0,0,1)
+			//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+			const Vector3 zAxis(0,0,1);
+			const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
+			//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
+			//const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+
+			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
+			//Eigen::Vector3f aligned = (rotMat * avg).normalized();
+			const Vector3 aligned = (rotMat * avg).normalized();
+			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+			return rotMat;
+
+		}
+
+	};
+
+	/** live-pose-estimation using moving median of the smartphone's accelerometer */
+	struct EstMovingMedian {
+
+		// median the accelerometer
+		MovingMedianTS<float> medianX;
+		MovingMedianTS<float> medianY;
+		MovingMedianTS<float> medianZ;
+
+		EstMovingMedian(const Timestamp window) :
+			medianX(window), medianY(window), medianZ(window) {
+
+			// start approximately
+			addAcc(Timestamp(), AccelerometerData(0,0,9.81));
+
+		}
+
+		/** add the given accelerometer reading */
+		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
+			medianX.add(ts, acc.x);
+			medianY.add(ts, acc.y);
+			medianZ.add(ts, acc.z);
+		}
+
+		AccelerometerData getBase() const {
+			return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
+		}
+
+		/** get the current rotation matrix estimation */
+		//Eigen::Matrix3f get() const {
+		Matrix3 get() const {
+
+			const Vector3 base(medianX.get(), medianY.get(), medianZ.get());
+
+			// rotate average-accelerometer into (0,0,1)
+			const Vector3 zAxis(0,0,1);
+			const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
+
+			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
+			const Vector3 aligned = (rotMat * base).normalized();
+			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+			return rotMat;
+
+		}
+
+	};
+
+
+
+private:
+
+	struct {
+		//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
+		Matrix3 rotationMatrix = Matrix3::identity();
+		bool isKnown = false;
+		Timestamp lastEstimation;
+	} orientation;
+
+	/** how the pose is estimated */
+	//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
+	EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450));
+	//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));
+
+	#ifdef WITH_DEBUG_PLOT
+		PoseDetectionPlot plot;
+	#endif
+
+
+public:
+
+	/** ctor */
+	PoseDetection() {
+		;
+	}
+
+//	/** get the smartphone's rotation matrix */
+//	Eigen::Matrix3f getMatrix() const {
+//		return orientation.rotationMatrix;
+//	}
+
+	/** get the smartphone's rotation matrix */
+	const Matrix3& getMatrix() const {
+		return orientation.rotationMatrix;
+	}
+
+	/** is the pose known and stable? */
+	bool isKnown() const {
+		return orientation.isKnown;
+	}
+
+	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
+
+		// add accelerometer data
+		est.addAcc(ts, acc);
+
+		// update (if needed)
+		orientation.rotationMatrix = est.get();
+		orientation.isKnown = true;
+		orientation.lastEstimation = ts;
+
+		// debug-plot (if configured)
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(ts, est.getBase(), orientation.rotationMatrix);
+		#endif
+
+	}
+
+public:
+
+//	/** get a matrix that rotates the vector "from" into the vector "to" */
+//	static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
+
+//		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
+
+//		const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
+//		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
+
+//		Eigen::Matrix3f A; A <<
+//			0,		-x(2),	x(1),
+//			x(2),	0,		-x(0),
+//			-x(1),	x(0),	0;
+
+//		return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
+
+//	}
+
+	/** get a matrix that rotates the vector "from" into the vector "to" */
+	static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {
+
+		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
+
+		const Vector3 v = from.cross(to) / from.cross(to).norm();
+		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
+
+		Matrix3 A({
+			0.0f,	-v.z,	v.y,
+			v.z,	0.0f,	-v.x,
+			-v.y,	v.x,	0.0f
+		});
+
+		return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));
+
+	}
+
+
+
+
+
+//    /** get a rotation matrix for the given x,y,z rotation (in radians) */
+//    static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
+//        const float g = x; const float b = y; const float a = z;
+//        const float a11 = std::cos(a)*std::cos(b);
+//        const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
+//        const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
+//        const float a21 = std::sin(a)*std::cos(b);
+//        const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
+//        const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
+//        const float a31 = -std::sin(b);
+//        const float a32 = std::cos(b)*std::sin(g);
+//        const float a33 = std::cos(b)*std::cos(g);
+//        Eigen::Matrix3f m;
+//        m <<
+//            a11,	a12,	a13,
+//            a21,	a22,	a23,
+//            a31,	a32,	a33;
+//        ;
+//        return m;
+//    }
+
+//	/** estimate the smartphones current holding position */
+//	void estimateHolding2() {
+
+
+//		// z-axis points through the device and is the axis we are interested in
+//		// http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
+
+//		avgAcc = Eigen::Vector3f::Zero();
+
+//		for (const AccelerometerData& acc : accData) {
+//		//for (const GyroscopeData& acc : gyroData) {
+//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+//		//	Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+//			avgAcc += vec;
+//		}
+
+//		//avgAcc /= accData.size();
+//		avgAcc = avgAcc.normalized();
+
+//		Eigen::Vector3f rev; rev << 0,0,1;
+
+//		rotMat = getRotationMatrix(avgAcc, rev);
+
+
+//		//Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
+//		//Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
+////		Eigen::Vector3f re = rotMat * avgAcc;
+////		Eigen::Vector3f diff = rev-re;
+////		Assert::isTrue(diff.norm() < 0.001, "rotation error");
+
+//	}
+
+//	struct RotationMatrixEstimationUsingAccAngle {
+
+//		Eigen::Vector3f lastAvg;
+//		Eigen::Vector3f avg;
+//		int cnt;
+
+//		RotationMatrixEstimationUsingAccAngle() {
+//			reset();
+//		}
+
+//		void add(const float x, const float y, const float z) {
+
+//			Eigen::Vector3f vec; vec << x,y,z;
+//			avg += vec;
+//			++cnt;
+
+//		}
+
+//		void reset() {
+//			cnt = 0;
+//			avg = Eigen::Vector3f::Zero();
+//		}
+
+//		Eigen::Matrix3f get() {
+
+//			// http://www.hobbytronics.co.uk/accelerometer-info
+
+//			avg /= cnt;
+//			lastAvg = avg;
+
+//			//const float mag = avg.norm();
+
+//			const float baseX = 0;
+//			const float baseY = 0;
+//			const float baseZ = 0;	// mag?
+
+//			const float x = avg(0) - baseX;
+//			const float y = avg(1) - baseY;
+//			const float z = avg(2) - baseZ;
+
+//			const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
+//			const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
+
+//			const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0);		// TODO -ax or +ax?
+
+//			// sanity check
+//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+//			Eigen::Vector3f aligned = (rotMat * avg).normalized();
+//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+//			//			int i = 0; (void) i;
+
+//			reset();
+//			return rotMat;
+
+//		}
+
+//	} est;
+
+//	struct PCA {
+
+//		Eigen::Vector3f avg;
+//		Eigen::Vector3f lastAvg;
+//		Eigen::Matrix3f covar;
+//		int cnt = 0;
+
+//		PCA() {
+//			reset();
+//		}
+
+//		void add(const float x, const float y, const float z) {
+
+//			Eigen::Vector3f vec; vec << x,y,z;
+//			avg += vec;
+//			covar += vec*vec.transpose();
+//			++cnt;
+
+//		}
+
+//		Eigen::Matrix3f get() {
+
+//			avg /= cnt;
+//			covar /= cnt;
+//			lastAvg = avg;
+
+//			std::cout << avg << std::endl;
+
+//			Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
+//			for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
+
+//			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
+//			solver.eigenvalues();
+//			solver.eigenvectors();
+
+//			const auto eval = solver.eigenvalues();
+//			const auto evec = solver.eigenvectors();
+//			Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
+
+//			Eigen::Matrix3f rotMat;
+//			rotMat.col(0) = evec.col(0);
+//			rotMat.col(1) = evec.col(1);
+//			rotMat.col(2) = evec.col(2);		// 0,0,1 (z-axis) belongs to the strongest eigenvalue
+//			rotMat.transposeInPlace();
+
+//			//Eigen::Vector3f gy; gy << 0, 30, 30;
+//			Eigen::Vector3f avg1 = rotMat * avg;
+//			int i = 0; (void) i;
+
+//			reset();
+
+//			return rotMat;
+
+//		}
+
+//		void reset() {
+//			cnt = 0;
+//			avg = Eigen::Vector3f::Zero();
+//			covar = Eigen::Matrix3f::Zero();
+//		}
+
+
+//	} pca1;
+
+
+
+
+//	/** estimate the smartphones current holding position */
+//	void estimateHolding() {
+
+//		Eigen::Vector3f avg = Eigen::Vector3f::Zero();
+//		Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
+
+//		for (const AccelerometerData& acc : accData) {
+////		for (const GyroscopeData& acc : gyroData) {
+//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
+////			Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
+//			avg += vec;
+//			covar += vec * vec.transpose();
+//		}
+
+//		avg /= accData.size();		// TODO
+//		covar /= accData.size();	//TODO
+
+//		avgAcc = avg.normalized();
+
+
+};
+
+
+
+#endif // INDOOR_IMU_POSEDETECTION_H
--- a/sensors/imu/PoseDetectionPlot.h
+++ b/sensors/imu/PoseDetectionPlot.h
@@ -0,0 +1,146 @@
+#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H
+#define INDOOR_IMU_POSEDETECTIONPLOT_H
+
+#ifdef WITH_DEBUG_PLOT
+
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
+
+	#include "../../data/Timestamp.h"
+	#include "../../math/Matrix3.h"
+	#include "AccelerometerData.h"
+
+	class PoseDetectionPlot {
+
+		Timestamp plotRef;
+		Timestamp lastPlot;
+
+		K::Gnuplot gp1;
+		K::Gnuplot gp2;
+
+		K::GnuplotPlot plotAcc;
+		K::GnuplotPlotElementLines lineAccX;
+		K::GnuplotPlotElementLines lineAccY;
+		K::GnuplotPlotElementLines lineAccZ;
+
+		K::GnuplotSplot plotPose;
+		K::GnuplotSplotElementLines linePose;
+		//K::GnuplotSplotElementEmpty emptyPose;
+
+		std::vector<std::vector<std::vector<float>>> pose;
+
+	public:
+
+		/** ctor */
+		PoseDetectionPlot() {
+
+			gp1 << "set autoscale xfix\n";
+			gp2 << "set view equal xyz\n";
+
+			plotAcc.setTitle("Accelerometer");
+			plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
+			plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
+			plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");
+
+			plotPose.setTitle("Pose");
+			plotPose.getView().setEnabled(false);
+			plotPose.add(&linePose);
+			//plotPose.add(&emptyPose);
+
+			plotPose.getAxisX().setRange(-8,+8);
+			plotPose.getAxisY().setRange(-8,+8);
+			plotPose.getAxisZ().setRange(-8,+8);
+
+			const float a = 0.05; const float b = 0.95;
+			pose = {
+				{{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden
+				{{0, 0, 0},{0, 0, 1},{0, 1, 1},{0, 1, 0},{0, 0, 0}}, // links
+				{{1, 0, 0},{1, 0, 1},{1, 1, 1},{1, 1, 0},{1, 0, 0}}, // rechts
+				{{0, 1, 0},{1, 1, 0},{1, 1, 1},{0, 1, 1},{0, 1, 0}}, // oben
+				{{0, 0, 0},{1, 0, 0},{1, 0, 1},{0, 0, 1},{0, 0, 0}}, // unten
+				{{0, 0, 1},{1, 0, 1},{1, 1, 1},{0, 1, 1},{0, 0, 1}}, // deckel
+				{{a, 0.15, 1},{b, 0.15, 1},{b, 0.95, 1},{a, 0.95, 1},{a, 0.15, 1}},	// display
+			};
+
+			//K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000"));
+			K::GnuplotStroke stroke = K::GnuplotStroke::NONE();
+			K::GnuplotFill fillOut = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"));
+			K::GnuplotFill fillSide = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"));
+			K::GnuplotFill fillDisp = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#333333"));
+
+			plotPose.getObjects().set(1, new K::GnuplotObjectPolygon(fillOut, stroke));
+			plotPose.getObjects().set(2, new K::GnuplotObjectPolygon(fillSide, stroke));
+			plotPose.getObjects().set(3, new K::GnuplotObjectPolygon(fillSide, stroke));
+			plotPose.getObjects().set(4, new K::GnuplotObjectPolygon(fillSide, stroke));
+			plotPose.getObjects().set(5, new K::GnuplotObjectPolygon(fillSide, stroke));
+			plotPose.getObjects().set(6, new K::GnuplotObjectPolygon(fillOut, stroke));
+			plotPose.getObjects().set(7, new K::GnuplotObjectPolygon(fillDisp, stroke));
+
+		}
+
+		void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) {
+
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
+
+			// acc
+			lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) );
+			lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) );
+			lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) );
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+				lastPlot = tsPlot;
+
+				// update 3D smartphone model
+				for (size_t i = 0; i < pose.size(); ++i) {
+					K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear();
+					for (const std::vector<float>& pts : pose[i]) {
+						const Vector3 vec1(pts[0], pts[1], pts[2]);
+						const Vector3 vec2 = vec1 - Vector3(0.5, 0.5, 0.5);			// center cube at 0,0,0
+						const Vector3 vec3 = vec2 * Vector3(7, 15, 1);				// stretch cube
+						const Vector3 vec4 = rotation * vec3;
+						gp->add(K::GnuplotCoordinate3(vec4.x, vec4.y, vec4.z, K::GnuplotCoordinateSystem::FIRST));
+					}
+				}
+
+				// add coordinate system
+				const Vector3 vx = rotation * Vector3(2,0,0);
+				const Vector3 vy = rotation * Vector3(0,3,0);
+				const Vector3 vz = rotation * Vector3(0,0,5);
+				linePose.clear();
+				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z));
+				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z));
+				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z));
+
+				// remove old accelerometer entries
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineAccX.removeIf(remove);
+				lineAccY.removeIf(remove);
+				lineAccZ.removeIf(remove);
+
+				// raw accelerometer
+				gp1.draw(plotAcc);
+				gp1.flush();
+
+				// 3D pose
+				gp2.draw(plotPose);
+				gp2.flush();
+
+
+			}
+
+		}
+
+	};
+
+#endif
+
+#endif // INDOOR_IMU_POSEDETECTIONPLOT_H
--- a/sensors/imu/StepDetection.h
+++ b/sensors/imu/StepDetection.h
@@ -8,12 +8,14 @@
 #include <cmath>
 #include <vector>

-#include <KLib/misc/gnuplot/Gnuplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-#include <KLib/misc/gnuplot/GnuplotPlot.h>
-#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
-
+#ifdef WITH_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+#endif

 #include "../../Assertions.h"
 #include "../../math/MovingAverageTS.h"
@@ -38,11 +40,35 @@ private:
    const float upperThreshold = +0.4*0.6f;					// + is usually smaller than down (look at graphs)
    const float lowerThreshold = -1.5*0.6f;					// the 0.8 is for testing!

+
+	#ifdef WITH_DEBUG_PLOT
+		K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines lineDet;
+		K::GnuplotPlotElementPoints pointDet;
+		K::GnuplotPlotElementLines lineX;
+		K::GnuplotPlotElementLines lineY;
+		K::GnuplotPlotElementLines lineZ;
+		Timestamp plotRef;
+		Timestamp lastPlot;
+	#endif
+
+
 public:

 	/** ctor */
 	StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
-		;
+
+		#ifdef WITH_DEBUG_PLOT
+			gp << "set autoscale xfix\n";
+			plot.setTitle("Step Detection");
+			plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
+			plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
+			plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
+			plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
+			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
+		#endif
+
 	}

 	/** does the given data indicate a step? */
@@ -58,37 +84,61 @@ public:
 		bool step = false;


-		if (blockUntil > ts) {return false;}
+		if (blockUntil > ts) {
+
+			step = false;
+
+		} else {
+
+			// wait for a rising edge
+			if (waitForUp && delta > upperThreshold) {
+				blockUntil = ts + blockTime;				// block some time
+				waitForUp = false;
+			}
+
+			// wait for a falling edge
+			if (!waitForUp && delta < lowerThreshold) {
+				blockUntil = ts + blockTime;				// block some time
+				waitForUp = true;
+				step = true;
+			}

-		// wait for a rising edge
-		if (waitForUp && delta > upperThreshold) {
-			blockUntil = ts + blockTime;				// block some time
-			waitForUp = false;
 		}

-		// wait for a falling edge
-		if (!waitForUp && delta < lowerThreshold) {
-			blockUntil = ts + blockTime;				// block some time
-			waitForUp = true;
-			step = true;
-		}

-//		static K::Gnuplot gp;
-//		static K::GnuplotPlot plot;
-//		static K::GnuplotPlotElementLines lines1; plot.add(&lines1);
-//		static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff");
-//		static Timestamp ref = ts;
+		#ifdef WITH_DEBUG_PLOT

-//		static int i = 0;
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);

-//		//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
-//		lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) );
+			//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
+			lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
+			lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) );
+			lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
+			lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );

-//		if (++i % 100 == 0) {
-//			gp.draw(plot);
-//			gp.flush();
-//			usleep(1000*25);
-//		}
+			if (step) {
+				pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
+			}
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineX.removeIf(remove);
+				lineY.removeIf(remove);
+				lineZ.removeIf(remove);
+				lineDet.removeIf(remove);
+				pointDet.removeIf(remove);
+
+				gp.draw(plot);
+				gp.flush();
+				usleep(100);
+
+			}
+
+		#endif

 		return step;

--- a/sensors/imu/TurnDetection.h
+++ b/sensors/imu/TurnDetection.h
@@ -5,17 +5,17 @@
 #include "AccelerometerData.h"
 #include "../../data/Timestamp.h"
 #include "../../math/MovingAverageTS.h"
+#include "../../math/Matrix3.h"
+
+#include "../../geo/Point3.h"
+#include "PoseDetection.h"

 #include <eigen3/Eigen/Dense>

 #include <cmath>
 #include <vector>

-#include <KLib/misc/gnuplot/Gnuplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-#include <KLib/misc/gnuplot/GnuplotPlot.h>
-#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+#include "TurnDetectionPlot.h"


 #include "../../Assertions.h"
@@ -24,26 +24,22 @@ class TurnDetection {

 private:

+	PoseDetection* pose = nullptr;

 	//std::vector<GyroscopeData> gyroData;
-	Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
+	//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
+	Vector3 prevGyro = Vector3(0,0,0);

 	Timestamp lastGyroReading;

-
-	struct {
-		Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
-		bool isKnown = false;
-		Timestamp lastEstimation;
-	} orientation;
-
-
-
+	#ifdef WITH_DEBUG_PLOT
+		TurnDetectionPlot plot;
+	#endif

 public:

 	/** ctor */
-	TurnDetection() {
+	TurnDetection(PoseDetection* pose) : pose(pose) {
 		;
 	}

@@ -51,7 +47,6 @@ public:
 	// does not seem to help...
 //	struct DriftEstimator {

-
 //		MovingAverageTS<Eigen::Vector3f> avg;

 //		DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
@@ -70,12 +65,8 @@ public:
 //	} driftEst;


-
-
-
 	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {

-
 		// ignore the first reading completely, just remember its timestamp
 		if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}

@@ -88,18 +79,20 @@ public:

 		// ignore readings until the first orientation-estimation is available
 		// otherwise we would use a wrong rotation matrix which yields wrong results!
-		if (!orientation.isKnown) {return 0.0f;}
-
+		if (!pose->isKnown()) {return 0.0f;}

 		// get the  current gyro-reading as vector
-		Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
+		//Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
+		const Vector3 vec(gyro.x, gyro.y, gyro.z);

 		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
-		Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
+		//Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
+		const Vector3 curGyro = pose->getMatrix() * vec;
 		//driftEst.removeDrift(ts, curGyro);

 		// area
-		const Eigen::Vector3f area =
+		//const Eigen::Vector3f area =
+		const Vector3 area =

 				// Trapezoid rule (should be more accurate but does not always help?!)
 				//(prevGyro * curDiff.sec()) +					// squared region
@@ -115,340 +108,18 @@ public:
 		prevGyro = curGyro;

 		// rotation = z-axis only!
-		const float delta = area(2);
+		//const float delta = area(2);
+		const float delta = area.z;
+
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(ts, delta, gyro, curGyro);
+		#endif

 		// done
 		return delta;

 	}

-
-
-	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
-
-		// add accelerometer data
-		//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
-		est.addAcc(acc);
-
-		// start with the first available timestamp
-		if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
-
-		// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
-		if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
-			orientation.rotationMatrix = est.get();
-			orientation.isKnown = true;
-			orientation.lastEstimation = ts;
-			est.reset();
-		}
-
-	}
-
-private:
-
-//	/** estimate the smartphones current holding position */
-//	void estimateHolding2() {
-
-
-//		// z-axis points through the device and is the axis we are interested in
-//		// http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
-
-//		avgAcc = Eigen::Vector3f::Zero();
-
-//		for (const AccelerometerData& acc : accData) {
-//		//for (const GyroscopeData& acc : gyroData) {
-//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
-//		//	Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
-//			avgAcc += vec;
-//		}
-
-//		//avgAcc /= accData.size();
-//		avgAcc = avgAcc.normalized();
-
-//		Eigen::Vector3f rev; rev << 0,0,1;
-
-//		rotMat = getRotationMatrix(avgAcc, rev);
-
-
-//		//Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
-//		//Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
-////		Eigen::Vector3f re = rotMat * avgAcc;
-////		Eigen::Vector3f diff = rev-re;
-////		Assert::isTrue(diff.norm() < 0.001, "rotation error");
-
-//	}
-
-public:
-
-	/** get a matrix that rotates the vector "from" into the vector "to" */
-	static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
-
-		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
-
-		const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
-		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
-
-		Eigen::Matrix3f A; A <<
-			0,		-x(2),	x(1),
-			x(2),	0,		-x(0),
-			-x(1),	x(0),	0;
-
-		return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
-
-	}
-
-
-	struct XYZ {
-
-		Eigen::Vector3f sum;
-		int cnt;
-
-		XYZ() {
-			reset();
-		}
-
-		/** add the given accelerometer reading */
-		void addAcc(const AccelerometerData& acc) {
-
-			// did NOT improve the result for every smartphone (only some)
-			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
-			//if (deltaMag > 5.0) {return;}
-
-			// adjust sum and count (for average calculation)
-			Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
-			sum += vec;
-			++cnt;
-
-		}
-
-		/** get the current rotation matrix estimation */
-		Eigen::Matrix3f get() const {
-
-			// get the current acceleromter average
-			const Eigen::Vector3f avg = sum / cnt;
-
-			// rotate average accelerometer into (0,0,1)
-			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
-			const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
-
-			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
-			Eigen::Vector3f aligned = (rotMat * avg).normalized();
-			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
-
-			return rotMat;
-
-		}
-
-		/** reset the current sum etc. */
-		void reset() {
-			cnt = 0;
-			sum = Eigen::Vector3f::Zero();
-		}
-
-
-	} est;
-
-
-//	struct RotationMatrixEstimationUsingAccAngle {
-
-//		Eigen::Vector3f lastAvg;
-//		Eigen::Vector3f avg;
-//		int cnt;
-
-//		RotationMatrixEstimationUsingAccAngle() {
-//			reset();
-//		}
-
-//		void add(const float x, const float y, const float z) {
-
-//			Eigen::Vector3f vec; vec << x,y,z;
-//			avg += vec;
-//			++cnt;
-
-//		}
-
-//		void reset() {
-//			cnt = 0;
-//			avg = Eigen::Vector3f::Zero();
-//		}
-
-//		Eigen::Matrix3f get() {
-
-//			// http://www.hobbytronics.co.uk/accelerometer-info
-
-//			avg /= cnt;
-//			lastAvg = avg;
-
-//			//const float mag = avg.norm();
-
-//			const float baseX = 0;
-//			const float baseY = 0;
-//			const float baseZ = 0;	// mag?
-
-//			const float x = avg(0) - baseX;
-//			const float y = avg(1) - baseY;
-//			const float z = avg(2) - baseZ;
-
-//			const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
-//			const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
-
-//			const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0);		// TODO -ax or +ax?
-
-//			// sanity check
-//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
-//			Eigen::Vector3f aligned = (rotMat * avg).normalized();
-//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
-//			//			int i = 0; (void) i;
-
-//			reset();
-//			return rotMat;
-
-//		}
-
-//	} est;
-
-
-	/** get a rotation matrix for the given x,y,z rotation (in radians) */
-	static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
-		const float g = x; const float b = y; const float a = z;
-		const float a11 = std::cos(a)*std::cos(b);
-		const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
-		const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
-		const float a21 = std::sin(a)*std::cos(b);
-		const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
-		const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
-		const float a31 = -std::sin(b);
-		const float a32 = std::cos(b)*std::sin(g);
-		const float a33 = std::cos(b)*std::cos(g);
-		Eigen::Matrix3f m;
-		m <<
-			a11,	a12,	a13,
-			a21,	a22,	a23,
-			a31,	a32,	a33;
-		;
-		return m;
-	}
-
-
-//	struct PCA {
-
-//		Eigen::Vector3f avg;
-//		Eigen::Vector3f lastAvg;
-//		Eigen::Matrix3f covar;
-//		int cnt = 0;
-
-//		PCA() {
-//			reset();
-//		}
-
-//		void add(const float x, const float y, const float z) {
-
-//			Eigen::Vector3f vec; vec << x,y,z;
-//			avg += vec;
-//			covar += vec*vec.transpose();
-//			++cnt;
-
-//		}
-
-//		Eigen::Matrix3f get() {
-
-//			avg /= cnt;
-//			covar /= cnt;
-//			lastAvg = avg;
-
-//			std::cout << avg << std::endl;
-
-//			Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
-//			for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
-
-//			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
-//			solver.eigenvalues();
-//			solver.eigenvectors();
-
-//			const auto eval = solver.eigenvalues();
-//			const auto evec = solver.eigenvectors();
-//			Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
-
-//			Eigen::Matrix3f rotMat;
-//			rotMat.col(0) = evec.col(0);
-//			rotMat.col(1) = evec.col(1);
-//			rotMat.col(2) = evec.col(2);		// 0,0,1 (z-axis) belongs to the strongest eigenvalue
-//			rotMat.transposeInPlace();
-
-//			//Eigen::Vector3f gy; gy << 0, 30, 30;
-//			Eigen::Vector3f avg1 = rotMat * avg;
-//			int i = 0; (void) i;
-
-//			reset();
-
-//			return rotMat;
-
-//		}
-
-//		void reset() {
-//			cnt = 0;
-//			avg = Eigen::Vector3f::Zero();
-//			covar = Eigen::Matrix3f::Zero();
-//		}
-
-
-//	} pca1;
-
-
-
-
-//	/** estimate the smartphones current holding position */
-//	void estimateHolding() {
-
-//		Eigen::Vector3f avg = Eigen::Vector3f::Zero();
-//		Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
-
-//		for (const AccelerometerData& acc : accData) {
-////		for (const GyroscopeData& acc : gyroData) {
-//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
-////			Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
-//			avg += vec;
-//			covar += vec * vec.transpose();
-//		}
-
-//		avg /= accData.size();		// TODO
-//		covar /= accData.size();	//TODO
-
-//		avgAcc = avg.normalized();
-
-
-
-
-
-
-
-
-
-////		static K::Gnuplot gp;
-////		gp << "set view equal xyz\n";
-////		gp << "set xrange[-1:+1]\n";
-////		gp << "set yrange[-1:+1]\n";
-////		gp << "set zrange[-1:+1]\n";
-
-////		K::GnuplotSplot plot;
-////		K::GnuplotSplotElementLines lines; plot.add(&lines);
-
-////		K::GnuplotPoint3 p0(0,0,0);
-////		K::GnuplotPoint3 px(evec(0,0), evec(1,0), evec(2,0)); //px = px * eval(0);
-////		K::GnuplotPoint3 py(evec(0,1), evec(1,1), evec(2,1)); //py = py * eval(1);
-////		K::GnuplotPoint3 pz(evec(0,2), evec(1,2), evec(2,2)); //pz = pz * eval(2);
-
-////		K::GnuplotPoint3 pa(avg(0), avg(1), avg(2));
-
-////		lines.addSegment(p0, px);
-////		lines.addSegment(p0, py);
-////		lines.addSegment(p0, pz);
-////		lines.addSegment(p0, pa);
-
-////		gp.draw(plot);
-////		gp.flush();
-
-//	}
-
 };

 #endif // TURNDETECTION_H
--- a/sensors/imu/TurnDetectionPlot.h
+++ b/sensors/imu/TurnDetectionPlot.h
@@ -0,0 +1,118 @@
+#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H
+#define INDOOR_IMU_TURNDETECTIONPLOT_H
+
+#ifdef WITH_DEBUG_PLOT
+
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+
+	#include "GyroscopeData.h"
+	#include "../../data/Timestamp.h"
+	#include "../../math/Matrix3.h"
+
+	class TurnDetectionPlot {
+
+		Timestamp plotRef;
+		Timestamp lastPlot;
+
+		K::Gnuplot gp1;
+
+		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
+
+		K::GnuplotPlot plotGyroRaw;
+		K::GnuplotPlotElementLines lineGyroRawX;
+		K::GnuplotPlotElementLines lineGyroRawY;
+		K::GnuplotPlotElementLines lineGyroRawZ;
+
+		K::GnuplotPlot plotGyroFix;
+		K::GnuplotPlotElementLines lineGyroFixX;
+		K::GnuplotPlotElementLines lineGyroFixY;
+		K::GnuplotPlotElementLines lineGyroFixZ;
+
+		K::GnuplotSplot plotPose;
+		K::GnuplotSplotElementLines linePose;
+
+		float plotCurHead = 0;
+
+	public:
+
+		TurnDetectionPlot() {
+
+			gp1 << "set autoscale xfix\n";
+			gp1 << "set view equal xyz\n";
+
+			multiplot.add(&plotGyroRaw);
+			multiplot.add(&plotGyroFix);
+
+			plotGyroRaw.setTitle("Gyroscope (raw)");
+			plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
+			plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
+			plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");
+
+			plotGyroFix.setTitle("Gyroscope (fixed)");
+			plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
+			plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
+			plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");
+
+			plotPose.setTitle("Pose");
+			plotPose.getView().setEnabled(false);
+			plotPose.add(&linePose);
+			plotPose.getAxisX().setRange(-5,+5);
+			plotPose.getAxisY().setRange(-5,+5);
+			plotPose.getAxisZ().setRange(-5,+5);
+
+		}
+
+		void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
+
+			plotCurHead += delta;
+
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
+
+			// raw gyro
+			lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
+			lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
+			lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );
+
+			// adjusted gyro
+			lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) );
+			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
+			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineGyroRawX.removeIf(remove);
+				lineGyroRawY.removeIf(remove);
+				lineGyroRawZ.removeIf(remove);
+				lineGyroFixX.removeIf(remove);
+				lineGyroFixY.removeIf(remove);
+				lineGyroFixZ.removeIf(remove);
+
+				const float ax = 0.85 + std::cos(plotCurHead)*0.1;
+				const float ay = 0.85 + std::sin(plotCurHead)*0.1;
+				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
+				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
+
+				gp1.draw(multiplot);
+				gp1.flush();
+
+			}
+
+		}
+
+	};
+
+#endif
+
+#endif // INDOOR_IMU_TURNDETECTIONPLOT_H
--- a/sensors/offline/FilePlayer.h
+++ b/sensors/offline/FilePlayer.h
@@ -83,6 +83,10 @@ namespace Offline {
 			// get all sensor events from the offline file
 			const std::vector<Entry> events = reader->getEntries();

+			// reference time (system vs. first-event)
+			Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
+			Timestamp tsRef2 = Timestamp::fromRunningTime();
+
 			// process every event
 			for (const Entry& e : events) {

@@ -92,21 +96,30 @@ namespace Offline {
 				// timestamp
 				const Timestamp ts = Timestamp::fromMS(e.ts);

+				// ensure events happen occur fast as they did during recording
+				if (realtime) {
+					const Timestamp ts1 = ts-tsRef1;
+					const Timestamp ts2 = Timestamp::fromRunningTime() - tsRef2;
+					const Timestamp diff = ts1-ts2;
+					if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
+				}
+
 				// event index
 				const size_t idx = e.idx;

 #warning "some sensors todo:"
 				switch(e.type) {
-				    case Sensor::ACC:		listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
-				    case Sensor::BARO:		listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
-				    case Sensor::BEACON:	break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
-				    case Sensor::COMPASS:	listener->onCompass(ts, reader->getCompass()[idx].data); break;
-				    case Sensor::GPS:		listener->onGPS(ts, reader->getGPS()[idx].data); break;
-				    case Sensor::GRAVITY:	listener->onGravity(ts, reader->getGravity()[idx].data); break;
-				    case Sensor::GYRO:		listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
-				    case Sensor::LIN_ACC:	break;//listener->on(ts, reader->getBarometer()[idx].data); break;
-				    case Sensor::WIFI:		listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
-				    default:				throw Exception("code error. found not-yet-implemented sensor");
+					case Sensor::ACC:			listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
+					case Sensor::BARO:			listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
+					case Sensor::BEACON:		break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
+					case Sensor::COMPASS:		listener->onCompass(ts, reader->getCompass()[idx].data); break;
+					case Sensor::MAGNETOMETER:	listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
+					case Sensor::GPS:			listener->onGPS(ts, reader->getGPS()[idx].data); break;
+					case Sensor::GRAVITY:		listener->onGravity(ts, reader->getGravity()[idx].data); break;
+					case Sensor::GYRO:			listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
+					case Sensor::LIN_ACC:		break;//listener->on(ts, reader->getBarometer()[idx].data); break;
+					case Sensor::WIFI:			listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
+					default:					throw Exception("code error. found not-yet-implemented sensor");
 				}

 			}
--- a/sensors/offline/FileReader.h
+++ b/sensors/offline/FileReader.h
@@ -16,6 +16,7 @@
 #include "../../sensors/beacon/BeaconMeasurements.h"
 #include "../../sensors/gps/GPSData.h"
 #include "../../sensors/imu/CompassData.h"
+#include "../../sensors/imu/MagnetometerData.h"

 #include "../../geo/Point2.h"
 #include "../../grid/factory/v2/GridFactory.h"
@@ -51,6 +52,7 @@ namespace Offline {
 		std::vector<TS<GravityData>> gravity;
 		std::vector<TS<GPSData>> gps;
 		std::vector<TS<CompassData>> compass;
+		std::vector<TS<MagnetometerData>> magnetometer;

 		/** all entries in linear order as they appeared while recording */
 		std::vector<Entry> entries;
@@ -88,6 +90,7 @@ namespace Offline {
 			barometer.clear();
 			lin_acc.clear();
 			gravity.clear();
+			magnetometer.clear();
 		}

 		const std::vector<Entry>& getEntries() const {return entries;}
@@ -113,6 +116,8 @@ namespace Offline {

 		const std::vector<TS<GravityData>>& getGravity() const {return gravity;}

+		const std::vector<TS<MagnetometerData>>& getMagnetometer() const {return magnetometer;}
+
 		/** get an interpolateable ground-truth based on the time-clicks during recording */
 		GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {

@@ -147,7 +152,7 @@ namespace Offline {
 		void parse(const std::string& file) {

 			std::ifstream inp(file);
-			if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
+			if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file: " + file);}

 			while(!inp.eof() && !inp.bad()) {

@@ -172,6 +177,7 @@ namespace Offline {
 				else if (idx == (int)Sensor::GRAVITY)		{parseGravity(ts,data);}
 				else if (idx == (int)Sensor::COMPASS)		{parseCompass(ts,data);}
 				else if (idx == (int)Sensor::GPS)			{parseGPS(ts,data);}
+				else if (idx == (int)Sensor::MAGNETOMETER)	{parseMagnetometer(ts,data);}

 				// TODO: this is a hack...
 				// the loop is called one additional time after the last entry
@@ -343,6 +349,24 @@ namespace Offline {

 		}

+		void parseMagnetometer(const uint64_t ts, const std::string& data) {
+
+			MagnetometerData mag;
+			Splitter s(data, sep);
+
+			mag.x = s.has(0) ? (s.getFloat(0)) : (NAN);
+			mag.y = s.has(1) ? (s.getFloat(1)) : (NAN);
+			mag.z = s.has(2) ? (s.getFloat(2)) : (NAN);
+
+			TS<MagnetometerData> elem(ts, mag);
+			this->magnetometer.push_back(elem);
+			entries.push_back(Entry(Sensor::MAGNETOMETER, ts, this->magnetometer.size()-1));
+
+			// inform listener
+			//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
+
+		}
+
 		/** parse the given GPS entry */
 		void parseGPS(const uint64_t ts, const std::string& data) {

--- a/sensors/offline/Listener.h
+++ b/sensors/offline/Listener.h
@@ -2,11 +2,15 @@
 #define OFFLINE_LISTENER_H

 #include "../gps/GPSData.h"
+
+#include "../pressure/BarometerData.h"
+
 #include "../imu/CompassData.h"
 #include "../imu/GravityData.h"
-#include "../pressure/BarometerData.h"
 #include "../imu/GyroscopeData.h"
 #include "../imu/AccelerometerData.h"
+#include "../imu/MagnetometerData.h"
+
 #include "../radio/WiFiMeasurements.h"

 namespace Offline {
@@ -25,6 +29,7 @@ namespace Offline {
 		virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
 		virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
 		virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
+		virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) = 0;

 	};

--- a/sensors/offline/Sensors.h
+++ b/sensors/offline/Sensors.h
@@ -8,6 +8,7 @@ namespace Offline {
 		GRAVITY = 1,
 		LIN_ACC = 2,
 		GYRO = 3,
+		MAGNETOMETER = 4,
 		BARO = 5,
 		COMPASS = 6,		// also called "orientatioN"
 		WIFI = 8,
--- a/sensors/radio/AccessPoint.h
+++ b/sensors/radio/AccessPoint.h
@@ -58,6 +58,13 @@ public:
 	/** OPTIONAL: get the AP's ssid (if any) */
 	inline const std::string& getSSID() const {return ssid;}

+	/** as string for debuging */
+	std::string asString() const {
+		std::string res = "AP(" + mac.asString();
+		if (!ssid.empty()) {res += ", '" + ssid + "'";}
+		res += ")";
+		return res;
+	}

 };

--- a/sensors/radio/VAPGrouper.h
+++ b/sensors/radio/VAPGrouper.h
@@ -40,6 +40,20 @@ public:

 	};

+	/** how to determine the grouped timestamp */
+	enum class TimeAggregation {
+
+		/** use the smallest timestamp among all grouped APs */
+		MINIMUM,
+
+		/** use the average timestamp among all grouped APs */
+		AVERAGE,
+
+		/** use the maximum timestamp among all grouped APs */
+		MAXIMUM,
+
+	};
+
 private:

 	static constexpr const char* name = "VAPGrp";
@@ -50,14 +64,17 @@ private:
 	/** the signal-strength aggregation algorithm to use */
 	const Aggregation agg;

+	/** how to aggreage the grouped time */
+	const TimeAggregation timeAgg;
+
 	/** respect only outputs with at-least X occurences of one physical hardware [can be used to prevent issues] */
 	int minOccurences;

 public:

 	/** ctor */
-	VAPGrouper(const Mode mode, const Aggregation agg, const int minOccurences = 2) :
-		mode(mode), agg(agg), minOccurences(minOccurences) {
+	VAPGrouper(const Mode mode, const Aggregation agg, const TimeAggregation timeAgg = TimeAggregation::AVERAGE, const int minOccurences = 2) :
+		mode(mode), agg(agg), timeAgg(timeAgg), minOccurences(minOccurences) {
 		;
 	}

@@ -127,6 +144,15 @@ public:

 private:

+	struct FieldRSSI {
+		static float get(const WiFiMeasurement& m) { return m.getRSSI(); }
+	};
+	struct FieldTS {
+		static Timestamp get(const WiFiMeasurement& m) { return m.getTimestamp(); }
+	};
+
+
+
 	/** combine all of the given VAPs into one entry using the configured aggregation method */
 	WiFiMeasurement groupVAPs(const MACAddress& baseMAC, const std::vector<WiFiMeasurement>& vaps) const {

@@ -134,17 +160,27 @@ private:
 		const AccessPoint baseAP(baseMAC);

 		// the resultign timestamp
-		const Timestamp baseTS = vaps.front().getTimestamp();
+		//Timestamp baseTS = vaps.front().getTimestamp();

 		// calculate the rssi using the configured aggregate function
 		float rssi = NAN;
 		switch(agg) {
-			case Aggregation::AVERAGE:	rssi = getAVG(vaps); break;
-			case Aggregation::MEDIAN:	rssi = getMedian(vaps); break;
-			case Aggregation::MAXIMUM:	rssi = getMax(vaps); break;
-			default:					throw Exception("unsupported vap-aggregation method");
+			case Aggregation::AVERAGE:	rssi = getAVG<float, FieldRSSI>(vaps);	break;
+			case Aggregation::MEDIAN:	rssi = getMedian(vaps);					break;
+			case Aggregation::MAXIMUM:	rssi = getMax<float, FieldRSSI>(vaps);	break;
+			default:					throw Exception("unsupported rssi-aggregation method");
 		}

+		// calculate the time using the configured aggregate function
+		Timestamp baseTS;
+		switch(timeAgg) {
+			case TimeAggregation::MINIMUM:	baseTS = getMin<Timestamp, FieldTS>(vaps);	break;
+			case TimeAggregation::AVERAGE:	baseTS = getAVG<Timestamp, FieldTS>(vaps);	break;
+			case TimeAggregation::MAXIMUM:	baseTS = getMax<Timestamp, FieldTS>(vaps);	break;
+			default:					throw Exception("unsupported time-aggregation method");
+		}
+
+
 		// create the result measurement
 		return WiFiMeasurement(baseAP, rssi, baseTS);

@@ -153,13 +189,18 @@ private:
 private:

 	/** get the average signal strength */
-	inline float getAVG(const std::vector<WiFiMeasurement>& vaps) const {
+	template <typename T, typename Field> inline T getAVG(const std::vector<WiFiMeasurement>& vaps) const {

-		float rssi = 0;
+//		T field = T();
+//		for (const WiFiMeasurement& vap : vaps) {
+//			field = field + Field::get(vap);
+//		}
+//		return field / vaps.size();
+		Stats::Average<T> avg;
 		for (const WiFiMeasurement& vap : vaps) {
-			rssi += vap.getRSSI();
+			avg.add(Field::get(vap));
 		}
-		return rssi / vaps.size();
+		return avg.get();

 	}

@@ -174,19 +215,25 @@ private:

 	}

-	/** get the maximum signal strength */
-	inline float getMax(const std::vector<WiFiMeasurement>& vaps) const {
+	/** get the maximum value */
+	template <typename T, typename Field> inline T getMax(const std::vector<WiFiMeasurement>& vaps) const {

-		Stats::Maximum<float> max;
+		Stats::Maximum<T> max;
 		for (const WiFiMeasurement& vap : vaps) {
-			max.add(vap.getRSSI());
+			max.add(Field::get(vap));
 		}
 		return max.get();
-//		float max = -9999999;
-//		for (const WiFiMeasurement& vap : vaps) {
-//			if (vap.getRSSI() > max) {max = vap.getRSSI();}
-//		}
-//		return max;
+
+	}
+
+	/** get the minimum value */
+	template <typename T, typename Field> inline T getMin(const std::vector<WiFiMeasurement>& vaps) const {
+
+		Stats::Minimum<T> min;
+		for (const WiFiMeasurement& vap : vaps) {
+			min.add(Field::get(vap));
+		}
+		return min.get();

 	}

--- a/sensors/radio/WiFiMeasurement.h
+++ b/sensors/radio/WiFiMeasurement.h
@@ -19,7 +19,7 @@ private:
 	/** the measured signal strength */
 	float rssi;

-    /** OPTIONAL. frequence the signal was received */
+	/** OPTIONAL. frequence the signal was received */
 	float freq = NAN;

 	/** OPTIONAL. timestamp the measurement was recorded at */
@@ -32,40 +32,56 @@ public:
 		;
 	}

-    /** ctor with freq*/
-    WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) {
-            ;
-    }
+	/** ctor with freq*/
+	WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) {
+			;
+	}

-    /** ctor with timestamp */
-    WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
-            ;
-    }
+	/** ctor with timestamp */
+	WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), freq(NAN), ts(ts) {
+			;
+	}

-    /** ctor with timestamp and freq*/
-    WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) {
-            ;
-    }
+	/** ctor with timestamp and freq*/
+	WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) {
+			;
+	}

 public:

-    /** get the AP we got the measurement for */
-    const AccessPoint& getAP() const {return ap;}
+	/** get the AP we got the measurement for */
+	const AccessPoint& getAP() const {return ap;}

-    /** get the measurement's signal strength */
-    float getRSSI() const {return rssi;}
+	/** get the measurement's signal strength */
+	float getRSSI() const {return rssi;}

-    /** OPTIONAL: get the measurement's timestamp (if known!) */
-    const Timestamp& getTimestamp() const {return ts;}
+	/** OPTIONAL: get the measurement's timestamp (if known!) */
+	const Timestamp& getTimestamp() const {return ts;}

-    /** OPTIONAL: get the measurement's frequence (if known!) */
-    float getFrequency() const {return freq;}
+	/** timestamp known? */
+	bool hasTimestamp() const {return ts == ts;}

-    /** set another signal strength */
-    void setRssi(float value){rssi = value;}
+	/** OPTIONAL: get the measurement's frequency (if known!) */
+	float getFrequency() const {return freq;}
+
+	/** frequency known? */
+	bool hasFrequency() const {return freq == freq;}
+
+	/** set another signal strength */
+	void setRssi(float value){rssi = value;}
+
+	/** set the timestamp */
+	void setTimestamp(const Timestamp& val){ts = val;}
+
+	/** as string for debug printing */
+	std::string asString() const {
+		std::string res = ap.asString();
+		if (hasTimestamp()) {res += " @" + std::to_string(ts.ms());}
+		if (hasFrequency()) {res += " " + std::to_string((int)freq) + " MHz";}
+		res += " - " + std::to_string(rssi) + " dBm ";
+		return res;
+	}

-    /** set the timestamp */
-    void setTimestamp(const Timestamp& val){ts = val;}
 };


--- a/sensors/radio/WiFiProbabilityFree.h
+++ b/sensors/radio/WiFiProbabilityFree.h
@@ -69,7 +69,7 @@ public:
 			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
 			Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?");

-			Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range");
+                        Assert::isBetween(scanRSSI, -100.0f, -20.0f, "scan-rssi out of range");
 			//Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range");

 			// sigma grows with measurement age
--- a/sensors/radio/scan/WiFiChannels.h
+++ b/sensors/radio/scan/WiFiChannels.h
@@ -0,0 +1,30 @@
+#ifndef INDOOR_WIFICHANNELS_H
+#define INDOOR_WIFICHANNELS_H
+
+class WiFiChannels {
+
+public:
+
+	static int freqToChannel(const int freq) {
+		switch(freq) {
+			case 2412: return 1;
+			case 2417: 	return 2;
+			case 2422: 	return 3;
+			case 2427: 	return 4;
+			case 2432: 	return 5;
+			case 2437: 	return 6;
+			case 2442: 	return 7;
+			case 2447: 	return 8;
+			case 2452: 	return 9;
+			case 2457: 	return 10;
+			case 2462: 	return 11;
+			case 2467: 	return 12;
+			case 2472: 	return 13;
+			case 2484: 	return 14;
+		}
+		throw "error";
+	}
+
+};
+
+#endif // INDOOR_WIFICHANNELS_H
--- a/sensors/radio/scan/WiFiRAW.h
+++ b/sensors/radio/scan/WiFiRAW.h
@@ -0,0 +1,47 @@
+#ifndef INDOOR_WIFIRAW_H
+#define INDOOR_WIFIRAW_H
+
+#include <iostream>
+
+/**
+ * parse raw binary wifi packets as defined within the standard
+ */
+class WiFiRAW {
+
+public:
+
+	enum Tags {
+		TAG_SSID = 0x00
+	};
+
+	struct TaggedParams {
+		std::string ssid;
+	};
+
+	/** parsed tagged params within wifi packets: [tag][len][len-bytes][tag][len][len-bytes]... */
+	static TaggedParams parseTaggedParams(const uint8_t* data, const size_t len) {
+
+		TaggedParams res;
+
+		int pos = 0;
+		while(pos < len) {
+
+			const int tag = data[pos+0];	// the tag-ID
+			const int len = data[pos+1];	// the lenght of the following data
+
+			switch(tag) {
+				case TAG_SSID:		res.ssid = std::string( (const char*) &(data[pos+2]), len); break;
+			}
+
+			// position at the start of the next tag
+			pos += 1 + 1 + len;
+
+		}
+
+		return res;
+
+	}
+
+};
+
+#endif // INDOOR_WIFIRAW_H
--- a/sensors/radio/scan/WiFiScan.h
+++ b/sensors/radio/scan/WiFiScan.h
@@ -0,0 +1,15 @@
+#ifndef INDOOR_WIFI_SCAN_H
+#define INDOOR_WIFI_SCAN_H
+
+#include "../WiFiMeasurements.h"
+
+class WiFiScan {
+
+public:
+
+	/** scan for nearby access points */
+	virtual WiFiMeasurements scan() = 0;
+
+};
+
+#endif //INDOOR_WIFI_SCAN_H
--- a/sensors/radio/scan/WiFiScanLinux.h
+++ b/sensors/radio/scan/WiFiScanLinux.h
@@ -0,0 +1,547 @@
+#ifndef INDOOR_WIFISCANNER_LINUX_H
+#define INDOOR_WIFISCANNER_LINUX_H
+
+/*
+ * scan_access_points.c: Prints all detected access points with wlan0 using NL80211 (netlink).
+ *
+ * Only works on network interfaces whose drivers are compatible with Netlink. Test this by running `iw list`.
+ *
+ * Since only privileged users may submit NL80211_CMD_TRIGGER_SCAN, you'll have to run the compiled program as root.
+ *
+ * Build with: gcc $(pkg-config --cflags --libs libnl-genl-3.0) scan_access_points.c
+ *
+ * Raspbian prerequisites:
+ *      sudo apt-get install libnl-genl-3-dev
+ *
+ * Resources:
+ *      http://git.kernel.org/cgit/linux/kernel/git/jberg/iw.git/tree/scan.c
+ *      http://stackoverflow.com/questions/21601521/how-to-use-the-libnl-library-to-trigger-nl80211-commands
+ *      http://stackoverflow.com/questions/23760780/how-to-send-single-channel-scan-request-to-libnl-and-receive-single-
+ */
+#include <errno.h>
+#include <netlink/errno.h>
+#include <netlink/netlink.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+#include <linux/nl80211.h>
+#include <net/if.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <string>
+#include <thread>
+
+#include "WiFiRAW.h"
+#include "WiFiScan.h"
+
+class WiFiScanLinux : public WiFiScan {
+
+	struct TMP {
+		Timestamp tsStart;
+		WiFiMeasurements res;
+	};
+
+	struct trigger_results {
+		int done;
+		int aborted;
+	};
+
+	struct handler_args {  // For family_handler() and nl_get_multicast_id().
+		const char *group;
+		int id;
+	};
+
+	static int error_handler(struct sockaddr_nl* nla, struct nlmsgerr* err, void* arg) {
+		(void) nla;
+		// Callback for errors.
+		printf("error_handler() called.\n");
+		int* ret = (int*) arg;
+		*ret = err->error;
+		return NL_STOP;
+	}
+
+	static int finish_handler(struct nl_msg* msg, void* arg) {
+		(void) msg;
+		// Callback for NL_CB_FINISH.
+		int* ret = (int*) arg;
+		*ret = 0;
+		return NL_SKIP;
+	}
+
+	static int ack_handler(struct nl_msg* msg, void* arg) {
+		(void) msg;
+		// Callback for NL_CB_ACK.
+		int* ret = (int*)arg;
+		*ret = 0;
+		return NL_STOP;
+	}
+
+	static int no_seq_check(struct nl_msg* msg, void* arg) {
+		(void) msg;
+		(void) arg;
+		// Callback for NL_CB_SEQ_CHECK.
+		return NL_OK;
+	}
+
+	static int family_handler(struct nl_msg* msg, void* arg) {
+		// Callback for NL_CB_VALID within nl_get_multicast_id(). From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html.
+		struct handler_args* grp = (struct handler_args*) arg;
+		struct nlattr *tb[CTRL_ATTR_MAX + 1];
+		struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg));
+		struct nlattr *mcgrp;
+		int rem_mcgrp;
+
+		nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
+
+		if (!tb[CTRL_ATTR_MCAST_GROUPS]) return NL_SKIP;
+
+		nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {  // This is a loop.
+			struct nlattr* tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
+
+			nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX, (struct nlattr*) nla_data(mcgrp), nla_len(mcgrp), NULL);
+
+			if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] || !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]) continue;
+			if (strncmp((const char*) nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]), grp->group,
+					nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]))) {
+				continue;
+					}
+
+			grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
+			break;
+		}
+
+		return NL_SKIP;
+	}
+
+
+	int nl_get_multicast_id(struct nl_sock *sock, const char *family, const char *group) {
+
+		// From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html.
+		struct nl_msg *msg;
+		struct nl_cb *cb;
+		int ret, ctrlid;
+
+		//struct handler_args grp = { .group = group, .id = -ENOENT, };
+		struct handler_args grp;
+		grp.group = group;
+		grp.id = -ENOENT;
+
+		msg = nlmsg_alloc();
+		if (!msg) return -ENOMEM;
+
+		cb = nl_cb_alloc(NL_CB_DEFAULT);
+		if (!cb) {
+			ret = -ENOMEM;
+			goto out_fail_cb;
+		}
+
+		ctrlid = genl_ctrl_resolve(sock, "nlctrl");
+
+		genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
+
+		ret = -ENOBUFS;
+		NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
+
+		ret = nl_send_auto_complete(sock, msg);
+		if (ret < 0) goto out;
+
+		ret = 1;
+
+		nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret);
+		nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret);
+		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, family_handler, &grp);
+
+		while (ret > 0) nl_recvmsgs(sock, cb);
+
+		if (ret == 0) ret = grp.id;
+
+		nla_put_failure:
+			out:
+				nl_cb_put(cb);
+			out_fail_cb:
+				nlmsg_free(msg);
+				return ret;
+	}
+
+
+	static int callback_trigger(struct nl_msg *msg, void *arg) {
+
+		// Called by the kernel when the scan is done or has been aborted.
+		struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg));
+		struct trigger_results* results = (struct trigger_results*) arg;
+
+		//printf("Got something.\n");
+		//printf("%d\n", arg);
+		//nl_msg_dump(msg, stdout);
+
+		if (gnlh->cmd == NL80211_CMD_SCAN_ABORTED) {
+			printf("Got NL80211_CMD_SCAN_ABORTED.\n");
+			results->done = 1;
+			results->aborted = 1;
+		} else if (gnlh->cmd == NL80211_CMD_NEW_SCAN_RESULTS) {
+			printf("Got NL80211_CMD_NEW_SCAN_RESULTS.\n");
+			results->done = 1;
+			results->aborted = 0;
+		}  // else probably an uninteresting multicast message.
+
+		return NL_SKIP;
+	}
+
+
+
+	static int addResult(struct nl_msg* msg, void* arg) {
+
+		TMP* tmp = (TMP*) arg;
+
+		// Called by the kernel with a dump of the successful scan's data. Called for each SSID.
+		struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg));
+	//	char mac_addr[20];
+		struct nlattr *tb[NL80211_ATTR_MAX + 1];
+		struct nlattr *bss[NL80211_BSS_MAX + 1];
+
+		static struct nla_policy bss_policy[NL80211_BSS_MAX + 1];
+		memset(&bss_policy, 0, sizeof(bss_policy));
+			bss_policy[NL80211_BSS_TSF].type = NLA_U64;
+			bss_policy[NL80211_BSS_FREQUENCY].type = NLA_U32;
+//			bss_policy[NL80211_BSS_BSSID] = { };
+			bss_policy[NL80211_BSS_BEACON_INTERVAL].type = NLA_U16;
+			bss_policy[NL80211_BSS_CAPABILITY].type = NLA_U16;
+//			bss_policy[NL80211_BSS_INFORMATION_ELEMENTS] = { };
+			bss_policy[NL80211_BSS_SIGNAL_MBM].type = NLA_U32;
+			bss_policy[NL80211_BSS_SIGNAL_UNSPEC].type = NLA_U8;
+			bss_policy[NL80211_BSS_STATUS].type = NLA_U32;
+			bss_policy[NL80211_BSS_SEEN_MS_AGO].type = NLA_U32;
+//			bss_policy[NL80211_BSS_BEACON_IES] = { };
+
+
+		// Parse and error check.
+		nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
+		if (!tb[NL80211_ATTR_BSS]) {
+			printf("bss info missing!\n");
+			return NL_SKIP;
+		}
+		if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], bss_policy)) {
+			printf("failed to parse nested attributes!\n");
+			return NL_SKIP;
+		}
+		if (!bss[NL80211_BSS_BSSID]) return NL_SKIP;
+		if (!bss[NL80211_BSS_INFORMATION_ELEMENTS]) return NL_SKIP;
+
+		const uint64_t seen_ago_ms = nla_get_u32(bss[NL80211_BSS_SEEN_MS_AGO]);
+		const int rssi = (nla_get_s32(bss[NL80211_BSS_SIGNAL_MBM])) / 100.0f;
+
+//		// Start printing.
+//		mac_addr_n2a(mac_addr, (unsigned char*) nla_data(bss[NL80211_BSS_BSSID]));
+//		printf("%s, ", mac_addr);
+//		printf("%d MHz, ", nla_get_u32(bss[NL80211_BSS_FREQUENCY]));
+//		//print_ssid((unsigned char*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]));
+//		printf(" %d ms", seen_ago_ms);
+//		printf(" %d dBm", rssi);
+//		printf("\n");
+
+		WiFiRAW::TaggedParams params = WiFiRAW::parseTaggedParams(
+			(const uint8_t*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]),
+			nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS])
+		);
+
+		const Timestamp ts = Timestamp::fromUnixTime() - Timestamp::fromMS(seen_ago_ms);
+		const int freq_MHz = nla_get_u32(bss[NL80211_BSS_FREQUENCY]);
+		const uint8_t* macPtr = (const uint8_t*) nla_data(bss[NL80211_BSS_BSSID]);
+		const uint64_t macLng =  ((uint64_t)macPtr[5]<<40)|((uint64_t)macPtr[4]<<32)|((uint64_t)macPtr[3]<<24)|((uint64_t)macPtr[2]<<16)|((uint64_t)macPtr[1]<<8)|((uint64_t)macPtr[0]<<0);
+		const MACAddress mac(macLng);
+		const AccessPoint ap(mac, params.ssid);
+		const WiFiMeasurement mes(ap, rssi, freq_MHz, ts);
+
+		// by default, linux also lists older scan results
+		// remove them here!
+		if (ts > tmp->tsStart) {
+			//std::cout << seen_ago_ms << std::endl;
+			tmp->res.entries.push_back(mes);
+			std::cout << mes.asString() << std::endl;
+		}
+
+
+
+		return NL_SKIP;
+
+	}
+
+
+//	int do_scan_trigger(struct nl_sock *socket, int if_index, int driver_id) {
+
+//		// Starts the scan and waits for it to finish. Does not return until the scan is done or has been aborted.
+//		struct trigger_results results;
+//		results.done = 0;
+//		results.aborted = 0;
+
+//		struct nl_msg *msg;
+//		struct nl_cb *cb;
+//		struct nl_msg *ssids_to_scan;
+//		int err;
+//		int ret;
+//		int mcid = nl_get_multicast_id(socket, "nl80211", "scan");
+//		nl_socket_add_membership(socket, mcid);  // Without this, callback_trigger() won't be called.
+
+//		// Allocate the messages and callback handler.
+//		msg = nlmsg_alloc();
+//		if (!msg) {throw Exception("Failed to allocate netlink message for msg.");}
+
+//		ssids_to_scan = nlmsg_alloc();
+//		if (!ssids_to_scan) {
+//			nlmsg_free(msg);
+//			throw Exception("Failed to allocate netlink message for ssids_to_scan.");
+//		}
+
+//		cb = nl_cb_alloc(NL_CB_DEFAULT);
+//		if (!cb) {
+//			nlmsg_free(msg);
+//			nlmsg_free(ssids_to_scan);
+//			throw Exception("Failed to allocate netlink callbacks.");
+//		}
+
+//		// Setup the messages and callback handler.
+//		genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0);  // Setup which command to run.
+//		nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index);  // Add message attribute, which interface to use.
+//		nla_put(ssids_to_scan, 1, 0, "");  // Scan all SSIDs.
+//		nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan);  // Add message attribute, which SSIDs to scan for.
+//		nlmsg_free(ssids_to_scan);  // Copied to `msg` above, no longer need this.
+//		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results);  // Add the callback.
+//		nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
+//		nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
+//		nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
+//		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);  // No sequence checking for multicast messages.
+
+//		// Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on
+//		// success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process.
+//		err = 1;
+//		ret = nl_send_auto(socket, msg);  // Send the message.
+//		printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret);
+//		printf("Waiting for scan to complete...\n");
+//		while (err > 0) ret = nl_recvmsgs(socket, cb);  // First wait for ack_handler(). This helps with basic errors.
+//		if (err < 0) {
+//			printf("WARNING: err has a value of %d.\n", err);
+//		}
+//		if (ret < 0) {
+//			printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret));
+//			return ret;
+//		}
+//		while (!results.done) nl_recvmsgs(socket, cb);  // Now wait until the scan is done or aborted.
+//		if (results.aborted) {
+//			printf("ERROR: Kernel aborted scan.\n");
+//			return 1;
+//		}
+//		printf("Scan is done.\n");
+
+//		// Cleanup.
+//		nlmsg_free(msg);
+//		nl_cb_put(cb);
+//		nl_socket_drop_membership(socket, mcid);  // No longer need this.
+//		return 0;
+//	}
+
+
+	int if_index;
+	struct nl_sock* socket;
+	int driver_id;
+
+
+	struct nl_cb *cb;
+	int mcid;
+
+	int err;
+
+	struct trigger_results results;
+
+	/** configure all needed callback (from netlink to code) once */
+	void setupOnce() {
+
+		mcid = nl_get_multicast_id(socket, "nl80211", "scan");
+		nl_socket_add_membership(socket, mcid);  // Without this, callback_trigger() won't be called.
+
+		cb = nl_cb_alloc(NL_CB_DEFAULT);
+		if (!cb) {throw Exception("Failed to allocate netlink callbacks.");}
+
+		// Setup the messages and callback handler.
+		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results);  // Add the callback.
+		nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
+		nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
+		nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
+		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);  // No sequence checking for multicast messages.
+
+	}
+
+	/** triggers a new scan within the wifi hardware */
+	void triggerNewScan() {
+
+		std::cout << "triggerNewScan()" << std::endl;
+
+		struct nl_msg *ssids_to_scan;
+		ssids_to_scan = nlmsg_alloc();
+		if (!ssids_to_scan) {throw Exception("Failed to allocate netlink message for ssids_to_scan.");}
+		nla_put(ssids_to_scan, 1, 0, "");  // Scan all SSIDs.
+
+		// construct message
+		struct nl_msg* msg = nlmsg_alloc();
+		if (!msg) {throw Exception("Failed to allocate netlink message for msg.");}
+
+		genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0);  // Setup which command to run.
+		nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index);  // Add message attribute, which interface to use.
+		nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan);  // Add message attribute, which SSIDs to scan for.
+		nlmsg_free(ssids_to_scan);  // Copied to `msg` above, no longer need this.
+
+		results.done = 0;
+		results.aborted = 0;
+
+		// trigger scan by sending the constructed message
+		const int ret = nl_send_auto(socket, msg);  // Send the message.
+		printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret);
+		printf("Waiting for scan to complete...\n");
+		nlmsg_free(msg);
+
+	}
+
+	/** blocks until the scan-result is available. true if OK, false otherwise */
+	bool waitForScanResult() {
+
+		// Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on
+		// success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process.
+		err = 0;
+//		ret = nl_send_auto(socket, msg);  // Send the message.
+//		printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret);
+//		printf("Waiting for scan to complete...\n");
+//		while (err > 0) ret = nl_recvmsgs(socket, cb);  // First wait for ack_handler(). This helps with basic errors.
+//		if (err < 0) {
+//			printf("WARNING: err has a value of %d.\n", err);
+//		}
+
+		while(true) {
+			const int ret = nl_recvmsgs(socket, cb);
+			printf("-- ret: %d err: %d \n", ret, err);
+			if (results.done) {
+				return true;
+			}
+			if (ret < 0 || err < 0) {
+				nl_recvmsgs(socket, cb);	// seems to fix issues when device is busy?!
+				printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret));
+				return false;
+			}
+		}
+
+	}
+
+	void scanResult(TMP* res) {
+
+		// Now get info for all SSIDs detected.
+		struct nl_msg *msg = nlmsg_alloc();  // Allocate a message.
+		genlmsg_put(msg, 0, 0, driver_id, 0, NLM_F_DUMP, NL80211_CMD_GET_SCAN, 0);  // Setup which command to run.
+		nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index);  // Add message attribute, which interface to use.
+		nl_socket_modify_cb(socket, NL_CB_VALID, NL_CB_CUSTOM, addResult, res);  // Add the callback and the measurements to fill
+		int ret = nl_send_auto(socket, msg);  // Send the message.
+		printf("NL80211_CMD_GET_SCAN sent %d bytes to the kernel.\n", ret);
+		ret = nl_recvmsgs_default(socket);  // Retrieve the kernel's answer. callback_dump() prints SSIDs to stdout.
+		nlmsg_free(msg);
+		if (ret < 0) {
+			printf("ERROR: nl_recvmsgs_default() returned %d (%s).\n", ret, nl_geterror(-ret));
+			throw "error";
+		}
+
+	}
+
+	void scanCleanup() {
+
+		// Cleanup.
+		//nlmsg_free(msg);
+		nl_cb_put(cb);
+		nl_socket_drop_membership(socket, mcid);  // No longer need this.
+		//return 0;
+
+	}
+
+
+
+
+
+public:
+
+	WiFiScanLinux(const std::string& devName) {
+
+		// convert interface-name to interface-index
+		if_index = if_nametoindex(devName.c_str());
+
+		// Open socket to kernel.
+		socket = nl_socket_alloc();							// Allocate new netlink socket in memory.
+		genl_connect(socket);								// Create file descriptor and bind socket.
+		driver_id = genl_ctrl_resolve(socket, "nl80211");	// Find the nl80211 driver ID.
+
+		setupOnce();
+
+	}
+
+	~WiFiScanLinux() {
+
+		// cleanup
+		nl_socket_free(socket);
+
+	}
+
+
+
+	/** triger WiFiScan and fetch the result */
+	WiFiMeasurements scan() {
+
+
+		TMP res;
+
+
+		// Issue NL80211_CMD_TRIGGER_SCAN to the kernel and wait for it to finish.
+//		while(true) {
+
+//			// use the current timestamp to suppress older scan results
+//			// which are cached by linux by default
+//			res.tsStart = Timestamp::fromUnixTime();
+
+//			// trigger a scan
+//			//int err = do_scan_trigger(socket, if_index, driver_id);
+//			int err = scanTrigger();
+////			if (err == -25)	{std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue;}				// currently busy. try again
+////			if (err != 0)	{throw Exception("do_scan_trigger() failed with code: " + std::to_string(err));}
+////			break;
+
+//		}
+
+		again:;
+
+		triggerNewScan();
+
+		std::cout << "scan triggered" << std::endl;
+
+		if (waitForScanResult()) {
+
+			std::cout << "scan done" << std::endl;
+			scanResult(&res);
+
+			// return constructed result
+			return res.res;
+
+
+		} else {
+
+			std::this_thread::sleep_for(std::chrono::milliseconds(100));
+			goto again;
+
+		}
+
+
+
+
+	}
+
+
+
+};
+
+
+// gcc -I /usr/include/libnl3 main.c -lnl-genl-3 -lnl-3
+
+
+
+#endif // INDOOR_WIFISCANNER_LINUX_H
--- a/sensors/radio/scan/WiFiScanLinux_IW.h
+++ b/sensors/radio/scan/WiFiScanLinux_IW.h
@@ -0,0 +1,100 @@
+#ifndef INDOOR_WIFI_SCAN_LINUX_H
+#define INDOOR_WIFI_SCAN_LINUX_H
+
+#include "WiFiChannels.h"
+#include "WiFiScan.h"
+#include "../WiFiMeasurements.h"
+
+#include <stdio.h>
+#include <time.h>
+#include <iwlib.h>
+#include <string>
+#include <iostream>
+
+class WiFiScanLinux : public WiFiScan {
+
+private:
+
+	wireless_scan_head head;
+	wireless_scan *result;
+	iwrange range;
+	int sock;
+
+	std::string dev;
+
+public:
+
+	WiFiScanLinux(const std::string& dev) : dev(dev) {
+
+		/* Open socket to kernel */
+		sock = iw_sockets_open();
+		std::cout << sock << std::endl;
+
+	}
+
+	WiFiMeasurements scan() override {
+
+		WiFiMeasurements res;
+		char* dev = (char*) this->dev.c_str();
+
+		/* Get some metadata to use for scanning */
+		if (iw_get_range_info(sock, dev, &range) < 0) {
+			printf("Error during iw_get_range_info. Aborting.\n");
+			exit(2);
+		}
+		if (range.we_version_compiled < 14) {
+			printf("scanning not supported");
+			exit(2);
+		}
+
+//		// params
+//		struct iwreq request;
+//		request.u.param.flags = IW_SCAN_DEFAULT;
+//		request.u.param.value = 0;
+//		if (iw_set_ext(sock, dev, SIOCSIWSCAN, &request) == -1) {
+//			printf("iw_set_ext(SIOCSIWSCAN)");
+//			exit(EXIT_FAILURE);
+//		}
+
+		/* Perform the scan */
+		if (iw_scan(sock, dev, range.we_version_compiled, &head) < 0) {
+			printf("Error during iw_scan. Aborting.\n");
+			exit(2);
+		}
+
+		/* Traverse the results */
+		result = head.result;
+		while (NULL != result) {
+
+			// access-point's MAC
+			const uint8_t* macPtr = (const uint8_t*) result->ap_addr.sa_data;
+			const uint64_t macLng =  ((uint64_t)macPtr[5]<<40)|((uint64_t)macPtr[4]<<32)|((uint64_t)macPtr[3]<<24)|((uint64_t)macPtr[2]<<16)|((uint64_t)macPtr[1]<<8)|((uint64_t)macPtr[0]<<0);
+			const MACAddress mac(macLng);
+
+			const int8_t rssi = result->stats.qual.level;
+			const std::string ssid = result->b.essid;
+
+			result->b.
+
+			const int freq = (result->b.freq)/10e5;
+			const int channel = WiFiChannels::freqToChannel(freq);
+			//std::cout << ssid << "\t" << "freq: " << freq << "\t" << "rssi: " << (int) (rssi) << " dBm" << std::endl;
+			//printf("%s - %d\n", result->b.essid,
+			result = result->next;
+
+			std::cout << mac.asString() << "\t" << ssid << "\t" << channel << "\t" << (int)rssi << " dBm" << std::endl;
+
+			AccessPoint ap(mac, ssid);
+
+			WiFiMeasurement mes(ap, rssi);
+			res.entries.push_back(mes);
+
+		}
+
+		return res;
+
+	}
+
+};
+
+#endif //INDOOR_WIFI_SCAN_LINUX_H
--- a/synthetic/SyntheticPath.h
+++ b/synthetic/SyntheticPath.h
@@ -0,0 +1,109 @@
+#ifndef INDOOR_SYNTHETICPATH_H
+#define INDOOR_SYNTHETICPATH_H
+
+#include "../math/Interpolator.h"
+#include "../floorplan/v2/Floorplan.h"
+#include "../floorplan/v2/FloorplanHelper.h"
+
+/** allows interpolation along a synthetic path */
+class SyntheticPath : private Interpolator<float, Point3> {
+
+	using Base = Interpolator<float, Point3>;
+	using Entry = Base::InterpolatorEntry;
+
+public:
+
+	/** create path using the given ground-truth points from the map */
+	void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
+
+		// get all ground-truth points from the map
+		auto gtps = FloorplanHelper::getGroundTruthPoints(map);
+		float dist = 0;
+
+		// create distance-based entries within the interpolator
+		for (int i = 0; i < ids.size(); ++i) {
+			const int id = ids[i];
+			Point3 gtp = gtps[id];
+			if (i >= 1) {
+				Point3 gtpPrev = gtps[id-1];
+				dist += gtpPrev.getDistance(gtp);
+			}
+			Base::add(dist, gtp);
+		}
+
+	}
+
+	/** get all individual entries from the underlying data-structure */
+	const std::vector<Entry>& getEntries() const {
+		return Base::getEntries();
+	}
+
+	/** smooth harsh angles */
+	void smooth(float delta = 1, int numRuns = 1) {
+
+		float t = delta*2;
+
+		for (int j = 0; j < numRuns; ++j) {
+
+			t/=2;
+
+			for (int i = 1; i < (int)entries.size()-1; ++i) {
+
+				// the entry to-be-replaced by two others
+				const Entry& e = entries[i];
+				const float key = e.key;
+
+				const Entry e1(key-t, Base::get(key-t));
+				const Entry e2(key+t, Base::get(key+t));
+
+				entries.erase(entries.begin()+i); --i;
+				++i; entries.insert(entries.begin()+i, e1);
+				++i; entries.insert(entries.begin()+i, e2);
+
+			}
+
+		}
+
+	}
+
+//	/** smooth harsh angles */
+//	void smooth(float delta = 1, int numRuns = 1) {
+
+//		float t = delta/numRuns;
+
+//		for (int i = 1; i < (int)entries.size()-1; ++i) {
+
+//			// the entry to-be-replaced by several others
+//			const Entry& e = entries[i];
+//			const float key = e.key;
+
+//			std::vector<Entry> newEntries;
+//			for (int x = -numRuns; x <= +numRuns; x+= 2) {
+//				const float percent = (float)x / (float)numRuns;
+//				const float keyO = key + percent*t;
+//				const Point3 pos1 = Base::get(keyO-t*2);
+//				const Point3 pos2 = Base::get(keyO+t*2);
+//				const Point3 posO = (pos1+pos2) / 2;
+//				Entry e(keyO, posO);
+//				newEntries.push_back(e);
+//			}
+
+//			entries.erase(entries.begin()+i); --i;
+
+//			for (const Entry& e : newEntries) {
+//				++i;
+//				entries.insert(entries.begin()+i, e);
+//			}
+
+//		}
+
+//	}
+
+	/** get the position along the path after the given walking distance */
+	Point3 getPosAfterDistance(const float distance) const {
+		return Base::get(distance);
+	}
+
+};
+
+#endif // INDOOR_SYNTEHTICPATH_H
--- a/synthetic/SyntheticSteps.h
+++ b/synthetic/SyntheticSteps.h
@@ -0,0 +1,118 @@
+#ifndef SYNTHETICSTEPS_H
+#define SYNTHETICSTEPS_H
+
+#include "SyntheticWalker.h"
+
+#include "../sensors/imu/AccelerometerData.h"
+
+#include "../math/distribution/Normal.h"
+
+/**
+ * fakes accelerometer-data
+ * based on synthetic walking data
+ */
+class SyntheticSteps : SyntheticWalker::Listener {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0;
+	};
+
+private:
+
+	/** the walker to listen to */
+	SyntheticWalker* walker;
+
+	/** the pedestrian's step-size (in meter) */
+	float stepSize_m = 0.7;
+
+	float lastStepAtDistance = 0;
+
+	Timestamp refStepPattern;
+	Interpolator<Timestamp, AccelerometerData> stepPattern;
+
+	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
+	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
+	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
+
+	int stepPatternPos = -1;
+
+	std::vector<Listener*> listeners;
+
+public:
+
+	/** ctor with the walker to follow */
+	SyntheticSteps(SyntheticWalker* walker) {
+
+		walker->addListener(this);
+		dX.setSeed(1);
+		dY.setSeed(3);
+		dZ.setSeed(5);
+
+		// build the step-pattern (how does a step look-like on the accelerometer)
+		// TODO: switch to MS?! use interpolator?
+//		int duration_ms = 350;
+//		for (int i = 0; i < duration_ms; i += 10) {
+//			float z = std::sin(i*M_PI*2/duration_ms) * 3.0;
+//			if (z < 0) {z *= 0.75;}	// less pronounced in the negative part
+//			float y = std::cos(i*M_PI*2/duration_ms) * 0.5;
+//			const float x = dO.draw();
+//			z += dO.draw()*2;
+//			y += dO.draw();
+//			AccelerometerData acc(x,y,z);
+//			stepPattern.add(Timestamp::fromMS(i), acc);
+//		}
+		stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
+		stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
+		stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
+		stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
+
+	}
+
+	/** attach a listener to this provider */
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+protected:
+
+
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+
+		(void) curPos;
+		const float nextStepAt = (lastStepAtDistance + stepSize_m);
+
+		// 1st, start with random noise on the accelerometer
+		const float x = dX.draw();
+		const float y = dY.draw();
+		const float z = dZ.draw();
+		const AccelerometerData base(0, 4, 9.7);
+		const AccelerometerData noise(x, y, z);
+		AccelerometerData acc = base + noise;
+
+		// is it time to inject a "step" into the accelerometer data?
+		if (walkedDistance > nextStepAt) {
+			lastStepAtDistance = walkedDistance;
+			refStepPattern = walkedTime;
+		}
+
+		// overlay the noise with a step-pattern (see ctor)
+		if (refStepPattern.ms() > 0) {
+			Timestamp curPatPos = walkedTime - refStepPattern;
+			if (curPatPos >= stepPattern.getMaxKey()) {
+				refStepPattern = Timestamp::fromMS(0);
+			} else {
+				const AccelerometerData step = stepPattern.get(curPatPos);
+				acc = base + noise*2.5f + step;
+			}
+		}
+
+		for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}
+
+	}
+
+};
+
+#endif // SYNTHETICSTEPS_H
--- a/synthetic/SyntheticTurns.h
+++ b/synthetic/SyntheticTurns.h
@@ -0,0 +1,127 @@
+#ifndef INDOOR_SYNTHETICTURNS_H
+#define INDOOR_SYNTHETICTURNS_H
+
+#include "SyntheticWalker.h"
+
+#include "../sensors/imu/AccelerometerData.h"
+#include "../sensors/imu/GyroscopeData.h"
+#include "../geo/Heading.h"
+
+#include "../math/distribution/Normal.h"
+
+/**
+ * simulates acceleromter and gyroscope data
+ * based on synthetic walking data
+ *
+ * @brief The SyntheticTurns class
+ */
+class SyntheticTurns : public SyntheticWalker::Listener {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) = 0;
+	};
+
+private:
+
+	/** the walker to listen to */
+	SyntheticWalker* walker;
+
+	Distribution::Normal<float> dAccX = Distribution::Normal<float>(0, 2.5);
+	Distribution::Normal<float> dAccY = Distribution::Normal<float>(0, 1.5);
+	Distribution::Normal<float> dAccZ = Distribution::Normal<float>(0, 1);
+
+	Distribution::Normal<float> dGyroX = Distribution::Normal<float>(0, 0.02);
+	Distribution::Normal<float> dGyroY = Distribution::Normal<float>(0, 0.02);
+	Distribution::Normal<float> dGyroZ = Distribution::Normal<float>(0, 0.02);
+
+	Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
+	Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
+	Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10);	// heading error, slightly biased
+
+
+	std::vector<Listener*> listeners;
+
+public:
+
+	/** ctor with the walker to follow */
+	SyntheticTurns(SyntheticWalker* walker) {
+		walker->addListener(this);
+		dAccX.setSeed(1);
+		dAccY.setSeed(3);
+		dAccZ.setSeed(5);
+		dGyroX.setSeed(7);
+		dGyroY.setSeed(9);
+		dGyroZ.setSeed(11);
+	}
+
+	/** attach a listener to this provider */
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+protected:
+
+	Timestamp lastTs;
+	Heading desiredHead = Heading(0);
+	Heading curHead = Heading(0);
+	Point3 lastPos = Point3(NAN, NAN, NAN);
+	float change;
+
+	inline float clamp(const float val, const float min, const float max) {
+		if (val < min) {return min;}
+		if (val > max) {return max;}
+		return val;
+	}
+
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+
+		// time sine last onWalk();
+		if (lastTs.isZero()) {lastTs = walkedTime; return;}
+		const Timestamp deltaTs = walkedTime - lastTs;
+		lastTs = walkedTime;
+
+		if (lastPos.x != lastPos.x) {
+			lastPos = curPos;
+		} else {
+			desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
+			lastPos = curPos;
+		}
+
+		// difference between current-heading and desired-heading
+		const float diffRad = Heading::getSignedDiff(curHead, desiredHead);
+
+		// slowly change the current heading to match the desired one
+		const float maxChange = dMaxChange.draw();
+		const float toChange = clamp(diffRad, -maxChange, +maxChange);
+		//if (change < toChange) {change += toChange*0.01;}
+		if (change > toChange) {change *= 0.93;}
+		if (change < toChange) {change += dChange.draw()/10000;}
+		//if (change > toChange) {change -= dChange.draw();}
+
+
+
+		curHead += change;
+
+		// convert to gyro's radians-per-second
+		const float radPerSec = change * 1000 / deltaTs.ms();;
+
+		const float accX = 0.00 + dAccX.draw();
+		const float accY = 0.00 + dAccY.draw();
+		const float accZ = 9.81 + dAccZ.draw();
+		AccelerometerData acc(accX, accY, accZ);
+
+		const float gyroX = dGyroX.draw();
+		const float gyroY = dGyroY.draw();
+		const float gyroZ = dGyroZ.draw() + radPerSec;
+		GyroscopeData gyro(gyroX, gyroY, gyroZ);
+
+		for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}
+
+	}
+
+};
+
+#endif // INDOOR_SYNTHETICTURNS_H
--- a/synthetic/SyntheticWalker.h
+++ b/synthetic/SyntheticWalker.h
@@ -0,0 +1,72 @@
+#ifndef SYNTHETICWALKER_H
+#define SYNTHETICWALKER_H
+
+#include "SyntheticPath.h"
+
+/** walk along a path using a known walking speed */
+class SyntheticWalker {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0;
+	};
+
+private:
+
+	/** the path to walk along */
+	SyntheticPath path;
+
+	/** walking-speed in meter per sec */
+	float walkSpeed_meterPerSec = 1.2;
+
+	/** adjusted while walking */
+	float walkedDistance = 0;
+
+	/** adjusted while walking */
+	Timestamp walkedTime;
+
+	/** the listener to inform */
+	std::vector<Listener*> listeners;
+
+	const char* name = "SynWalker";
+
+public:
+
+	/** ctor */
+	SyntheticWalker(SyntheticPath path) : path(path) {
+		;
+	}
+
+	/** attach a new listener */
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+	/** increment the walk */
+	Point3 tick(const Timestamp timePassed) {
+
+		// update time
+		this->walkedTime += timePassed;
+
+		// update the walked distance using the walking speed
+		this->walkedDistance += walkSpeed_meterPerSec * timePassed.sec();
+
+		// get the current position along the path
+		const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance);
+
+		Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );
+
+		// inform listener
+		for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
+
+		return curPosOnPath;
+
+	}
+
+
+
+};
+
+#endif // SYNTHETICWALKER_H
--- a/tests/data/TestTimestamp.cpp
+++ b/tests/data/TestTimestamp.cpp
@@ -52,4 +52,35 @@ TEST(Timestamp, add) {

 }

+TEST(Timestamp, div) {
+
+	Timestamp ts1 = Timestamp::fromMS(1000);
+
+	ASSERT_EQ(100, (ts1/(size_t)10).ms());
+
+}
+
+TEST(Timestamp, minmax) {
+
+	Timestamp tsLow = std::numeric_limits<Timestamp>::lowest();
+
+
+	Timestamp tsMin = std::numeric_limits<Timestamp>::min();
+	Timestamp tsMax = std::numeric_limits<Timestamp>::max();
+	Timestamp ts0 = Timestamp::fromMS(0);
+	Timestamp tsNeg = Timestamp::fromMS(-99999999999999L);
+	Timestamp tsPos = Timestamp::fromMS(+99999999999999L);
+
+	ASSERT_EQ(tsMin, tsLow);
+
+	ASSERT_TRUE(tsMin < tsMax);
+	ASSERT_TRUE(tsMin < ts0);
+	ASSERT_TRUE(tsMin < tsNeg);
+
+	ASSERT_TRUE(tsMax > tsMin);
+	ASSERT_TRUE(tsMax > ts0);
+	ASSERT_TRUE(tsMax > tsPos);
+
+}
+
 #endif
--- a/tests/sensors/radio/TestVAPGrouper.cpp
+++ b/tests/sensors/radio/TestVAPGrouper.cpp
@@ -38,9 +38,9 @@ TEST(WiFiVAPGrouper, baseMAC) {

 TEST(WiFiVAPGrouper, aggregation) {

-	VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
-	VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN);
-	VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM);
+	VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE, VAPGrouper::TimeAggregation::AVERAGE);
+	VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::MINIMUM);
+	VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::MAXIMUM);

 	WiFiMeasurements scan;

@@ -59,7 +59,7 @@ TEST(WiFiVAPGrouper, aggregation) {

 	scan.entries.push_back(WiFiMeasurement(vap20, -69, Timestamp::fromMS(22)));
 	scan.entries.push_back(WiFiMeasurement(vap21, -61, Timestamp::fromMS(25)));
-	scan.entries.push_back(WiFiMeasurement(vap22, -62, Timestamp::fromMS(23)));
+	scan.entries.push_back(WiFiMeasurement(vap22, -62, Timestamp::fromMS(21)));
 	scan.entries.push_back(WiFiMeasurement(vap23, -60, Timestamp::fromMS(20)));

 	const WiFiMeasurements gAvg = vgAvg.group(scan);
@@ -71,26 +71,73 @@ TEST(WiFiVAPGrouper, aggregation) {
 	ASSERT_EQ(2, gMedian.entries.size());
 	ASSERT_EQ(2, gMax.entries.size());

-	// correct average values?
+	// correct average rssi / average timestamp?
 	ASSERT_EQ(-72, gAvg.entries.back().getRSSI());
 	ASSERT_EQ(-63, gAvg.entries.front().getRSSI());
-	ASSERT_EQ(Timestamp::fromMS(11), gAvg.entries.back().getTimestamp());
-	ASSERT_EQ(Timestamp::fromMS(22), gAvg.entries.front().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(12), gAvg.entries.back().getTimestamp());		// average ts
+	ASSERT_EQ(Timestamp::fromMS(22), gAvg.entries.front().getTimestamp());		// average ts

-	// correct median values?
+	// correct median rssi / min timestamp?
 	ASSERT_EQ(-71, gMedian.entries.back().getRSSI());
 	ASSERT_EQ(-61.5, gMedian.entries.front().getRSSI());
-	ASSERT_EQ(Timestamp::fromMS(11), gMedian.entries.back().getTimestamp());
-	ASSERT_EQ(Timestamp::fromMS(22), gMedian.entries.front().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(11), gMedian.entries.back().getTimestamp());	// min ts
+	ASSERT_EQ(Timestamp::fromMS(20), gMedian.entries.front().getTimestamp());	// min ts

-	// correct max values?
+	// correct max rssi / max timestamp?
 	ASSERT_EQ(-70, gMax.entries.back().getRSSI());
 	ASSERT_EQ(-60, gMax.entries.front().getRSSI());
-	ASSERT_EQ(Timestamp::fromMS(11), gMax.entries.back().getTimestamp());
-	ASSERT_EQ(Timestamp::fromMS(22), gMax.entries.front().getTimestamp());
+	ASSERT_EQ(Timestamp::fromMS(13), gMax.entries.back().getTimestamp());	// max ts
+	ASSERT_EQ(Timestamp::fromMS(25), gMax.entries.front().getTimestamp());	// max ts


 }

+TEST(WiFiVAPGrouper, aggregationTS) {
+
+	VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE, VAPGrouper::TimeAggregation::AVERAGE);
+	VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::MINIMUM);
+	VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::MAXIMUM);
+
+	WiFiMeasurements scan;
+
+	const AccessPoint vap0("01:bb:cc:dd:11:a0");
+	const AccessPoint vap1("01:bb:cc:dd:11:a1");
+	const AccessPoint vap2("01:bb:cc:dd:11:a2");
+	const AccessPoint vap3("01:bb:cc:dd:11:a3");
+	const AccessPoint vap4("01:bb:cc:dd:11:a4");
+	const AccessPoint vap5("01:bb:cc:dd:11:a5");
+	const AccessPoint vap6("01:bb:cc:dd:11:a6");
+	const AccessPoint vap7("01:bb:cc:dd:11:a7");
+	const AccessPoint vap8("01:bb:cc:dd:11:a8");
+
+	Timestamp base = Timestamp::fromUnixTime();
+
+	scan.entries.push_back(WiFiMeasurement(vap0, -1, base+Timestamp::fromMS(1)));
+	scan.entries.push_back(WiFiMeasurement(vap1, -2, base+Timestamp::fromMS(2)));
+	scan.entries.push_back(WiFiMeasurement(vap2, -3, base+Timestamp::fromMS(3)));
+	scan.entries.push_back(WiFiMeasurement(vap3, -4, base+Timestamp::fromMS(4)));
+	scan.entries.push_back(WiFiMeasurement(vap4, -5, base+Timestamp::fromMS(5)));
+	scan.entries.push_back(WiFiMeasurement(vap5, -6, base+Timestamp::fromMS(6)));
+	scan.entries.push_back(WiFiMeasurement(vap6, -7, base+Timestamp::fromMS(7)));
+	scan.entries.push_back(WiFiMeasurement(vap7, -8, base+Timestamp::fromMS(8)));
+	scan.entries.push_back(WiFiMeasurement(vap8, -9, base+Timestamp::fromMS(9)));
+
+	const WiFiMeasurements gAvg = vgAvg.group(scan);
+	const WiFiMeasurements gMedian = vgMedian.group(scan);
+	const WiFiMeasurements gMax = vgMax.group(scan);
+
+	// correct average rssi / average timestamp?
+	ASSERT_EQ(-5, gAvg.entries.back().getRSSI());
+	ASSERT_EQ(base+Timestamp::fromMS(5), gAvg.entries.front().getTimestamp());
+
+	// correct median rssi / min timestamp?
+	ASSERT_EQ(-5, gMedian.entries.back().getRSSI());
+	ASSERT_EQ(base+Timestamp::fromMS(1), gMedian.entries.back().getTimestamp());	// min ts
+
+	// correct max rssi / max timestamp?
+	ASSERT_EQ(-1, gMax.entries.back().getRSSI());
+	ASSERT_EQ(base+Timestamp::fromMS(9), gMax.entries.back().getTimestamp());	// max ts
+
+}

 #endif