merged with master

some new helper methods
added support for floorplan metadata load/save

CMakeLists.txt

@@ -68,7 +68,7 @@ ADD_DEFINITIONS(
 	-fstack-protector-all
 
 	-g3
-	-O0
+        #-O0
 	-march=native
 
 	-DWITH_TESTS

floorplan/v2/Floorplan.h

@@ -9,6 +9,7 @@
 
 #include "../../geo/Point3.h"
 #include "../../geo/Point2.h"
+#include "../../geo/EarthPos.h"
 
 namespace Floorplan {
 
@@ -21,20 +22,86 @@ namespace Floorplan {
 	/** a free key-value meta element */
 	struct Meta {
 
+		struct KeyVal {
+			std::string key;
+			std::string val;
+			KeyVal() {;}
+			KeyVal(const std::string& key, const std::string& val) : key(key), val(val) {;}
+		};
+
 		const std::string EMPTY = "";
-		std::unordered_map<std::string, std::string> params;
+
+//		std::unordered_map<std::string, std::string> params;
+//		const std::string& getVal(const std::string& key) const {
+//			const auto it = params.find(key);
+//			return (it == params.end()) ? (EMPTY) : (it->second);
+//		}
+
+		std::vector<KeyVal> params;
+
+		KeyVal* getKV(const std::string& key) {
+			for (KeyVal& kv : params) {
+				if (kv.key == key) {return &kv;}
+			}
+			return nullptr;
+		}
+
+		const KeyVal* getKV(const std::string& key) const {
+			for (const KeyVal& kv : params) {
+				if (kv.key == key) {return &kv;}
+			}
+			return nullptr;
+		}
 
 		const std::string& getVal(const std::string& key) const {
-			const auto it = params.find(key);
-			return (it == params.end()) ? (EMPTY) : (it->second);
+			static const std::string EMPTY = "";
+			const KeyVal* kv = getKV(key);
+			return (kv) ? (kv->val) : (EMPTY);
+		}
+		void setVal(const std::string& key, const std::string& val) {(*this)[key] = val;}
+
+		void add(const std::string& key, const std::string& val) {params.push_back(KeyVal(key,val));}
+
+		std::string& operator [] (const std::string& key) {
+			KeyVal* kv = getKV(key);
+			if (!kv) {params.push_back(KeyVal(key, ""));}
+			return getKV(key)->val;
 		}
-		void setVal(const std::string& key, const std::string& val) {params[key] = val;}
 
 		float getFloat(const std::string& key) const			{ return std::stof(getVal(key)); }
-		void setFloat(const std::string& key, const float val)	{ params[key] = std::to_string(val); }
+		void setFloat(const std::string& key, const float val)	{ (*this)[key] = std::to_string(val); }
 
 		int getInt(const std::string& key) const				{ return std::stoi(getVal(key)); }
-		void setInt(const std::string& key, const int val)		{ params[key] = std::to_string(val); }
+		void setInt(const std::string& key, const int val)		{ (*this)[key] = std::to_string(val); }
+
+
+
+		size_t size() const {return params.size();}
+
+		const std::string& getKey(const int idx) const {return params[idx].key;}
+		const std::string& getVal(const int idx) const {return params[idx].val;}
+
+		void set(const int idx, const KeyVal& kv) {params[idx] = kv;}
+		void setKey(const int idx, const std::string& key) {params[idx].key = key;}
+		void setVal(const int idx, const std::string& val) {params[idx].val = val;}
+
+		/** delete the idx-th entry */
+		void deleteEntry(const int idx) {params.erase(params.begin()+idx);}
+
+	};
+
+	class HasMeta {
+
+		Meta* meta = nullptr;
+
+	public:
+
+		Meta* getMeta() const {return meta;}
+
+		void setMeta(Meta* meta) {
+			if (this->meta) {delete this->meta;}
+			this->meta = meta;
+		}
 
 	};
 
@@ -117,6 +184,7 @@ namespace Floorplan {
 	struct POI;
 	struct Stair;
 	struct Elevator;
+        struct GroundTruthPoint;
 
 	using FloorOutline = std::vector<FloorOutlinePolygon*>;
 	using FloorObstacles = std::vector<FloorObstacle*>;
@@ -127,6 +195,7 @@ namespace Floorplan {
 	using FloorPOIs = std::vector<POI*>;
 	using FloorStairs = std::vector<Stair*>;
 	using FloorElevators = std::vector<Elevator*>;
+        using FloorGroundTruthPoints = std::vector<GroundTruthPoint*>;
 
 	/** describes one floor within the map, starting at a given height */
 	struct Floor {
@@ -143,6 +212,7 @@ namespace Floorplan {
 		FloorPOIs pois;							// POIs within the floor
 		FloorStairs stairs;						// all stairs within one floor
 		FloorElevators elevators;				// all elevators within one floor
+                FloorGroundTruthPoints gtpoints;        // all ground truth points within one floor
 		//FloorKeyValue other;					// other, free elements
 
 		Floor() {;}
@@ -167,11 +237,25 @@ namespace Floorplan {
 		bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);}
 	};
 
+        /** a GroundTruthPoint located somewhere on a floor */
+        struct GroundTruthPoint {
+            int id; //TODO: this value can be changed and isn't set incremental within the indoormap
+            Point2 pos;
+            GroundTruthPoint() : id(), pos() {;}
+            GroundTruthPoint(const int& id, const Point2& pos) : id(id), pos(pos) {;}
+            bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
+        };
+
 	/** an AccessPoint located somewhere on a floor */
-	struct AccessPoint {
+	struct AccessPoint : public HasMeta {
 		std::string name;
 		std::string mac;
 		Point3 pos;		// z is relative to the floor's height
+		struct Model {
+			float txp = 0;
+			float exp = 0;
+			float waf = 0;
+		} model;
 		AccessPoint() : name(), mac(), pos() {;}
 		AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;}
 		bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
@@ -182,10 +266,19 @@ namespace Floorplan {
 	struct Beacon {
 		std::string name;
 		std::string mac;
+		std::string major;
+		std::string minor;
+		std::string uuid;
 		Point3 pos;		// z is relative to the floor's height
+		struct Model {
+			float txp;
+			float exp;
+			float waf;
+		} model;
 		Beacon() : name(), mac(), pos() {;}
 		Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
 		bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
+                Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);}	// relative to the floor's ground
 	};
 
 
@@ -375,8 +468,7 @@ namespace Floorplan {
 
 
 	/** base-class for stairs */
-	struct Stair {
-		Meta* meta = nullptr;
+	struct Stair : public HasMeta {
 		virtual std::vector<StairPart> getParts() const = 0;
 	};
 
@@ -427,7 +519,7 @@ namespace Floorplan {
 
 
 	/** an image file that can be added to the map */
-		struct UnderlayImage {
+	struct UnderlayImage {
 		std::string name;
 		std::string filename;
 		Point2 anchor;
@@ -437,16 +529,44 @@ namespace Floorplan {
 
 
 
+	/** one correspondence point: earth <-> map */
+	struct EarthPosMapPos {
+
+		EarthPos posOnEarth;
+
+		Point3 posOnMap_m;
+
+		/** ctor */
+		EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;}
+
+	};
+
+
+	/** describe the floorplan's location on earth */
+	struct EarthRegistration {
+
+		/** all available correspondences: earth <-> map */
+		std::vector<EarthPosMapPos> correspondences;
+
+	};
+
 	/** describes the whole indoor map */
 	struct IndoorMap {
 
 		float width;
 		float depth;
+
 		std::vector<Floor*> floors;
 
+		/** mapping: floorplan <-> earth */
+		EarthRegistration earthReg;
+
 		IndoorMap() {;}
 
+		/** no copy */
 		IndoorMap(const IndoorMap& o) = delete;
+
+		/** no copy assign */
 		void operator = (const IndoorMap& o) = delete;
 
 	};

floorplan/v2/FloorplanReader.h

@@ -13,6 +13,7 @@ namespace Floorplan {
 
 	using XMLAttr = tinyxml2::XMLAttribute;
 	using XMLElem = tinyxml2::XMLElement;
+	using XMLNode = tinyxml2::XMLNode;
 
 	/**
 	 * read an IndoorMaps from XML-data
@@ -31,7 +32,13 @@ namespace Floorplan {
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.LoadFile(file.c_str());
-			if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while loading XML " + file);}
+			if (res != tinyxml2::XMLError::XML_SUCCESS) {
+				throw Exception(
+					std::string() + "error while loading XML " + file + "\n" +
+					((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+					((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+				);
+			}
 			return parse(doc);
 		}
 
@@ -41,7 +48,13 @@ namespace Floorplan {
 			setlocale(LC_NUMERIC, "C");
 			tinyxml2::XMLDocument doc;
 			const tinyxml2::XMLError res = doc.Parse(str.c_str(), str.length());
-			if (res != tinyxml2::XMLError::XML_SUCCESS) {throw Exception("error while parsing XML");}
+			if (res != tinyxml2::XMLError::XML_SUCCESS) {
+				throw Exception(
+					std::string() + "error while parsing XML\n" +
+					((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+					((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+				);
+			}
 			return parse(doc);
 		}
 
@@ -97,6 +110,7 @@ namespace Floorplan {
 				if (std::string("pois") == n->Name())			{floor->pois = parseFloorPOIs(n);}
 				if (std::string("stairs") == n->Name())			{floor->stairs = parseFloorStairs(n);}
 				if (std::string("elevators") == n->Name())		{floor->elevators = parseFloorElevators(n);}
+                if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
 			}
 			return floor;
 		}
@@ -157,9 +171,7 @@ namespace Floorplan {
 
 			// stair meta information?
 			const XMLElem* meta = el->FirstChildElement("meta");
-			if (meta) {
-				stair->meta = parseMetaElement(meta);
-			}
+			if (meta) {stair->setMeta(parseMetaElement(meta));}
 
 			return stair;
 
@@ -185,6 +197,24 @@ namespace Floorplan {
 		}
 
 
+        /** parse the <gtpoints> tag */
+        static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
+            std::vector<GroundTruthPoint*> vec;
+            FOREACH_NODE(n, el) {
+                if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
+            }
+            return vec;
+        }
+
+        /** parse a <gtpoint> tag */
+        static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
+            GroundTruthPoint* gtp = new GroundTruthPoint();
+            gtp->id = el->IntAttribute("id");
+            gtp->pos = parsePoint2(el);
+            return gtp;
+        }
+
+
 		/** parse the <accesspoints> tag */
 		static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElem* el) {
 			std::vector<AccessPoint*> vec;
@@ -227,10 +257,16 @@ namespace Floorplan {
 		/** parse one <meta> element */
 		static Meta* parseMetaElement(const XMLElem* n) {
 			Meta* elem = new Meta();
-			const XMLAttr* attr = n->FirstAttribute();
-			while (attr) {
-				elem->params[attr->Name()] = attr->Value();
-				attr = attr->Next();
+//			const XMLAttr* attr = n->FirstAttribute();
+//			while (attr) {
+//				elem->setVal(attr->Name(), attr->Value());
+//				attr = attr->Next();
+//			}
+			const XMLElem* sub = n->FirstChildElement();
+			while(sub) {
+				// <entry key="123">abc</entry>
+				elem->add(sub->Attribute("key"), sub->FirstChild()->Value());
+				sub = sub->NextSiblingElement();
 			}
 			return elem;
 		}
@@ -241,6 +277,11 @@ namespace Floorplan {
 			ap->mac = n->Attribute("mac");
 			ap->name = n->Attribute("name");
 			ap->pos = parsePoint3(n);
+			ap->model.txp = n->FloatAttribute("mdl_txp");
+			ap->model.exp = n->FloatAttribute("mdl_exp");
+			ap->model.waf = n->FloatAttribute("mdl_waf");
+			const XMLElem* meta = n->FirstChildElement("meta");
+			if (meta) {ap->setMeta(parseMetaElement(meta));}
 			return ap;
 		}
 
@@ -259,6 +300,12 @@ namespace Floorplan {
 			Beacon* b = new Beacon();
 			b->mac = n->Attribute("mac");
 			b->name = n->Attribute("name");
+			b->major = n->Attribute("major") ? n->Attribute("major") : "";
+			b->minor = n->Attribute("minor") ? n->Attribute("minor") : "";
+			b->uuid = n->Attribute("uuid") ? n->Attribute("uuid") : "";
+			b->model.txp = n->FloatAttribute("mdl_txp");
+			b->model.exp = n->FloatAttribute("mdl_exp");
+			b->model.waf = n->FloatAttribute("mdl_waf");
 			b->pos = parsePoint3(n);
 			return b;
 		}

floorplan/v2/FloorplanWriter.h

@@ -105,7 +105,7 @@ namespace Floorplan {
 			XMLElem* stairs = doc.NewElement("stairs");
 			for (const Stair* _stair : mf->stairs) {
 				XMLElem* elem = doc.NewElement("stair");
-				addMetaElement(doc, elem, _stair->meta);
+				addMetaElement(doc, elem, _stair->getMeta());
 				if (dynamic_cast<const StairFreeform*>(_stair)) {
 					const StairFreeform* stair = (StairFreeform*) _stair;
 					elem->SetAttribute("type", 0);		// TODO: other types?
@@ -145,6 +145,16 @@ namespace Floorplan {
 			}
 			floor->InsertEndChild(pois);
 
+            XMLElem* gtpoints = doc.NewElement("gtpoints");
+            for (const GroundTruthPoint* gtp : mf->gtpoints) {
+                XMLElem* elem = doc.NewElement("gtpoint");
+                elem->SetAttribute("id", gtp->id);
+                elem->SetAttribute("x", gtp->pos.x);
+                elem->SetAttribute("y", gtp->pos.y);
+                gtpoints->InsertEndChild(elem);
+            }
+            floor->InsertEndChild(gtpoints);
+
 			XMLElem* accesspoints = doc.NewElement("accesspoints");
 			for (const AccessPoint* ap : mf->accesspoints) {
 				XMLElem* accesspoint = doc.NewElement("accesspoint");
@@ -153,6 +163,10 @@ namespace Floorplan {
 				accesspoint->SetAttribute("x", ap->pos.x);
 				accesspoint->SetAttribute("y", ap->pos.y);
 				accesspoint->SetAttribute("z", ap->pos.z);
+				accesspoint->SetAttribute("mdl_txp", ap->model.txp);
+				accesspoint->SetAttribute("mdl_exp", ap->model.exp);
+				accesspoint->SetAttribute("mdl_waf", ap->model.waf);
+				addMetaElement(doc, accesspoint, ap->getMeta());
 				accesspoints->InsertEndChild(accesspoint);
 			}
 			floor->InsertEndChild(accesspoints);
@@ -162,9 +176,15 @@ namespace Floorplan {
 				XMLElem* beacon = doc.NewElement("beacon");
 				beacon->SetAttribute("name", b->name.c_str());
 				beacon->SetAttribute("mac", b->mac.c_str());
+				beacon->SetAttribute("major", b->major.c_str());
+				beacon->SetAttribute("minor", b->minor.c_str());
+				beacon->SetAttribute("uuid", b->uuid.c_str());
 				beacon->SetAttribute("x", b->pos.x);
 				beacon->SetAttribute("y", b->pos.y);
 				beacon->SetAttribute("z", b->pos.z);
+				beacon->SetAttribute("mdl_txp", b->model.txp);
+				beacon->SetAttribute("mdl_exp", b->model.exp);
+				beacon->SetAttribute("mdl_waf", b->model.waf);
 				beacons->InsertEndChild(beacon);
 			}
 			floor->InsertEndChild(beacons);
@@ -210,8 +230,15 @@ namespace Floorplan {
 		static void addMetaElement(XMLDoc& doc, XMLElem* other, const Meta* meta) {
 			if (meta == nullptr) {return;}	// nothing to add
 			XMLElem* elem = doc.NewElement("meta");
-			for (auto it : meta->params) {
-				elem->Attribute(it.first.c_str(), it.second.c_str());
+//			for (auto it : meta->params) {
+//				elem->Attribute(it.first.c_str(), it.second.c_str());
+//			}
+			for (const Meta::KeyVal& kv : meta->params) {
+				// <entry key="123">abc</entry>
+				XMLElem* subElem = doc.NewElement("entry");
+				subElem->SetAttribute("key", kv.key.c_str());
+				subElem->SetText(kv.val.c_str());
+				elem->InsertEndChild(subElem);
 			}
 			other->InsertEndChild(elem);
 		}

geo/EarthMapping.h

@@ -0,0 +1,75 @@
+#ifndef EARTHMAPPING_H
+#define EARTHMAPPING_H
+
+#include "Point3.h"
+#include "EarthPos.h"
+
+/**
+ * mapping between positions on earth and positions within the floorplan
+ */
+class EarthMapping {
+
+private:
+
+	/** one 3D position within the floorplan */
+	Point3 center_map_m;
+
+	/** corresponding 3D position on earth */
+	EarthPos center_earth;
+
+	/** rotation [in degrees] to ensure that the map's y-up-axis points towards the north */
+	float rotation_deg;
+
+	double m_per_deg_lat;
+	double m_per_deg_lon;
+
+
+public:
+
+	void build() {
+
+		// TODO
+
+	}
+
+	/** convert from map-coordinates to earth-coordinates */
+	EarthPos mapToWorld(const Point3 mapPos_m) const {
+
+		Point3 pos = mapPos_m;
+
+		// move to (0,0,0)
+		pos -= center_map_m;
+
+		// undo the rotation
+		const Point2 xy = pos.xy().rotated(-rotation_deg / 180.0 * (float) M_PI);
+
+		// convert this "delta to (0,0,0)" to lon/lat and move it to the lon/lat-center
+		const double lat = cenLat + (xy.y / m_per_deg_lat);
+		const double lon = cenLon + (xy.x / m_per_deg_lon);
+		const float height = pos.z;
+
+		// done
+		return EarthPos(lat, lon, height);
+
+	}
+
+	/** convert from earth-coordinates to map-coordinates */
+	Point3 worldToMap(const EarthPos earthPos) const {
+
+		const double y_m = +(lat-cenLat) * m_per_deg_lat;
+		const double x_m = +(lon-cenLon) * m_per_deg_lon;
+
+		// rotate (our map is axis aligned)
+		Point2 pos(x_m, y_m);
+		pos = pos.rotated(rotDeg / 180 * M_PI);
+
+		// apply movement
+		pos += mapCenter_m;
+
+		return pos;
+
+	}
+
+};
+
+#endif // EARTHMAPPING_H

geo/EarthPos.h

@@ -0,0 +1,21 @@
+#ifndef EARTHPOS_H
+#define EARTHPOS_H
+
+/** describes the location on the earth's surface */
+struct EarthPos {
+
+	double lat;
+
+	double lon;
+
+	/** height above sea level */
+	float height;
+
+	/** ctor with values */
+	EarthPos(const double lat, const double lon, const float height) : lon(lon), lat(lat), height(height) {
+		;
+	}
+
+};
+
+#endif // EARTHPOS_H

geo/Point2.h

@@ -17,6 +17,8 @@ struct Point2 {
 	/** ctor */
 	Point2(const float x, const float y) : x(x), y(y) {;}
 
+	Point2 operator - () const {return Point2(-x, -y);}
+
 
 	Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);}
 
@@ -60,4 +62,9 @@ struct Point2 {
 
 };
 
+inline void swap(Point2& p1, Point2& p2) {
+	std::swap(p1.x, p2.x);
+	std::swap(p1.y, p2.y);
+}
+
 #endif // POINT2_H

grid/walk/v2/modules/WalkModuleActivityControlPercent.h

@@ -0,0 +1,91 @@
+#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H
+#define WALKMODULEACTIVITYCONTROLPERCENT_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+#include "../../../../sensors/pressure/ActivityButterPressurePercent.h"
+#include "../../../../grid/GridNode.h"
+
+
+/** favor z-transitions */
+template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControlPercent : public WalkModule<Node, WalkState> {
+
+private:
+
+    Control* ctrl;
+
+public:
+
+	/** ctor */
+    WalkModuleActivityControlPercent(Control* ctrl) : ctrl(ctrl) {
+		;
+	}
+
+    virtual void updateBefore(WalkState& state, const Node& startNode) override {
+        (void) state;
+        (void) startNode;
+    }
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+
+
+        const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
+
+        //floor and doors
+        if(potentialNode.getType() == 0 || potentialNode.getType() == 3){
+            return ctrl->activityPercent.stay;
+        }
+
+        //stairs
+        if(potentialNode.getType() == 1){
+
+//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown +  ctrl->barometer.actProbs.stairsUp){
+//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
+//            }
+
+            if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;}
+            if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;}
+            return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp);
+        }
+
+        //elevators
+        if(potentialNode.getType() == 2){
+
+//            //we need to do this, that particles are able to walk into an elevator even if the prob for that is low,
+//            //that happens often since the activity has some delay.
+//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown +  ctrl->barometer.actProbs.elevatorUp){
+//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
+//            }
+
+            if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;}
+            if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;}
+
+            //for walking out of the elevator
+            return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp);
+        }
+
+        std::cout << "Node has unknown Type" << std::endl;
+        return 1.0;
+    }
+};
+
+#endif // WALKMODULEACTIVITYCONTROLPERCENT_H

grid/walk/v2/modules/WalkModuleButterActivity.h

@@ -1,86 +0,0 @@
-#ifndef WALKMODULEBUTTERACTIVITY_H
-#define WALKMODULEBUTTERACTIVITY_H
-
-#include "WalkModule.h"
-
-#include "../../../../geo/Heading.h"
-#include "../../../../math/Distributions.h"
-#include "../../../../sensors/pressure/ActivityButterPressure.h"
-
-DEPREACTED
-SEE WalkModuleActivityControl
-
-struct WalkStateBarometerActivity {
-
-	/** innser-struct to prevent name-clashes */
-	struct Barometer {
-
-		/** activity currently detected from the baromter */
-		ActivityButterPressure::Activity activity;
-
-		Barometer() : activity(ActivityButterPressure::Activity::STAY) {;}
-
-	} barometer;
-
-	/** ctor */
-	WalkStateBarometerActivity() : barometer() {;}
-
-};
-
-/** favor z-transitions */
-template <typename Node, typename WalkState> class WalkModuleButterActivity : public WalkModule<Node, WalkState> {
-
-public:
-
-	/** ctor */
-    WalkModuleButterActivity() {
-
-		// ensure templates WalkState inherits from 'WalkStateBarometerActivity'
-		StaticAssert::AinheritsB<WalkState, WalkStateBarometerActivity>();
-
-	}
-
-	virtual void updateBefore(WalkState& state, const Node& startNode) override {
-		(void) state;
-		(void) startNode;
-	}
-
-	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
-		(void) state;
-		(void) startNode;
-		(void) endNode;
-
-	}
-
-	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
-		(void) state;
-		(void) curNode;
-		(void) nextNode;
-	}
-
-	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
-
-		(void) state;
-		(void) startNode;
-
-        const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
-
-		if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){
-            if (deltaZ_cm < 0) {return 0;}
-            if (deltaZ_cm == 0) {return 0.1;}
-            return 0.9;
-		} else if (state.barometer.activity == ActivityButterPressure::Activity::UP){
-            if (deltaZ_cm > 0) {return 0;}
-            if (deltaZ_cm == 0) {return 0.1;}
-            return 0.9;
-        } else {
-            if (deltaZ_cm == 0) {return 0.9;}
-            return 0.1;
-        }
-
-	}
-
-
-};
-
-#endif // WALKMODULEBUTTERACTIVITY_H

grid/walk/v2/modules/WalkModuleHeadingVonMises.h

@@ -0,0 +1,123 @@
+#ifndef WALKMODULEHEADINGVONMISES_H
+#define WALKMODULEHEADINGVONMISES_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingVonMises : public WalkModule<Node, WalkState> {
+
+private:
+
+    /** van-Mises distribution */
+    Distribution::VonMises<float> dist;
+
+    /** random noise */
+    Distribution::Normal<float> distNoise;
+
+    const Control* ctrl;
+
+public:
+
+    /** ctor 3.0 should be OK! */
+    WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) :
+        dist(Distribution::VonMises<float>(0.0f, 2.0f)),
+        distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
+        ctrl(ctrl) {
+
+        // ensure the template WalkState inherits from 'WalkStateHeading'!
+        StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
+
+    }
+
+
+    virtual void updateBefore(WalkState& state, const Node& startNode) override {
+
+        // NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
+        // reset this one after every transition!
+        Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
+
+        // sensor noise
+        float var = distNoise.draw();
+
+        // stair? -> increase variance
+        if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
+
+        // adjust the state's heading using the control-data
+        state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
+
+        //set kappa of mises
+        float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
+        dist.setKappa(kappa);
+
+    }
+
+    virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+        (void) state;
+        (void) startNode;
+        (void) endNode;
+    }
+
+    virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+        (void) state;
+
+        // ignore for stairs?
+        //if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
+
+        // for elevator edges [same (x,y) but different z] do not adjust anything
+        if (nextNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+        if (curNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
+        //if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
+
+        // get the heading denoted by the way from curNode to nextNode
+        const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
+
+        // get the heading requested by the state
+        const Heading stateHead = state.heading.direction;
+
+        // get the error (signed difference) between both
+        const float angularDiff = stateHead.getSignedDiff(head);
+
+        // adjust the error.
+        // note:	the error may get > +/- 2PI but this is not an issue!
+        //			when the error is added to the current heading within getProbability(),
+        //			it is ensured their sum is within [0:2pi]
+        state.heading.error += angularDiff;
+
+    }
+
+    double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+        (void) startNode;
+
+        // ignore for stairs?
+        //if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}
+
+        // for elevator edges [same (x,y) but different z] just return 1
+        if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
+        if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
+        //if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
+
+        // get the heading between curNode and potentialNode
+        const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
+
+        // compare the heading against the state's heading - the last error
+        const Heading stateHead = state.heading.direction + state.heading.error;
+
+        // get the difference
+        const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+        // determine probability
+        return dist.getProbability(angularDiff);
+
+    }
+
+
+};
+
+#endif // WALKMODULEHEADING_H

main.cpp

@@ -25,12 +25,12 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Grid.*";
 	//::testing::GTEST_FLAG(filter) = "*Dijkstra.*";
 
-	//::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*";
-	::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
+    //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*";
+    //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*";
 
 
-
-	//::testing::GTEST_FLAG(filter) = "*Barometer*";
+    ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*";
+    //::testing::GTEST_FLAG(filter) = "*Barometer*";
 	//::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*";
 
 	//::testing::GTEST_FLAG(filter) = "Heading*";

math/Distributions.h

@@ -8,5 +8,6 @@
 #include "distribution/VonMises.h"
 #include "distribution/Region.h"
 #include "distribution/Triangle.h"
+#include "distribution/NormalN.h"
 
 #endif // DISTRIBUTIONS_H

math/distribution/Normal.h

@@ -44,6 +44,15 @@ namespace Distribution {
 			gen.seed(seed);
 		}
 
+        /** get the mean value */
+        const T getMu() {
+            return this->mu;
+        }
+
+        /** get the standard deviation */
+        const T getSigma() {
+            return this->sigma;
+        }
 
 		/** get the probability for the given value */
 		static T getProbability(const T mu, const T sigma, const T val) {

math/distribution/NormalN.h

@@ -0,0 +1,50 @@
+#ifndef NORMALN_H
+#define NORMALN_H
+
+#include <eigen3/Eigen/Dense>
+
+namespace Distribution {
+
+    class NormalDistributionN {
+
+    private:
+
+        const Eigen::VectorXd mu;
+        const Eigen::MatrixXd sigma;
+
+        const double _a;
+        const Eigen::MatrixXd _sigmaInv;
+
+    public:
+
+        /** ctor */
+        NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) :
+            mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) {
+
+        }
+
+
+        /** get probability for the given value */
+        double getProbability(const Eigen::VectorXd val) const {
+            const double b = ((val-mu).transpose() * _sigmaInv * (val-mu));
+            return _a * std::exp(-b/2.0);
+        }
+
+        /** get the mean vector */
+        const Eigen::VectorXd getMu(){
+            return this->mu;
+        }
+
+        /** get covariance matrix */
+        const Eigen::MatrixXd getSigma(){
+            return this->sigma;
+        }
+
+        const Eigen::MatrixXd getSigmaInv(){
+            return this->_sigmaInv;
+        }
+
+    };
+
+}
+#endif // NORMALN_H

math/distribution/VonMises.h

@@ -19,7 +19,7 @@ namespace Distribution {
 		const T mu;
 
 		/** like 1.0/variance of the distribution */
-		const T kappa;
+        T kappa;
 
 		/** pre-calcuated look-up-table */
 		std::vector<T> lut;
@@ -30,7 +30,7 @@ namespace Distribution {
 	public:
 
 		/** ctor */
-		VonMises(const T mu, const T kappa) : mu(mu), kappa(kappa) {
+        VonMises(const T mu, T kappa) : mu(mu), kappa(kappa) {
 
 		}
 
@@ -49,6 +49,10 @@ namespace Distribution {
 
 		}
 
+        void setKappa(T _kappa){
+            kappa = _kappa;
+        }
+
 	};
 
 }

math/divergence/KullbackLeibler.h

@@ -0,0 +1,90 @@
+#ifndef KULLBACKLEIBLER_H
+#define KULLBACKLEIBLER_H
+
+#include "../distribution/Normal.h"
+#include "../distribution/NormalN.h"
+
+#include "../../Assertions.h"
+#include <string>
+
+namespace Divergence {
+
+    template <typename Scalar> class KullbackLeibler {
+
+        public:
+
+            /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution
+            *   Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/
+            */
+            static inline Scalar getUnivariateGauss(Distribution::Normal<Scalar> norm1, Distribution::Normal<Scalar> norm2){
+
+                auto sigma1Quad = norm1.getSigma() * norm1.getSigma();
+                auto sigma2Quad = norm2.getSigma() * norm2.getSigma();
+                auto mu12Quad = (norm1.getMu() - norm2.getMu()) * (norm1.getMu() - norm2.getMu());
+                auto log1 = std::log(norm1.getSigma());
+                auto log2 = std::log(norm2.getSigma());
+
+                // kl = log(sigma_2 / sigma_1) + ((sigma_1^2 + (mu_1 - mu_2)^2) / 2 * sigma_2^2) - 0.5
+                double klb = (log2 - log1) + ((sigma1Quad + mu12Quad)/(2.0 * sigma2Quad)) - 0.5;
+
+                //klb is always greater 0
+                if(klb < 0.0){
+                    Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible");
+                }
+                return klb;
+            }
+
+            /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution symmetric*/
+            static inline Scalar getUnivariateGaussSymmetric(Distribution::Normal<Scalar> norm1, Distribution::Normal<Scalar> norm2){
+                return getUnivariateGauss(norm1, norm2) + getUnivariateGauss(norm2, norm1);
+            }
+
+            /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution */
+            static inline Scalar getMultivariateGauss(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){
+
+                //both gaussian have the same dimension.
+                Assert::equal(norm1.getMu().rows(), norm2.getMu().rows(), "mean vectors do not have the same dimension");
+                Assert::equal(norm1.getSigma().rows(), norm2.getSigma().rows(), "cov matrices do not have the same dimension");
+                Assert::equal(norm1.getSigma().cols(), norm2.getSigma().cols(), "cov matrices do not have the same dimension");
+
+                //log
+                auto det1 = norm1.getSigma().determinant();
+                auto det2 = norm2.getSigma().determinant();
+                auto log1 = std::log(det1);
+                auto log2 = std::log(det2);
+
+                //trace
+                Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols());
+                toTrace = norm2.getSigmaInv() * norm1.getSigma();
+                auto trace = toTrace.trace();
+
+                //transpose
+                Eigen::VectorXd toTranspose(norm1.getMu().rows());
+                toTranspose = norm2.getMu() - norm1.getMu();
+                auto transpose = toTranspose.transpose();
+
+                //rawdensity
+                auto rawDensity = transpose * norm2.getSigmaInv() * toTranspose;
+                auto dimension = norm1.getMu().rows();
+
+                //0.5 * ((log(det(cov_2)/det(cov_1)) + tr(cov_2^-1 cov_1) + (mu_2 - mu_1)^T * cov_2^-1 * (mu_2 - mu_1) - dimension)
+                double klb = 0.5 * ((log2 - log1) + trace + rawDensity - dimension);
+
+                //klb is always greater 0
+                if(klb < 0.0){
+                    Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible");
+                }
+                return klb;
+            }
+
+             /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution symmetric*/
+            static inline Scalar getMultivariateGaussSymmetric(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){
+                return getMultivariateGauss(norm1, norm2) + getMultivariateGauss(norm2, norm1);
+            }
+
+
+    };
+
+}
+
+#endif // KULLBACKLEIBLER_H

sensors/MACAddress.h

@@ -47,15 +47,27 @@ public:
 	MACAddress(const std::string& str) {
 
 		// sanity check
-		if (str.size() != 17) {throw Exception("invalid hex string length. must be 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]);
+        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 :)");
+        }
 
 	}
 

sensors/beacon/Beacon.h

@@ -0,0 +1,59 @@
+#ifndef BEACON_H
+#define BEACON_H
+
+#include "../MACAddress.h"
+
+/**
+ * represents a single beacon
+ * a beacon is represented by its MAC-Address and
+ * may provide a sending power TXP
+ */
+class Beacon {
+
+private:
+
+	/** the AP's MAC-Address */
+	MACAddress mac;
+
+    /** OPTIONAL the beacons sending power */
+    float txp;
+
+public:
+
+	/** empty ctor */
+    Beacon() {
+		;
+	}
+
+    /** ctor with MAC and TXP */
+    Beacon(const MACAddress& mac, const float& txp) : mac(mac), txp(txp) {
+		;
+	}
+
+    /** ctor with MAC and TXP */
+    Beacon(const std::string& mac, const float& txp) : mac(mac), txp(txp) {
+		;
+	}
+
+    /** ctor with MAC and without TXP */
+    Beacon(const MACAddress& mac) : mac(mac), txp() {
+		;
+	}
+
+    /** ctor with MAC and without TXP */
+    Beacon(const std::string& mac) : mac(mac), txp() {
+		;
+	}
+
+public:
+
+	/** get the AP's MAC address */
+	inline const MACAddress& getMAC() const {return mac;}
+
+	/** OPTIONAL: get the AP's ssid (if any) */
+    inline const float& getTXP() const {return txp;}
+
+
+};
+
+#endif // BEACON_H

sensors/beacon/BeaconMeasurement.h

@@ -0,0 +1,44 @@
+#ifndef BEACONMEASUREMENT_H
+#define BEACONMEASUREMENT_H
+
+#include "../MACAddress.h"
+#include "../../data/Timestamp.h"
+#include "Beacon.h"
+
+#include <vector>
+
+
+/** one observed AP and its signal strength */
+class BeaconMeasurement {
+
+private:
+
+    /** the timestamp this beacon was discovered at */
+    Timestamp ts;
+
+    /** the beacon's mac address */
+    Beacon beacon;
+
+    /** signal strength */
+    float rssi;
+
+public:
+
+    /** ctor */
+    BeaconMeasurement(const Timestamp ts, const Beacon& beacon, const float rssi) : ts(ts), beacon(beacon), rssi(rssi) {;}
+
+
+public:
+
+    /** get the beacon */
+    const Beacon& getBeacon() const {return beacon;}
+
+    /** get the measurements timestamp */
+    const Timestamp& getTimestamp() const {return ts;}
+
+    /** get the rssi */
+    float getRSSI() const {return rssi;}
+};
+
+
+#endif // BEACONMEASUREMENT_H

sensors/beacon/BeaconMeasurements.h

@@ -0,0 +1,26 @@
+#ifndef BEACONMEASUREMENTS_H
+#define BEACONMEASUREMENTS_H
+
+#include <vector>
+
+#include "BeaconMeasurement.h"
+
+/**
+ * group of several beacon measurements
+ */
+struct BeaconMeasurements {
+
+    std::vector<BeaconMeasurement> entries;
+
+    /** remove entries older then 3000 ms*/
+    void removeOld(const Timestamp latestTS) {
+        auto lambda = [latestTS] (const BeaconMeasurement& e) {
+            Timestamp age = latestTS - e.getTimestamp();
+            return age > Timestamp::fromMS(1000*3);
+        };
+        entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end());
+    }
+
+};
+
+#endif // BEACONMEASUREMENTS_H

sensors/beacon/BeaconProbability.h

@@ -0,0 +1,15 @@
+#ifndef BEACONPROBABILITY_H
+#define BEACONPROBABILITY_H
+
+#include "BeaconMeasurements.h"
+
+/**
+ * base class for all Beacon probability calculators.
+ * such a calculator determines the probabilty for a location (e.g. x,y,z)
+ * given BeaconMeasurements
+ */
+class BeaconProbability {
+
+};
+
+#endif // BEACONPROBABILITY_H

sensors/beacon/BeaconProbabilityFree.h

@@ -0,0 +1,94 @@
+#ifndef BEACONPROBABILITYFREE_H
+#define BEACONPROBABILITYFREE_H
+
+#include "BeaconProbability.h"
+#include "BeaconMeasurements.h"
+#include "model/BeaconModel.h"
+#include "../../math/Distributions.h"
+#include "../../data/Timestamp.h"
+
+#include "../../floorplan/v2/Floorplan.h"
+
+#include <unordered_map>
+
+/**
+ * compare BeaconMeasurements within predictions of a given model.
+ * predictions are just based on the distance to the observed beacon.
+ */
+class BeaconObserverFree : public BeaconProbability {
+
+private:
+
+	const float sigma = 8.0f;
+	const float sigmaPerSecond = 3.0f;
+
+	/** the RSSI prediction model */
+    BeaconModel& model;
+
+	/** the map's floorplan */
+	Floorplan::IndoorMap* map;
+
+public:
+
+    /** ctor */
+    BeaconObserverFree(const float sigma, BeaconModel& model) : sigma(sigma), model(model) {
+
+	}
+
+    /** provides the probability for a specific point in space */
+    double getProbability(const Point3& pos_m, const Timestamp curTime, const BeaconMeasurements& obs) const {
+
+		double prob = 1.0;
+        int numMatchingBeacons = 0;
+
+		// process each measured AP
+        for (const BeaconMeasurement& entry : obs.entries) {
+
+			// sanity check
+			Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");
+
+            // updating the beacons sended txp if available
+            if(entry.getBeacon().getTXP() != 0.0f){
+                //TODO: check if this works
+                model.updateBeacon(entry);
+            }
+
+			// get the model's RSSI (if possible!)
+            const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m);
+
+			// NaN? -> AP not known to the model -> skip
+            if (modelRSSI != modelRSSI) {
+                continue;
+            }
+
+
+			// the scan's RSSI
+			const float scanRSSI = entry.getRSSI();
+
+			// the measurement's age
+			const Timestamp age = curTime - entry.getTimestamp();
+
+			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
+			Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
+
+			// sigma grows with measurement age
+			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
+
+			// update probability
+			prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
+			//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
+
+            ++numMatchingBeacons;
+
+		}
+
+		// sanity check
+        //Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?");
+
+		return prob;
+
+	}
+
+};
+
+#endif // WIFIPROBABILITYFREE_H

sensors/beacon/model/BeaconModel.h

@@ -0,0 +1,46 @@
+#ifndef BEACONMODEL_H
+#define BEACONMODEL_H
+
+#include "../Beacon.h"
+#include "../BeaconMeasurement.h"
+#include "../../../geo/Point3.h"
+
+#include <vector>
+
+/**
+ * interface for signal-strength prediction models.
+ *
+ * the model is passed a MAC-address of an AP in question, and a position.
+ * hereafter the model returns the RSSI for this AP at the questioned location.
+ */
+class BeaconModel {
+
+public:
+
+//	/** get the given access-point's RSSI at the provided location */
+//	virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
+
+	/** get a list of all APs known to the model */
+    virtual std::vector<Beacon> getAllBeacons() const = 0;
+
+    /**
+     * update the beacons signal strength using the current measurement
+     * this could happen if the txp is not updated within the floorplan
+     *
+     * be careful and don't use fantasy values, this could ruin your localitions
+     * completely
+     */
+    virtual void updateBeacon(const BeaconMeasurement beacon) = 0;
+
+
+	/**
+	 * get the RSSI expected at the given location (in meter)
+     * for an beacon identified by the given MAC.
+	 *
+     * if the model can not predict the RSSI for an beacon, it returns NaN!
+	 */
+	virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0;
+
+};
+
+#endif // BEACONMODEL_H

sensors/beacon/model/BeaconModelLogDist.h

@@ -0,0 +1,92 @@
+#ifndef BEACONMODELLOGDIST_H
+#define BEACONMODELLOGDIST_H
+
+#include "BeaconModel.h"
+#include "../../radio/model/LogDistanceModel.h"
+
+#include <unordered_map>
+
+/**
+ * signal-strength estimation using log-distance model
+ */
+class BeaconModelLogDist : public BeaconModel {
+
+public:
+
+    /** parameters describing one beacons to the model */
+	struct APEntry {
+
+		Point3 position_m;	// the AP's position (in meter)
+		float txp;			// sending power (-40)
+		float exp;			// path-loss-exponent (~2.0 - 4.0)
+
+		/** ctor */
+		APEntry(const Point3 position_m, const float txp, const float exp) :
+			position_m(position_m), txp(txp), exp(exp) {;}
+
+	};
+
+private:
+
+    /** map of all beacons (and their parameters) known to the model */
+    std::unordered_map<MACAddress, APEntry> beacons;
+
+public:
+
+	/** ctor */
+    BeaconModelLogDist() {
+		;
+	}
+
+    /** get a list of all beacons known to the model */
+    std::vector<Beacon> getAllBeacons() const {
+        std::vector<Beacon> aps;
+        for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
+		return aps;
+	}
+
+    /** make the given beacon (and its parameters) known to the model */
+    void addAP(const MACAddress& beacon, const APEntry& params) {
+
+		// sanity check
+		Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-90:-30]");
+		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+
+		// add
+        beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
+
+	}
+
+    void updateBeacon(const BeaconMeasurement beacon) override{
+        // try to get the corresponding parameters
+        const auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return;}
+
+        it->second.txp = beacon.getBeacon().getTXP();
+    }
+
+    virtual float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
+
+		// try to get the corresponding parameters
+        const auto it = beacons.find(beacon);
+
+		// AP unknown? -> NAN
+        if (it == beacons.end()) {return NAN;}
+
+        // the beacons' parameters
+		const APEntry& params = it->second;
+
+		// free-space (line-of-sight) RSSI
+		const float distance_m = position_m.getDistance(params.position_m);
+		const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
+
+		// done
+		return rssiLOS;
+
+	}
+
+};
+
+#endif // BEACONMODELLOGDIST_H

sensors/beacon/model/BeaconModelLogDistCeiling.h

@@ -0,0 +1,208 @@
+#ifndef BEACONMODELLOGDISTCEILING_H
+#define BEACONMODELLOGDISTCEILING_H
+
+#include "../../../floorplan/v2/Floorplan.h"
+
+#include "../../../Assertions.h"
+#include "BeaconModel.h"
+#include "../../radio/model/LogDistanceModel.h"
+#include "../BeaconMeasurement.h"
+
+#include <unordered_map>
+
+/**
+ * signal-strength estimation using log-distance model
+ * including ceilings between beacon and position
+ */
+class BeaconModelLogDistCeiling : public BeaconModel {
+
+public:
+
+    /** parameters describing one beacon to the model */
+	struct APEntry {
+
+        Point3 position_m;	// the beacon's position (in meter)
+		float txp;			// sending power (-40)
+		float exp;			// path-loss-exponent (~2.0 - 4.0)
+		float waf;			// attenuation per ceiling/floor (~-8.0)
+
+		/** ctor */
+		APEntry(const Point3 position_m, const float txp, const float exp, const float waf) :
+			position_m(position_m), txp(txp), exp(exp), waf(waf) {;}
+
+	};
+
+private:
+
+    /** map of all beacons (and their parameters) known to the model */
+    std::unordered_map<MACAddress, APEntry> beacons;
+
+	/** position (height) of all ceilings (in meter) */
+	std::vector<float> ceilingsAtHeight_m;
+
+public:
+
+	/** ctor with floorplan (needed for ceiling position) */
+    BeaconModelLogDistCeiling(const Floorplan::IndoorMap* map) {
+
+		// sanity checks
+		Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
+
+		// position of all ceilings
+		for (Floorplan::Floor* f : map->floors) {
+			ceilingsAtHeight_m.push_back(f->atHeight);
+		}
+
+	}
+
+    /** get a list of all beacons known to the model */
+    std::vector<Beacon> getAllBeacons() const {
+        std::vector<Beacon> aps;
+        for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
+		return aps;
+	}
+
+    /** load beacon information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
+    void loadBeaconsFromMap(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+
+		for (const Floorplan::Floor* floor : map->floors) {
+            for (const Floorplan::Beacon* beacon : floor->beacons) {
+                APEntry ape(beacon->getPos(floor), txp, exp, waf);
+                addBeacon(MACAddress(beacon->mac), ape);
+			}
+		}
+
+	}
+
+    /** load beacon information from a vector. use the given fixed TXP/EXP/WAF for all APs */
+    void loadBeaconsFromVector(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
+
+        for (const Floorplan::Floor* floor : map->floors) {
+            for (const Floorplan::Beacon* beacon : floor->beacons) {
+                APEntry ape(beacon->getPos(floor), txp, exp, waf);
+                addBeacon(MACAddress(beacon->mac), ape);
+            }
+        }
+
+    }
+
+    /** make the given beacon (and its parameters) known to the model */
+    void addBeacon(const MACAddress& beacon, const APEntry& params) {
+
+		// sanity check
+		Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
+                Assert::isBetween(params.txp, -90.0f, -30.0f, "TXP out of bounds [-50:-30]");
+		Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
+
+        Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!");
+
+		// add
+        beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
+
+	}
+
+    void updateBeacon(const BeaconMeasurement beacon) override {
+        // try to get the corresponding parameters
+        auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return;}
+
+
+        // TODO: Check if this works as expected
+        it->second.txp = beacon.getBeacon().getTXP();
+    }
+
+	/** remove all added APs */
+	void clear() {
+        beacons.clear();
+	}
+
+    float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
+
+		// try to get the corresponding parameters
+        const auto it = beacons.find(beacon);
+
+        // beacon unknown? -> NAN
+        if (it == beacons.end()) {return NAN;}
+
+		// the access-points' parameters
+		const APEntry& params = it->second;
+
+		// free-space (line-of-sight) RSSI
+		const float distance_m = position_m.getDistance(params.position_m);
+        const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
+
+		// WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value
+		const float wafLoss = params.waf * numCeilingsBetween(position_m, params.position_m);
+
+		// combine
+		return rssiLOS + wafLoss;
+
+	}
+
+
+
+protected:
+
+    FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilings);
+    FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilingsFloat);
+
+
+	/** get the number of ceilings between z1 and z2 */
+	float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
+
+
+		const float zMin = std::min(pos1.z, pos2.z);
+		const float zMax = std::max(pos1.z, pos2.z);
+
+		float cnt = 0;
+
+		for (const float z : ceilingsAtHeight_m) {
+			if (zMin < z && zMax > z) {
+				const float dmax = zMax - z;
+				cnt += (dmax > 1) ? (1) : (dmax);
+			}
+		}
+
+		return cnt;
+
+	}
+
+	/** get the number of ceilings between z1 and z2 */
+	int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
+
+		int cnt = 0;
+		const float zMin = std::min(pos1.z, pos2.z);
+		const float zMax = std::max(pos1.z, pos2.z);
+
+#ifdef WITH_ASSERTIONS
+
+		static int numNear = 0;
+		static int numFar = 0;
+		for (const float z : ceilingsAtHeight_m) {
+			const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
+			if (diff < 0.1) {++numNear;} else {++numFar;}
+		}
+		if ((numNear + numFar) > 150000) {
+			Assert::isTrue(numNear < numFar*0.1,
+			    "many requests to the WiFiModel address nodes (very) near to a ground! \
+			    due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
+			    expect very wrong outputs! \
+			    consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
+			);
+		}
+#endif
+
+		for (const float z : ceilingsAtHeight_m) {
+			if (zMin < z && zMax > z) {++cnt;}
+		}
+
+		return cnt;
+
+	}
+
+};
+
+
+#endif // WIFIMODELLOGDISTCEILING_H

sensors/imu/GravityData.h

@@ -0,0 +1,53 @@
+#ifndef GRAVITYDATA_H
+#define GRAVITYDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct GravityData {
+
+	float x;
+	float y;
+	float z;
+
+    GravityData() : x(0), y(0), z(0) {;}
+
+    GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    GravityData& operator += (const GravityData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    GravityData& operator -= (const GravityData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    GravityData operator - (const GravityData& o) const {
+        return GravityData(x-o.x, y-o.y, z-o.z);
+	}
+
+    GravityData operator / (const float val) const {
+        return GravityData(x/val, y/val, z/val);
+	}
+
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
+};
+
+#endif // GRAVITYDATA_H

sensors/imu/LinearAccelerationData.h

@@ -0,0 +1,53 @@
+#ifndef LINEARACCELERATIONDATA_H
+#define LINEARACCELERATIONDATA_H
+
+#include <cmath>
+#include <sstream>
+
+
+/** data received from an accelerometer sensor */
+struct LinearAccelerationData {
+
+	float x;
+	float y;
+	float z;
+
+    LinearAccelerationData() : x(0), y(0), z(0) {;}
+
+    LinearAccelerationData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+    LinearAccelerationData& operator += (const LinearAccelerationData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+    LinearAccelerationData& operator -= (const LinearAccelerationData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+    LinearAccelerationData operator - (const LinearAccelerationData& o) const {
+        return LinearAccelerationData(x-o.x, y-o.y, z-o.z);
+	}
+
+    LinearAccelerationData operator / (const float val) const {
+        return LinearAccelerationData(x/val, y/val, z/val);
+	}
+
+        std::string asString() const {
+            std::stringstream ss;
+            ss << "(" << x << "," << y << "," << z << ")";
+            return ss.str();
+        }
+
+};
+
+#endif // LINEARACCELERATIONDATA_H

sensors/imu/MotionDetection.h

@@ -0,0 +1,163 @@
+#ifndef MOTIONDETECTION_H
+#define MOTIONDETECTION_H
+
+#include "GravityData.h"
+#include "LinearAccelerationData.h"
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../misc/Debug.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 "../../Assertions.h"
+
+class MotionDetection {
+
+private:
+
+    bool newAccelerationMeasurementArrived = false;
+    bool newGravityMeasurementArrived = false;
+
+    Eigen::Vector3f curGravity;
+    Eigen::Vector3f curLinearAcceleration;
+
+    //fast algo
+    Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix
+
+    int numMeasurementsPerInterval, updateCnt;
+    int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms
+
+    struct Motion{
+        Eigen::Vector2f vec = Eigen::Vector2f::Identity();
+        Timestamp lastEstimation;
+    };
+
+    Motion curMotion;
+    Motion prevMotion;
+
+    const char* name = "MotionDetection";
+
+public:
+
+	/** ctor */
+    MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
+        ;
+	}
+
+    void addGravity(const Timestamp& ts, const GravityData& grav){
+
+            curGravity << grav.x, grav.y, grav.z;
+            newGravityMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {
+
+            curLinearAcceleration << acc.x, acc.y, acc.z;
+            newAccelerationMeasurementArrived = true;
+
+            updateProjectionVectorFast(ts);
+    }
+
+    /** return the current motion axis
+     * NOTE: if no data is available, this vector is the Identity
+    */
+    Eigen::Vector2f getCurrentMotionAxis(){
+        return curMotion.vec;
+    }
+
+    /** returns the radians between [-pi, pi] between successive motion axis estimations */
+    float getMotionChangeInRad(){
+        //TODO: put this in an EigenHelper Class within geo
+        const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
+        const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));
+
+        //nan?
+        if(std::isnan(ang)){
+            Log::add(name, "The motion change angle is nAn, this is not correct!");
+        }
+
+        if(updateCnt < 2){
+            return 0;
+        }
+
+        return ang;
+    }
+
+private:
+
+    FRIEND_TEST(MotionDetection, motionAngle);
+    FRIEND_TEST(MotionDetection, motionAxis);
+
+    Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){
+
+        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
+        return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
+    }
+
+    void updateProjectionVectorFast(const Timestamp& ts){
+
+        //make sure we have both measurements for calculation
+        if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){
+
+            numMeasurementsPerInterval++;
+
+            //project acc into perpendicular plane of grav (using standard vector projection)
+            Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;
+
+            //if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
+            if(proj.isZero()){
+                proj = curLinearAcceleration;
+                Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
+            }
+
+            //we are only interested in x,y
+            Eigen::Vector2f vec;
+            vec << proj.x(), proj.y();
+
+            // sum this up for later averaging.
+            sumProjectedCov += vec*vec.transpose();
+
+            // start with the first available timestamp
+            if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;}
+
+            //update the motion axis depending on the update interval
+            if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){
+
+                prevMotion = curMotion;
+
+                //calculate the average of the coveriance matrix
+                Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;
+
+                curMotion.vec = updateMotionAxis(Q);
+                curMotion.lastEstimation = ts;
+
+                reset();
+            }
+
+            newGravityMeasurementArrived = false;
+            newAccelerationMeasurementArrived = false;
+        }
+        //do nothing
+    }
+
+    void reset(){
+        numMeasurementsPerInterval = 0;
+        sumProjectedCov = Eigen::Matrix2f::Zero();
+        ++updateCnt;
+    }
+
+};
+
+#endif // MOTIONDETECTION_H

sensors/offline/FileReader.h

@@ -0,0 +1,299 @@
+#ifndef FILEREADER_H
+#define FILEREADER_H
+
+#include <fstream>
+#include <Indoor/Exception.h>
+#include <vector>
+#include <unordered_map>
+
+#include "../../math/Interpolator.h"
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include "../../sensors/pressure/BarometerData.h"
+#include "../../sensors/imu/AccelerometerData.h"
+#include "../../sensors/imu/GyroscopeData.h"
+#include "../../sensors/imu/GravityData.h"
+#include "../../sensors/imu/LinearAccelerationData.h"
+#include "../../sensors/beacon/BeaconMeasurements.h"
+
+
+#include "../../geo/Point2.h"
+#include "../../grid/factory/v2/GridFactory.h"
+#include "../../grid/factory/v2/Importance.h"
+#include "../../floorplan/v2/Floorplan.h"
+
+class FileReader {
+
+public:
+
+    template <typename T> struct TS {
+        const uint64_t ts;
+        T data;
+        TS(const uint64_t ts) : ts(ts) {;}
+        TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
+    };
+
+    enum class Sensor {
+        ACC,
+        GYRO,
+        WIFI,
+        POS,
+        BARO,
+        BEACON,
+        LIN_ACC,
+        GRAVITY,
+    };
+
+    /** entry for one sensor */
+    struct Entry {
+        Sensor type;
+        uint64_t ts;
+        int idx;
+        Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
+    };
+
+    std::vector<TS<int>> groundTruth;
+    std::vector<TS<WiFiMeasurements>> wifi;
+    std::vector<TS<BeaconMeasurement>> beacon;
+    std::vector<TS<AccelerometerData>> acc;
+    std::vector<TS<GyroscopeData>> gyro;
+    std::vector<TS<BarometerData>> barometer;
+    std::vector<TS<LinearAccelerationData>> lin_acc;
+    std::vector<TS<GravityData>> gravity;
+
+    /** ALL entries */
+    std::vector<Entry> entries;
+
+public:
+
+    FileReader(const std::string& file) {
+        parse(file);
+    }
+
+    const std::vector<Entry>& getEntries() const {return entries;}
+
+
+    const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
+
+    const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
+
+    const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
+
+    const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
+
+    const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
+
+    const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
+
+    const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
+
+    const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
+
+private:
+
+    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);}
+
+        while(!inp.eof() && !inp.bad()) {
+
+            uint64_t ts;
+            char delim;
+            int idx = -1;
+            std::string data;
+
+            inp >> ts;
+            inp >> delim;
+            inp >> idx;
+            inp >> delim;
+            inp >> data;
+
+            if		(idx == 8)	{parseWiFi(ts, data);}
+            else if	(idx == 9)	{parseBeacons(ts, data);}
+            else if	(idx == 99) {parseGroundTruth(ts, data);}
+            else if	(idx == 0)  {parseAccelerometer(ts, data);}
+            else if	(idx == 3)  {parseGyroscope(ts, data);}
+            else if (idx == 5)	{parseBarometer(ts, data);}
+            else if (idx == 2)  {parseLinearAcceleration(ts,data);}
+            else if (idx == 1)  {parseGravity(ts,data);}
+
+            // TODO: this is a hack...
+            // the loop is called one additional time after the last entry
+            // and keeps the entries of entry
+
+        }
+
+        inp.close();
+
+    }
+
+    void parseLinearAcceleration(const uint64_t ts, const std::string& data){
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
+        lin_acc.push_back(elem);
+        entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
+    }
+
+    void parseGravity(const uint64_t ts, const std::string& data){
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<GravityData> elem(ts, GravityData(std::stof(x), std::stof(y), std::stof(z)));
+        gravity.push_back(elem);
+        entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
+    }
+
+    void parseAccelerometer(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
+        acc.push_back(elem);
+        entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
+
+    }
+
+    void parseGyroscope(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+
+        const std::string x = data.substr(0, pos1);
+        const std::string y = data.substr(pos1+1, pos2-pos1-1);
+        const std::string z = data.substr(pos2+1);
+
+        TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
+        gyro.push_back(elem);
+        entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
+
+    }
+
+    void parseWiFi(const uint64_t ts, const std::string& data) {
+
+        std::string tmp = data;
+
+        // add new wifi reading
+        wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
+        entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1));
+
+        // process all APs
+        while(!tmp.empty()) {
+
+            auto pos1 = tmp.find(';');
+            auto pos2 = tmp.find(';', pos1+1);
+            auto pos3 = tmp.find(';', pos2+1);
+
+            std::string mac = tmp.substr(0, pos1);
+            std::string freq = tmp.substr(pos1+1, pos2);
+            std::string rssi = tmp.substr(pos2+1, pos3);
+
+            tmp = tmp.substr(pos3);
+            assert(tmp[0] == ';'); tmp = tmp.substr(1);
+
+            // append AP to current scan-entry
+            WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
+            wifi.back().data.entries.push_back(e);
+        }
+
+    }
+
+    void parseBeacons(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        const auto pos2 = data.find(';', pos1+1);
+        const auto pos3 = data.find(';', pos2+1);
+
+        const std::string mac = data.substr(0, pos1);
+        const std::string rssi = data.substr(pos1+1, pos2);
+        const std::string txp = data.substr(pos2+1, pos3);
+
+        //yes the timestamp is redundant here, but in case of multiusage...
+        TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
+        beacon.push_back(e);
+        entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
+
+    }
+
+    void parseGroundTruth(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+        std::string gtIndex = data.substr(0, pos1);
+
+        TS<int> elem(ts, std::stoi(gtIndex));
+        groundTruth.push_back(elem);
+
+    }
+
+    void parseBarometer(const uint64_t ts, const std::string& data) {
+
+        const auto pos1 = data.find(';');
+
+        const std::string hPa = data.substr(0, pos1);
+
+        TS<BarometerData> elem(ts, BarometerData(std::stof(hPa)));
+        barometer.push_back(elem);
+        entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
+
+    }
+
+public:
+    const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {
+
+        // finde alle positionen der waypoints im gtPath aus map
+        std::unordered_map<int, Point3> waypointsMap;
+        for(Floorplan::Floor* f : map->floors){
+            float h = f->atHeight;
+            for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){
+
+                //wenn die gleiche id 2x vergeben wurde, knallt es
+                if(waypointsMap.find(gtp->id) == waypointsMap.end()){
+                    waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)});
+                }
+                else{
+                    throw std::string("the floorplan's ground truth contains two points with identical id's!");
+                }
+
+            }
+        }
+
+        // bringe diese in richtige reihenfolge und füge timestamp hinzu
+        Interpolator<uint64_t, Point3> interpol;
+
+        int it = 0;
+        for(int id : gtPath){
+            auto itMap = waypointsMap.find(id);
+            if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}
+
+            //the time, when the gt button was clicked on the app
+            uint64_t tsGT = groundTruth[it++].ts;
+            interpol.add(tsGT, itMap->second);
+
+        }
+
+        if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){
+            throw std::string("No Ground Truth points found within the map.xml file");
+        }
+
+        return interpol;
+    }
+
+};
+
+#endif // FILEREADER_H

sensors/pressure/ActivityButterPressure.h

@@ -18,8 +18,6 @@ public:
 
     enum Activity {DOWN, STAY, UP};
 
-
-
     struct History {
         Timestamp ts;
         BarometerData data;
@@ -27,23 +25,25 @@ public:
     };
 
     private:
-    //just for debugging and plotting
-    std::vector<History> input;
-    std::vector<History> inputInterp;
-    std::vector<History> output;
-    std::vector<float> sumHist;
-    std::vector<float> mvAvgHist;
-    std::vector<History> actHist;
 
+    std::vector<History> output;
     Activity currentActivity;
     MovingAVG<float> mvAvg = MovingAVG<float>(20);
 
-    /** change this values for much success */
+    /** change this values for much success
+    *
+    *   Nexus 6:
+    *   butter =  Filter::ButterworthLP<float>(10,0.1f,2);
+    *   threshold = 0.025;
+    *   diffSize = 20;
+    *   FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
+    */
     const bool additionalLowpassFilter = false;
-	const int diffSize = 20; //the number values used for finding the activity.
-    const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
-	Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.1f,2);
-    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.1f,2);
+    const unsigned long diffSize = 20; //the number values used for finding the activity.
+    const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
+    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
+    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.05f,2);
+
     FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
 
 public:
@@ -68,14 +68,11 @@ public:
             return STAY;
         }
 
-        input.push_back(History(ts, baro));
-
         bool newInterpolatedValues = false;
 
         //interpolate & butter
         auto callback = [&] (const Timestamp ts, const float val) {
             float interpValue = val;
-            inputInterp.push_back(History(ts, BarometerData(interpValue)));
 
             //butter
             float butterValue = butter.process(interpValue);
@@ -89,10 +86,10 @@ public:
         if(newInterpolatedValues == true){
 
             //getActivity
-			if((int)output.size() > diffSize){
+            if(output.size() > diffSize){
                 //diff
                 std::vector<float> diff;
-                for(int i = output.size() - diffSize; i < output.size() - 1; ++i){
+                for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
 
                     float diffVal = output[i+1].data.hPa - output[i].data.hPa;
 
@@ -113,7 +110,6 @@ public:
                 }else{
                     actValue = sum;
                 }
-                sumHist.push_back(actValue);
 
                 if(actValue > threshold){
                     currentActivity = DOWN;
@@ -127,8 +123,6 @@ public:
             }
         }
 
-        actHist.push_back(History(ts, BarometerData(currentActivity)));
-
         return currentActivity;
 
 	}
@@ -137,49 +131,6 @@ public:
     Activity getCurrentActivity() {
         return currentActivity;
 	}
-
-    std::vector<float> getSensorHistory(){
-        std::vector<float> tmp;
-
-        for(History val : input){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getInterpolatedHistory(){
-        std::vector<float> tmp;
-
-        for(History val : inputInterp){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getOutputHistory(){
-
-        std::vector<float> tmp;
-
-        for(History val : output){
-            tmp.push_back(val.data.hPa);
-        }
-
-        return tmp;
-    }
-
-    std::vector<float> getSumHistory(){
-        return sumHist;
-    }
-
-
-    std::vector<History> getActHistory(){
-
-        return actHist;
-    }
-
-
 };
 
 #endif // ACTIVITYBUTTERPRESSURE_H

sensors/pressure/ActivityButterPressurePercent.h

@@ -0,0 +1,187 @@
+#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H
+#define ACTIVITYBUTTERPRESSUREPERCENT_H
+
+#include "../../data/Timestamp.h"
+#include "../../math/filter/Butterworth.h"
+#include "../../math/FixedFrequencyInterpolator.h"
+#include "../../math/distribution/Normal.h"
+#include <KLib/Assertions.h>
+#include <vector>
+
+
+#include "BarometerData.h"
+
+/**
+ * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
+ * activity recognition based on a small window given by matlabs diff(window)
+ *
+ * todo: if an elevator is detected, first we have a short time the stairs are more prober.
+ */
+class ActivityButterPressurePercent {
+
+public:
+
+    struct ActivityProbabilities{
+        float elevatorDown;
+        float stairsDown;
+        float stay;
+        float stairsUp;
+        float elevatorUp;
+
+        ActivityProbabilities(float elevatorDown, float stairsDown,
+                              float stay, float stairsUp, float elevatorUp) :
+            elevatorDown(elevatorDown), stairsDown(stairsDown),
+            stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;}
+
+        ActivityProbabilities() :
+            elevatorDown(0.01f), stairsDown(0.01f),
+            stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;}
+    };
+
+
+    struct History {
+        Timestamp ts;
+        BarometerData data;
+        History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;}
+    };
+
+private:
+
+    std::vector<History> output;
+    bool initialize;
+
+    ActivityProbabilities currentActivity;
+
+    /** change this values for much success */
+    const unsigned long diffSize = 20; //the number values used for finding the activity.
+    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
+    FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
+
+    const float variance = 0.02f;
+
+    const float muStairs = 0.04f;
+    const float muStay = 0.00f;
+    const float muEleveator = 0.08f;
+
+    std::vector<float> densities = std::vector<float>(5, 1);
+    std::vector<float> densitiesOld = std::vector<float>(5, 1);
+
+public:
+
+
+	/** ctor */
+    ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){
+        initialize = true;
+	}
+
+
+	/** add new sensor readings that were received at the given timestamp */
+    ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) {
+
+        //init
+        if(initialize){
+            butter.stepInitialization(baro.hPa);
+            initialize = false;
+
+            return currentActivity;
+        }
+
+        bool newInterpolatedValues = false;
+
+        //interpolate & butter
+        auto callback = [&] (const Timestamp ts, const float val) {
+            float interpValue = val;
+
+            //butter
+            float butterValue = butter.process(interpValue);
+            output.push_back(History(ts, BarometerData(butterValue)));
+
+            newInterpolatedValues = true;
+
+        };
+        ffi.add(ts, baro.hPa, callback);
+
+        if(newInterpolatedValues == true){
+
+            //getActivity
+            if(output.size() > diffSize){
+                //diff
+                std::vector<float> diff;
+                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);
+                }
+
+                float sum = 0;
+                for(float val : diff){
+                    sum += val;
+                }
+
+                float actValue = sum;
+
+                //calculate the probabilites of walking down/up etc...
+                densitiesOld = densities;
+
+                //in one building there is an ultra fast elevator, therefore we need to clip the activity value...
+                if(actValue > muEleveator){
+                    actValue = muEleveator;
+                }
+                if(actValue < -muEleveator){
+                    actValue = -muEleveator;
+                }
+
+                float densityElevatorDown = Distribution::Normal<float>::getProbability(muEleveator, variance, actValue);
+                float densityStairsDown = Distribution::Normal<float>::getProbability(muStairs, variance, actValue);
+                float densityStay = Distribution::Normal<float>::getProbability(muStay, variance, actValue);
+                float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
+                float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);
+
+                _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
+                _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
+                _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
+                _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
+                _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
+
+                _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
+                _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
+                _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
+                _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
+                _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");
+
+                //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
+                densities[0] = densityElevatorDown;
+                densities[1] = densityStairsDown;
+                densities[2] = densityStay;
+                densities[3] = densityStairsUp;
+                densities[4] = densityElevatorUp;
+
+                //normalize
+                float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4];
+
+                for(unsigned long i = 0; i < densities.size(); ++i){
+                    densities[i] /= densitySum;
+
+                    //values cant be zero!
+                    densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f);
+                }
+
+                currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]);
+
+            }
+        }
+
+        //retruns for every call, indepedent of callback.
+        return currentActivity;
+	}
+
+    /** get the current Activity */
+    ActivityProbabilities getCurrentActivity() {
+        return currentActivity;
+	}
+
+
+};
+
+#endif // ACTIVITYBUTTERPRESSUREPERCENT_H

sensors/radio/LocatedAccessPoint.h

@@ -1,27 +0,0 @@
-#ifndef LOCATEDACCESSPOINT_H
-#define LOCATEDACCESSPOINT_H
-
-#include "AccessPoint.h"
-#include "../../geo/Point3.h"
-#include "../../floorplan/v2/Floorplan.h"
-
-/**
- * describes an access-point including its position (in meter)
- */
-class LocatedAccessPoint : public AccessPoint, public Point3 {
-
-public:
-
-	/** ctor */
-	LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) {
-		;
-	}
-
-	/** ctor */
-	LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) {
-		;
-	}
-
-};
-
-#endif // LOCATEDACCESSPOINT_H

sensors/radio/WiFiMeasurement.h

@@ -9,7 +9,7 @@
  */
 class WiFiMeasurement {
 
-public:
+private:
 
 	friend class VAPGrouper;
 
@@ -19,6 +19,9 @@ public:
 	/** the measured signal strength */
 	float rssi;
 
+    /** OPTIONAL. frequence the signal was received */
+    float freq;
+
 	/** OPTIONAL. timestamp the measurement was recorded at */
 	Timestamp ts;
 
@@ -29,22 +32,42 @@ public:
 		;
 	}
 
-	/** ctor with timestamp */
-	WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
-		;
-	}
+    /** 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 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;}
+
+    /** set another signal strength */
+    void setRssi(float value){rssi = value;}
+
+    /** set the timestamp */
+    void setTimestamp(const Timestamp& val){ts = val;}
 };
 
+
+
 #endif // WIFIMEASUREMENT_H

sensors/radio/WiFiProbabilityFree.h

@@ -5,6 +5,7 @@
 #include "model/WiFiModel.h"
 #include "../../math/Distributions.h"
 #include "VAPGrouper.h"
+#include "../../floorplan/v2/Floorplan.h"
 
 #include <unordered_map>
 
@@ -57,7 +58,7 @@ public:
 			const Timestamp age = curTime - entry.getTimestamp();
 
 			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
-			Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
+            //Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
 
 			// sigma grows with measurement age
 			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
@@ -71,14 +72,15 @@ public:
 		}
 
 		// sanity check
-		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
+        //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
 
 		return prob;
 
 	}
 
 	template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {
-		throw Exception("todo??");
+
+        return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs);
 	}
 
 };

sensors/radio/WiFiProbabilityGrid.h

@@ -59,7 +59,7 @@ public:
 		// after some runtime, check whether the wifi timestamps make sense
 		// those must not be zero, otherwise something is wrong!
 		if (!obs.entries.empty()) {
-			Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
+            Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().getTimestamp().isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
 		}
 
 		// process each observed measurement
@@ -73,7 +73,7 @@ public:
 			const Timestamp age = curTime - measurement.getTimestamp();
 
 			// sigma grows with measurement age
-			float sigma = this->sigma + this->sigmaPerSecond * age.sec();
+            float sigma = this->sigma + this->sigmaPerSecond * age.sec();
 
 			// the RSSI from the scan
 			const float measuredRSSI = measurement.getRSSI();
@@ -102,8 +102,7 @@ public:
 		}
 
 		// sanity check
-//		Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
-//		if (numMatchingAPs == 0) {return 0;}
+        //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
 
 		// as not every node has the same number of visible/matching APs
 		// we MUST return something like the average probability

sensors/radio/model/WiFiModel.h

@@ -1,7 +1,8 @@
 #ifndef WIFIMODEL_H
 #define WIFIMODEL_H
 
-#include "../LocatedAccessPoint.h"
+#include "../AccessPoint.h"
+#include "../../../geo/Point3.h"
 
 /**
  * interface for signal-strength prediction models.

sensors/radio/model/WiFiModelLogDist.h

@@ -4,6 +4,8 @@
 #include "WiFiModel.h"
 #include "LogDistanceModel.h"
 
+#include <unordered_map>
+
 /**
  * signal-strength estimation using log-distance model
  */

sensors/radio/setup/WiFiFingerprint.h

@@ -46,9 +46,9 @@ struct WiFiFingerprint {
 			const WiFiMeasurements& apMeasurements = it.second;
 			WiFiMeasurement avg = apMeasurements.entries.front();			// average starts with a copy of the first entry (to get all data-fields beside the rssi)
 			for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) {		// sum up all other entries [1:end]
-				avg.rssi += apMeasurements.entries[i].rssi;
+                avg.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI());
 			}
-			avg.rssi /= apMeasurements.entries.size();
+            avg.setRssi(avg.getRSSI() / apMeasurements.entries.size());
 			res.entries.push_back(avg);										// add to output
 		}
 
@@ -62,7 +62,7 @@ struct WiFiFingerprint {
 		out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n";
 		out << "num: " << measurements.entries.size() << "\n";
 		for (const WiFiMeasurement& wm : measurements.entries) {
-			out << wm.getTimestamp().ms() << " " << wm.ap.getMAC().asString() << " " << wm.getRSSI() << "\n";
+            out << wm.getTimestamp().ms() << " " << wm.getAP().getMAC().asString() << " " << wm.getRSSI() << "\n";
 		}
 	}
 

sensors/radio/setup/WiFiOptimizer.h

@@ -89,7 +89,7 @@ public:
 
 		// add each available AP to its slot (lookup map)
 		for (const WiFiMeasurement& m : measurements.entries) {
-			const RSSIatPosition rap(fp.pos_m, m.rssi);
+            const RSSIatPosition rap(fp.pos_m, m.getRSSI());
 			apMap[m.getAP().getMAC()].push_back(rap);
 		}
 

tests/Tests.h

@@ -6,8 +6,8 @@
 #include <gtest/gtest.h>
 
 static inline std::string getDataFile(const std::string& name) {
-	return "/mnt/data/workspaces/Indoor/tests/data/" + name;
-	//return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
+    //return "/mnt/data/workspaces/Indoor/tests/data/" + name;
+    return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name;
 }
 
 

tests/math/divergence/TestKullbackLeibler.cpp

@@ -0,0 +1,214 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+#include "../../../math/divergence/KullbackLeibler.h"
+#include "../../../math/Distributions.h"
+
+#include <random>
+
+
+TEST(KullbackLeibler, univariateGaussEQ) {
+    //if the distributions are equal, kld is 0
+    Distribution::Normal<float> norm1(0,1);
+    Distribution::Normal<float> norm2(0,1);
+
+    ASSERT_EQ(0.0f, Divergence::KullbackLeibler<float>::getUnivariateGauss(norm1, norm2));
+    ASSERT_EQ(0.0f, Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm1, norm2));
+}
+
+TEST(KullbackLeibler, univariateGaussGEmu) {
+    //bigger mu means greater kld
+    Distribution::Normal<float> norm1(0,1);
+    Distribution::Normal<float> norm2(0,1);
+    Distribution::Normal<float> norm3(0,1);
+    Distribution::Normal<float> norm4(1,1);
+
+    ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGauss(norm3, norm4), Divergence::KullbackLeibler<float>::getUnivariateGauss(norm1, norm2));
+    ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm3, norm4), Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm1, norm2));
+}
+
+TEST(KullbackLeibler, univariateGaussGEsigma) {
+    //bigger sigma means greater kld
+    Distribution::Normal<float> norm1(0,1);
+    Distribution::Normal<float> norm2(0,1);
+    Distribution::Normal<float> norm5(0,1);
+    Distribution::Normal<float> norm6(0,3);
+
+    ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGauss(norm5, norm6), Divergence::KullbackLeibler<float>::getUnivariateGauss(norm1, norm2));
+    ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm5, norm6), Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm1, norm2));
+}
+
+TEST(KullbackLeibler, univariateGaussRAND) {
+
+    for(int i = 0; i < 20; i++){
+        auto randMu1 = rand() % 100;
+        auto randMu2 = rand() % 100 + 100;
+
+        auto randMu3 = rand() % 100;
+        auto randMu4 = rand() % 100 + 200;
+
+        Distribution::Normal<float> norm7(randMu1,1);
+        Distribution::Normal<float> norm8(randMu2,1);
+
+        Distribution::Normal<float> norm9(randMu3,1);
+        Distribution::Normal<float> norm10(randMu4,1);
+
+        ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGauss(norm9, norm10), Divergence::KullbackLeibler<float>::getUnivariateGauss(norm8, norm7));
+        ASSERT_GE(Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm9, norm10), Divergence::KullbackLeibler<float>::getUnivariateGaussSymmetric(norm8, norm7));
+    }
+
+}
+
+TEST(KullbackLeibler, multivariateGaussEQ) {
+
+    //eq
+    Eigen::VectorXd mu1(2);
+    mu1[0] = 1.0;
+    mu1[1] = 1.0;
+
+    Eigen::VectorXd mu2(2);
+    mu2[0] = 1.0;
+    mu2[1] = 1.0;
+
+    Eigen::MatrixXd cov1(2,2);
+    cov1(0,0) = 1.0;
+    cov1(0,1) = 0.0;
+    cov1(1,0) = 0.0;
+    cov1(1,1) = 1.0;
+
+    Eigen::MatrixXd cov2(2,2);
+    cov2(0,0) = 1.0;
+    cov2(0,1) = 0.0;
+    cov2(1,0) = 0.0;
+    cov2(1,1) = 1.0;
+
+    Distribution::NormalDistributionN norm1(mu1, cov1);
+    Distribution::NormalDistributionN norm2(mu2, cov2);
+
+    ASSERT_EQ(0.0f, Divergence::KullbackLeibler<float>::getMultivariateGauss(norm1, norm2));
+    ASSERT_EQ(0.0f, Divergence::KullbackLeibler<float>::getMultivariateGaussSymmetric(norm1, norm2));
+
+
+}
+
+TEST(KullbackLeibler, multivariateGaussGeMu) {
+
+    //ge mu
+    Eigen::VectorXd mu1(2);
+    mu1[0] = 1.0;
+    mu1[1] = 1.0;
+
+    Eigen::VectorXd mu2(2);
+    mu2[0] = 1.0;
+    mu2[1] = 1.0;
+
+    Eigen::VectorXd mu3(2);
+    mu3[0] = 1.0;
+    mu3[1] = 1.0;
+
+    Eigen::VectorXd mu4(2);
+    mu4[0] = 1.0;
+    mu4[1] = 3.0;
+
+    Eigen::MatrixXd cov1(2,2);
+    cov1(0,0) = 1.0;
+    cov1(0,1) = 0.0;
+    cov1(1,0) = 0.0;
+    cov1(1,1) = 1.0;
+
+    Eigen::MatrixXd cov2(2,2);
+    cov2(0,0) = 1.0;
+    cov2(0,1) = 0.0;
+    cov2(1,0) = 0.0;
+    cov2(1,1) = 1.0;
+
+    Eigen::MatrixXd cov3(2,2);
+    cov3(0,0) = 1.0;
+    cov3(0,1) = 0.0;
+    cov3(1,0) = 0.0;
+    cov3(1,1) = 1.0;
+
+    Eigen::MatrixXd cov4(2,2);
+    cov4(0,0) = 1.0;
+    cov4(0,1) = 0.0;
+    cov4(1,0) = 0.0;
+    cov4(1,1) = 1.0;
+
+    Distribution::NormalDistributionN norm1(mu1, cov1);
+    Distribution::NormalDistributionN norm2(mu2, cov2);
+    Distribution::NormalDistributionN norm3(mu3, cov3);
+    Distribution::NormalDistributionN norm4(mu4, cov4);
+
+    double kld12 = Divergence::KullbackLeibler<float>::getMultivariateGauss(norm1, norm2);
+    double kld34 = Divergence::KullbackLeibler<float>::getMultivariateGauss(norm3, norm4);
+    std::cout << kld34 << " > " << kld12 << std::endl;
+
+    double kld12sym = Divergence::KullbackLeibler<float>::getMultivariateGaussSymmetric(norm1, norm2);
+    double kld34sym = Divergence::KullbackLeibler<float>::getMultivariateGaussSymmetric(norm3, norm4);
+    std::cout << kld34sym << " > " << kld12sym << std::endl;
+
+    ASSERT_GE(kld34, kld12);
+    ASSERT_GE(kld34sym, kld12sym);
+}
+
+TEST(KullbackLeibler, multivariateGaussGeCov) {
+
+    //ge cov
+    Eigen::VectorXd mu1(2);
+    mu1[0] = 1.0;
+    mu1[1] = 1.0;
+
+    Eigen::VectorXd mu2(2);
+    mu2[0] = 1.0;
+    mu2[1] = 1.0;
+
+    Eigen::VectorXd mu3(2);
+    mu3[0] = 1.0;
+    mu3[1] = 1.0;
+
+    Eigen::VectorXd mu4(2);
+    mu4[0] = 1.0;
+    mu4[1] = 1.0;
+
+    Eigen::MatrixXd cov1(2,2);
+    cov1(0,0) = 1.0;
+    cov1(0,1) = 0.0;
+    cov1(1,0) = 0.0;
+    cov1(1,1) = 1.0;
+
+    Eigen::MatrixXd cov2(2,2);
+    cov2(0,0) = 1.0;
+    cov2(0,1) = 0.0;
+    cov2(1,0) = 0.0;
+    cov2(1,1) = 1.0;
+
+    Eigen::MatrixXd cov3(2,2);
+    cov3(0,0) = 1.0;
+    cov3(0,1) = 0.0;
+    cov3(1,0) = 0.0;
+    cov3(1,1) = 1.0;
+
+    Eigen::MatrixXd cov4(2,2);
+    cov4(0,0) = 3.0;
+    cov4(0,1) = 0.0;
+    cov4(1,0) = 0.0;
+    cov4(1,1) = 1.0;
+
+    Distribution::NormalDistributionN norm1(mu1, cov1);
+    Distribution::NormalDistributionN norm2(mu2, cov2);
+    Distribution::NormalDistributionN norm3(mu3, cov3);
+    Distribution::NormalDistributionN norm4(mu4, cov4);
+
+    double kld12 = Divergence::KullbackLeibler<float>::getMultivariateGauss(norm1, norm2);
+    double kld34 = Divergence::KullbackLeibler<float>::getMultivariateGauss(norm3, norm4);
+    std::cout << kld34 << " >" << kld12 << std::endl;
+
+    double kld12sym = Divergence::KullbackLeibler<float>::getMultivariateGaussSymmetric(norm1, norm2);
+    double kld34sym = Divergence::KullbackLeibler<float>::getMultivariateGaussSymmetric(norm3, norm4);
+    std::cout << kld34sym << " > " << kld12sym << std::endl;
+
+    ASSERT_GE(kld34, kld12);
+    ASSERT_GE(kld34sym, kld12sym);
+}
+
+#endif

tests/sensors/beacon/TestLogDistanceCeilingModel.cpp

@@ -0,0 +1,121 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/beacon/model/BeaconModelLogDistCeiling.h"
+
+TEST(LogDistanceCeilingModelBeacon, calc) {
+
+	// dummy floorplan
+	Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
+	Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
+	Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
+
+	Floorplan::IndoorMap map;
+	map.floors.push_back(f0);
+	map.floors.push_back(f1);
+	map.floors.push_back(f2);
+
+	//LocatedAccessPoint ap0("00:00:00:00:00:00", Point3(0,0,0));
+	//LocatedAccessPoint ap25("00:00:00:00:00:00", Point3(0,0,2.5));
+
+    BeaconModelLogDistCeiling model(&map);
+
+	const MACAddress ap0 = MACAddress("00:00:00:00:00:00");
+	const MACAddress ap25 = MACAddress("00:00:00:00:00:01");
+
+    model.addBeacon(ap0, BeaconModelLogDistCeiling::APEntry( Point3(0,0,0), -40, 1.0, -8.0 ));
+    model.addBeacon(ap25, BeaconModelLogDistCeiling::APEntry( Point3(0,0,2.5), -40, 1.0, -8.0 ));
+
+
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(1,0,0)));
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,1,0)));
+	ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,0,1)));
+
+	ASSERT_EQ(-40, model.getRSSI(ap25, Point3(1,0,2.5)));
+	ASSERT_EQ(-40, model.getRSSI(ap25, Point3(0,1,2.5)));
+	ASSERT_EQ(-40-8, model.getRSSI(ap25, Point3(0,0,3.5)));		// one floor within
+
+	ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,8,0)));
+	ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,0,8))+8+8);		// two ceilings within
+
+}
+
+TEST(LogDistanceCeilingModelBeacon, numCeilings) {
+
+	// dummy floorplan
+	Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
+	Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
+	Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
+
+	Floorplan::IndoorMap map;
+	map.floors.push_back(f0);
+	map.floors.push_back(f1);
+	map.floors.push_back(f2);
+
+    BeaconModelLogDistCeiling model(&map);
+
+	ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) );
+	ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) );
+
+	ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) );
+	ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) );
+
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) );
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) );
+
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) );
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) );
+
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) );
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) );
+
+	ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) );
+
+	ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) );
+	ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) );
+
+}
+
+TEST(LogDistanceCeilingModelBeacon, numCeilingsFloat) {
+
+	// dummy floorplan
+	Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0;
+	Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3;
+	Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7;
+
+	Floorplan::IndoorMap map;
+	map.floors.push_back(f0);
+	map.floors.push_back(f1);
+	map.floors.push_back(f2);
+
+    BeaconModelLogDistCeiling model(&map);
+
+	const float d = 0.01;
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,0)), d );
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,-1)), d );
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,1)), d );
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,1), Point3(0,0,0)), d );
+
+	ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,-0.01), Point3(0,0,+0.50)), d );
+	ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,+0.50), Point3(0,0,-0.01)), d );
+
+	ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,2.99), Point3(0,0,3.20)), d );
+	ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,3.20), Point3(0,0,2.99)), d );
+
+	ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,6.99), Point3(0,0,8.33)), d );
+	ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,6.99)), d );
+	ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,0.00), Point3(0,0,8.33)), d );
+	ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,0.00)), d );
+
+	ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,7.00), Point3(0,0,99)), d );
+
+	ASSERT_NEAR(1, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,7)), d );
+	ASSERT_NEAR(3, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,8)), d );
+
+}
+
+
+#endif

tests/sensors/beacon/TestProbabilityFree.cpp

@@ -0,0 +1,7 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+//todo
+
+#endif

tests/sensors/imu/TestMotionDetection.cpp

@@ -0,0 +1,200 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../sensors/imu/MotionDetection.h"
+#include "../../../sensors/imu/TurnDetection.h"
+#include "../../../sensors/offline/FileReader.h"
+
+/** visualize the motionAxis */
+TEST(MotionDetection, motionAxis) {
+
+    MotionDetection md;
+
+    //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
+
+    //Walking with smartphone straight and always parallel to motion axis
+    //std::string filename = getDataFile("motion/straight_potrait.csv");
+
+    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+    std::string filename = getDataFile("motion/straight_landscape_left.csv");
+    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+
+    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+
+    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+
+    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+
+    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+
+    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+
+    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+    //std::string filename = getDataFile("motion/table_flat.csv");
+
+    FileReader fr(filename);
+
+    K::Gnuplot gp;
+    K::GnuplotPlot plot;
+
+    gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
+
+
+    Eigen::Vector2f curVec;
+    float motionAxisAngleRad;
+    Timestamp ts;
+    Timestamp lastTs;
+
+    //calc motion axis
+    for (const FileReader::Entry& e : fr.getEntries()) {
+
+        ts = Timestamp::fromMS(e.ts);
+
+        if (e.type == FileReader::Sensor::LIN_ACC) {
+            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+
+        } else if (e.type == FileReader::Sensor::GRAVITY) {
+            md.addGravity(ts, fr.getGravity()[e.idx].data);
+            curVec = md.getCurrentMotionAxis();
+            motionAxisAngleRad = md.getMotionChangeInRad();
+        }
+
+        // start with the first available timestamp
+        if (lastTs.isZero()) {lastTs = ts;}
+
+        if(ts - lastTs > Timestamp::fromMS(500)) {
+
+            lastTs = ts;
+
+            K::GnuplotPoint2 raw_p1(0, 0);
+            K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
+            K::GnuplotPlotElementLines motionLines;
+            motionLines.addSegment(raw_p1, raw_p2);
+            plot.add(&motionLines);
+
+            gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159  << "' at screen 0.1,0.1\n";
+
+            gp.draw(plot);
+            gp.flush();
+            //usleep(5000*33);
+        }
+    }
+
+    //was passiert bei grenzwerten. 90° oder sowas.
+    //wie stabil ist die motion axis eigentlich?
+    //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
+    //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
+
+}
+
+/** comparing motionAngle and turnAngle */
+TEST(MotionDetection, motionAngle) {
+
+    MotionDetection md;
+    TurnDetection td;
+
+    //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
+
+    //Walking with smartphone straight and always parallel to motion axis
+    std::string filename = getDataFile("motion/straight_potrait.csv");
+
+    //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
+    //std::string filename = getDataFile("motion/straight_landscape_left.csv");
+    //std::string filename = getDataFile("motion/straight_landscape_right.csv");
+
+    //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
+    //std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
+
+    //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait.csv");
+
+    //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_landscape.csv");
+
+    //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
+    //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
+
+    //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
+    //std::string filename = getDataFile("motion/rounds_pocket.csv");
+
+    //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
+    //std::string filename = getDataFile("motion/table_flat.csv");
+
+    FileReader fr(filename);
+    Timestamp ts;
+
+    //save for later plotting
+    std::vector<float> delta_motionAngles;
+    std::vector<float> delta_turnAngles;
+
+    //calc motion axis
+    for (const FileReader::Entry& e : fr.getEntries()) {
+
+        ts = Timestamp::fromMS(e.ts);
+
+        if (e.type == FileReader::Sensor::LIN_ACC) {
+            md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
+
+        } else if (e.type == FileReader::Sensor::GRAVITY) {
+            md.addGravity(ts, fr.getGravity()[e.idx].data);
+            delta_motionAngles.push_back(md.getMotionChangeInRad());
+
+        } else if (e.type == FileReader::Sensor::ACC) {
+            const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
+            td.addAccelerometer(ts, _acc.data);
+
+        } else if (e.type == FileReader::Sensor::GYRO) {
+            const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
+            delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
+        }
+
+    }
+
+    //draw motion
+    static K::Gnuplot gpMotion;
+    K::GnuplotPlot plotMotion;
+    K::GnuplotPlotElementLines motionLines;
+
+    for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
+
+        K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
+        K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
+        motionLines.addSegment(raw_p1, raw_p2);
+
+    }
+
+    gpMotion << "set title 'Motion Detection'\n";
+    plotMotion.add(&motionLines);
+    gpMotion.draw(plotMotion);
+    gpMotion.flush();
+
+
+    //draw rotation
+    static K::Gnuplot gpTurn;
+    K::GnuplotPlot plotTurn;
+    K::GnuplotPlotElementLines turnLines;
+
+    for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
+
+        K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
+        K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
+        turnLines.addSegment(raw_p1, raw_p2);
+    }
+
+    gpTurn << "set title 'Turn Detection'\n";
+    plotTurn.add(&turnLines);
+    gpTurn.draw(plotTurn);
+    gpTurn.flush();
+
+    sleep(1);
+
+}
+
+
+#endif

tests/sensors/pressure/TestBarometer.cpp

@@ -5,6 +5,7 @@
 #include "../../../sensors/pressure/RelativePressure.h"
 #include "../../../sensors/pressure/PressureTendence.h"
 #include "../../../sensors/pressure/ActivityButterPressure.h"
+#include "../../../sensors/pressure/ActivityButterPressurePercent.h"
 
 #include <random>
 
@@ -78,7 +79,7 @@ TEST(Barometer, LIVE_tendence) {
 
 	}
 
-	sleep(1000);
+    sleep(1);
 
 }
 
@@ -114,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) {
 
 	}
 
-	sleep(1000);
+    sleep(1);
 
 
 	// tendence must be clear and smaller than the sigma
@@ -130,6 +131,9 @@ TEST(Barometer, Activity) {
 	std::string filename = getDataFile("barometer/baro1.dat");
     std::ifstream infile(filename);
 
+    std::vector<ActivityButterPressure::History> actHist;
+    std::vector<ActivityButterPressure::History> rawHist;
+
     while (std::getline(infile, line))
     {
         std::istringstream iss(line);
@@ -138,34 +142,101 @@ TEST(Barometer, Activity) {
 
         while (iss >> ts >> value) {
             ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value));
+            rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
+            actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct)));
         }
     }
 
-    std::vector<float> sum = act.getSumHistory();
-    std::vector<float> interpolated = act.getInterpolatedHistory();
-    std::vector<float> raw = act.getSensorHistory();
-    std::vector<float> butter = act.getOutputHistory();
-    std::vector<ActivityButterPressure::History> actHist = act.getActHistory();
-
     K::Gnuplot gp;
+    K::Gnuplot gpRaw;
     K::GnuplotPlot plot;
+    K::GnuplotPlot plotRaw;
     K::GnuplotPlotElementLines rawLines;
+    K::GnuplotPlotElementLines resultLines;
 
     for(int i=0; i < actHist.size()-1; ++i){
 
+        //raw
+        K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
+        K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
+
+        rawLines.addSegment(raw_p1, raw_p2);
+
+        //results
         K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
         K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
 
-        rawLines.addSegment(input_p1, input_p2);
+        resultLines.addSegment(input_p1, input_p2);
     }
 
-    plot.add(&rawLines);
+    plotRaw.add(&rawLines);
+    plot.add(&resultLines);
 
     gp.draw(plot);
     gp.flush();
 
-    sleep(1);
+    gpRaw.draw(plotRaw);
+    gpRaw.flush();
 
+    sleep(5);
+
+}
+
+TEST(Barometer, ActivityPercent) {
+
+    ActivityButterPressurePercent act;
+
+    //read file
+    std::string line;
+    std::string filename = getDataFile("barometer/baro1.dat");
+    std::ifstream infile(filename);
+
+    std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
+    std::vector<double> rawHist;
+
+    while (std::getline(infile, line))
+    {
+        std::istringstream iss(line);
+        int ts;
+        double value;
+
+        while (iss >> ts >> value) {
+            ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
+            rawHist.push_back(value);
+            actHist.push_back(activity);
+        }
+    }
+
+    K::Gnuplot gp;
+    K::Gnuplot gpRaw;
+    K::GnuplotPlot plot;
+    K::GnuplotPlot plotRaw;
+    K::GnuplotPlotElementLines rawLines;
+    K::GnuplotPlotElementLines resultLines;
+
+    for(int i=0; i < actHist.size()-1; ++i){
+
+        K::GnuplotPoint2 raw_p1(i, rawHist[i]);
+        K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
+
+        rawLines.addSegment(raw_p1, raw_p2);
+
+        K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
+        K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
+
+        resultLines.addSegment(input_p1, input_p2);
+    }
+
+    plotRaw.add(&rawLines);
+    plot.add(&resultLines);
+
+    gp.draw(plot);
+    gp.flush();
+
+    gpRaw.draw(plotRaw);
+    gpRaw.flush();
+
+    sleep(5);
 }