added code from fusion2016

code/frank/BeaconEvaluation.h

@@ -0,0 +1,110 @@
+#ifndef BEACONEVALUATION_H
+#define BEACONEVALUATION_H
+
+#include <KLib/math/distribution/Normal.h>
+#include "BeaconObservation.h"
+#include "Settings.h"
+#include "../particles/MyState.h"
+#include "../particles/MyObservation.h"
+#include "PositionedBeacon.h"
+
+class BeaconEvaluation {
+
+private:
+
+	Settings settings;
+	//BeaconObservation obs;
+
+public:
+
+	double getProbability(const MyState& state, const MyObservation& observation) const {
+
+		//if (obs.entries.empty()) {return 1.0;}
+		double prob = 1.0;
+
+//		const double tx = -74;
+		const double waf = 8.0;
+
+//		// get the ap the client had the strongest measurement for
+//		const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
+//		const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
+//		const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
+//		const double mdlStrongestRSSI = distanceToRssi(tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
+
+		// process each detected beacon
+		for (const BeaconObservationEntry& entry : observation.beacons.entries) {
+
+			// get the AP data from the settings
+			const PositionedBeacon* beacon = settings.getBeacon(entry.mac);
+			if (!beacon) {continue;}
+
+			// distance (in meter) between particle and AP
+			//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
+			const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0;
+
+			// floor difference?
+			//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
+			const float floorDist = std::ceil(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
+
+			// estimate the rssi depending on above distance
+			const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
+
+			// the measured rssi
+			const double realRSSI = entry.rssi;
+
+//			// the measured relative rssi
+//			const double realRelRSSI = strongest.rssi - realRSSI;
+//			const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
+
+			// probability? (sigma grows with measurement's age)
+			const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
+			const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);
+			//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);
+
+			//prob *= p;
+			prob += std::log(p);
+
+		}
+
+		const double lambda = 0.15;
+		const double res = lambda * exp(- lambda * (-prob));
+		return res;
+		//return prob;
+
+	}
+
+//	WiFiObservation filter(const WiFiObservation* obs) const {
+
+//		WiFiObservation out;
+//		out.ts = obs->ts;
+//		for (const WiFiObservationEntry& entry : obs->entries) {
+//			// alter the mac
+//			WiFiObservationEntry ne = entry;
+//			ne.mac[ne.mac.length()-1] = '0';
+//			if (settings.getAP(ne.mac)) {out.entries.push_back(ne);}
+//		}
+//		return out;
+
+//	}
+
+//	/** get the strongest AP within all measurements */
+//	WiFiObservationEntry getStrongest(const WiFiObservation* obs) const {
+//		WiFiObservationEntry max = obs->entries.front();
+//		for (const WiFiObservationEntry& entry : obs->entries) {
+//			if (entry.rssi > max.rssi) {max = entry;}
+//		}
+//		return max;
+//	}
+
+	static double rssiToDistance(double txPower, double rssi, double pathLoss) {
+		return pow(10, (txPower - rssi) / (10 * pathLoss));
+	}
+
+	static double distanceToRssi(double txPower, double distance, double pathLoss) {
+		if (distance <= 1) {return txPower;}
+		return (txPower - (10 * pathLoss * log10(distance)));
+	}
+
+};
+
+#endif // BEACONEVALUATION_H

code/frank/BeaconObservation.h

@@ -0,0 +1,41 @@
+#ifndef BEACONOBSERVATION_H
+#define BEACONOBSERVATION_H
+
+#include "MACAddress.h"
+#include <vector>
+
+
+/** one observed AP and its signal strength */
+struct BeaconObservationEntry {
+
+	/** the timestamp this beacon was discovered at */
+	uint64_t ts;
+
+	/** the beacon's mac address */
+	std::string mac;
+
+	/** the beacon's rssi */
+	int rssi;
+
+	BeaconObservationEntry() : ts(0), mac(), rssi(0) {;}
+	BeaconObservationEntry(const uint64_t ts, const std::string& mac, const int rssi) : ts(ts), mac(mac), rssi(rssi) {;}
+
+};
+
+/** all APs observed during one scan */
+struct BeaconObservation {
+
+	std::vector<BeaconObservationEntry> entries;
+
+	void removeOld(uint64_t latestTS) {
+		auto lambda = [latestTS] (const BeaconObservationEntry& e) {
+			uint64_t age = latestTS - e.ts;
+			return age > 1000*3;
+		};
+		entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end());
+	}
+
+};
+
+
+#endif // BEACONOBSERVATION_H

code/frank/BeaconSensorReader.h

@@ -0,0 +1,60 @@
+#ifndef BEACONSENSORREADER_H
+#define BEACONSENSORREADER_H
+
+#include "../reader/SensorReader.h"
+#include "BeaconObservation.h"
+#include "Settings.h"
+#include <cassert>
+
+class BeaconSensorReader {
+
+public:
+
+//	/** get wifi observation data from one CSV entry */
+//	static BeaconObservation* readBeacons(const SensorEntry& se) {
+
+//		std::string tmp = se.data;
+//		BeaconObservation* obs = new BeaconObservation();
+//		obs->ts = se.ts;
+
+//		std::string mac = tmp.substr(0, 17);
+//		tmp = tmp.substr(17);
+//		assert(tmp[0] == ';');	tmp = tmp.substr(1);
+
+//		std::string rssi = tmp;
+
+//		BeaconObservationEntry e(mac, std::stoi(rssi));
+//		obs->entries.push_back(e);
+
+//		/** skip unknown beacons */
+//		if (settings.getBeacon(mac) == nullptr) {return nullptr;}
+
+//		return obs;
+
+//	}
+
+	/** get wifi observation data from one CSV entry */
+	static BeaconObservationEntry getBeacon(const SensorEntry& se) {
+
+		BeaconObservationEntry boe;
+		std::string tmp = se.data;
+
+		std::string mac = tmp.substr(0, 17);
+		tmp = tmp.substr(17);
+		assert(tmp[0] == ';');	tmp = tmp.substr(1);
+
+		std::string rssi = tmp;
+
+		BeaconObservationEntry e(se.ts, mac, std::stoi(rssi));
+
+		/** skip unknown beacons */
+		if (settings.getBeacon(mac) == nullptr) {return BeaconObservationEntry();}
+
+		return e;
+
+	}
+
+};
+
+
+#endif // BEACONSENSORREADER_H

code/frank/MACAddress.h

@@ -0,0 +1,131 @@
+#ifndef MACADDRESS_H
+#define MACADDRESS_H
+
+#include <cstdint>
+#include <string>
+
+/**
+ * describe a MAC-Address as 64-bit integer
+ * or 8-bit access to all fields
+ */
+union MACAddressValue {
+
+	struct {
+		uint8_t h5;
+		uint8_t h4;
+		uint8_t h3;
+		uint8_t h2;
+		uint8_t h1;
+		uint8_t h0;
+	};
+
+	uint64_t mac;
+
+	/** initialize everything with zeros */
+	MACAddressValue() : mac(0) {;}
+
+};
+
+class MACAddress {
+
+private:
+
+	/** the address as integer value */
+	MACAddressValue value;
+
+public:
+
+	/** empty ctor */
+	MACAddress() {
+		;
+	}
+
+	/** copy ctor */
+	MACAddress(const MACAddress& o) : value(o.value) {
+		;
+	}
+
+	/** ctor form string (e.g. "xx:xx:xx:xx:xx:xx") */
+	MACAddress(const std::string& str) {
+
+		// sanity check
+		if (str.size() != 17) {throw "invalid hex string length. must be 17";}
+
+		value.mac = 0;	// all zeros
+		value.h5 = hexWordToInt(str[ 0], str[ 1]);
+		value.h4 = hexWordToInt(str[ 3], str[ 4]);
+		value.h3 = hexWordToInt(str[ 6], str[ 7]);
+		value.h2 = hexWordToInt(str[ 9], str[10]);
+		value.h1 = hexWordToInt(str[12], str[13]);
+		value.h0 = hexWordToInt(str[15], str[16]);
+
+	}
+
+	/** convert to hex-string ("xx:xx:xx:xx:xx:xx") */
+	std::string asString() {
+
+		std::string str = ":::::::::::::::::";
+
+		intToHexStr(value.h5, &str[ 0]);
+		intToHexStr(value.h4, &str[ 3]);
+		intToHexStr(value.h3, &str[ 6]);
+		intToHexStr(value.h2, &str[ 9]);
+		intToHexStr(value.h1, &str[12]);
+		intToHexStr(value.h0, &str[15]);
+
+		return str;
+
+	}
+
+	/** get the mac address as a long-int value */
+	uint64_t asLong() const {
+		return value.mac;
+	}
+
+	/** equal? */
+	bool operator == (const MACAddress& o) const {
+		return o.asLong() == asLong();
+	}
+
+private:
+
+	/** convert the given hex char [0-F] to an integer [0-15] */
+	static uint8_t hexCharToInt(char hex) {
+
+		// to upper case
+		if (hex >= 'a') {hex -= 'a' - 'A';}
+
+		// convert
+		return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10);
+
+	}
+
+	/** convert the given hex-word to an integer */
+	static uint8_t hexWordToInt(char hi, char lo) {
+		return hexCharToInt(hi) << 4 | hexCharToInt(lo);
+	}
+
+	/** conver the given integer [0-15] to a hex char [0-F] */
+	static char intToHexChar(const uint8_t val) {
+		return (val < 10) ? ('0' + val) : ('A' - 10 + val);
+	}
+
+	/** insert two hex chars into the provided string buffer */
+	static void intToHexStr(const uint8_t val, char* dst) {
+		dst[0] = intToHexChar((val >> 4) & 0xF);
+		dst[1] = intToHexChar((val >> 0) & 0xF);
+	}
+
+};
+
+
+/** hash-method for MAC-Addresses */
+namespace std {
+	template <> struct hash<MACAddress> {
+		std::size_t operator() (const MACAddress& mac) const {
+			return std::hash<uint64_t>()(mac.asLong());
+		}
+	};
+}
+
+#endif // MACADDRESS_H

code/frank/OrientationObservation.h

@@ -0,0 +1,11 @@
+#ifndef ORIENTATIONOBSERVATION_H
+#define ORIENTATIONOBSERVATION_H
+
+/** android device orientation */
+struct OrientationObservation {
+
+	float values[3];
+
+};
+
+#endif // ORIENTATIONOBSERVATION_H

code/frank/OrientationSensorReader.h

@@ -0,0 +1,42 @@
+#ifndef ORIENTATIONSENSORREADER_H
+#define ORIENTATIONSENSORREADER_H
+
+
+#include "../reader/SensorReader.h"
+#include "OrientationObservation.h"
+
+#include <cassert>
+
+class OrientationSensorReader {
+
+public:
+
+	/** get wifi observation data from one CSV entry */
+	static OrientationObservation read(const SensorEntry& se) {
+
+		std::string tmp = se.data;
+		OrientationObservation obs;
+
+		size_t pos1 = tmp.find(';');
+		size_t pos2 = tmp.find(';', pos1+1);
+		size_t pos3 = tmp.find(';', pos2+1);
+
+		assert(pos1 != std::string::npos);
+		assert(pos2 != std::string::npos);
+		assert(pos3 != std::string::npos);
+
+		const std::string s1 = tmp.substr(0, pos1);
+		const std::string s2 = tmp.substr(pos1+1, pos2-pos1-1);
+		const std::string s3 = tmp.substr(pos2+1, pos3-pos2-1);
+
+		obs.values[0] = std::stof(s1);
+		obs.values[1] = std::stof(s2);
+		obs.values[2] = std::stof(s3);
+
+		return obs;
+
+	}
+
+};
+
+#endif // ORIENTATIONSENSORREADER_H

code/frank/Position3D.h

@@ -0,0 +1,36 @@
+#ifndef POSITION3D_H
+#define POSITION3D_H
+
+#include <cmath>
+
+/**
+ * represents a 3D position (x,y,z)
+ */
+struct Position3D {
+
+	/** x-position (in centimeter) */
+	double xCM;
+
+	/** y-position (in centimeter) */
+	double yCM;
+
+	/** floor number */
+	int zNr;
+
+	/** ctor */
+	Position3D() : xCM(0), yCM(0), zNr(0) {;}
+
+	/** ctor. x,y in centimeter, z = floor-number */
+	Position3D(const double xCM, const double yCM, const int zNr) : xCM(xCM), yCM(yCM), zNr(zNr) {;}
+
+	/** get the distance to the given position (in centimeter) */
+	double getDistanceCM(const Position3D& p) const {
+		const double dx = xCM - p.xCM;
+		const double dy = yCM - p.yCM;
+		const double dz = (zNr - p.zNr) * 300;			// 300 = average floor height (centimeter)
+		return std::sqrt(dx*dx + dy*dy + dz*dz);
+	}
+
+};
+
+#endif // POSITION3D_H

code/frank/PositionedBeacon.h

@@ -0,0 +1,30 @@
+#ifndef POSITIONEDBEACON_H
+#define POSITIONEDBEACON_H
+
+#include "WiFiAP.h"
+//#include "Position3D.h"
+#include <Indoor/geo/Point3.h>
+
+class PositionedBeacon : public Point3  {
+
+public:
+
+	MACAddress mac;
+	float tx;
+	float pl;
+
+//	/** ctor */
+//	PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
+//		mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
+//		;
+//	}
+
+	/** ctor */
+	PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
+		Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) {
+		;
+	}
+
+};
+
+#endif // POSITIONEDBEACON_H

code/frank/PositionedWiFiAP.h

@@ -0,0 +1,26 @@
+#ifndef POSITIONEDWIFIAP_H
+#define POSITIONEDWIFIAP_H
+
+#include "WiFiAP.h"
+//#include "Position3D.h"
+#include <Indoor/geo/Point3.h>
+
+class PositionedWifiAP : public WiFiAP, public Point3  {
+
+public:
+
+//	/** ctor */
+//	PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
+//		WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
+//		;
+//	}
+
+	/** ctor */
+	PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
+		WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) {
+		;
+	}
+
+};
+
+#endif // POSITIONEDWIFIAP_H

code/frank/Settings.h

@@ -0,0 +1,178 @@
+#ifndef SETTINGS_H
+#define SETTINGS_H
+
+#include "PositionedWiFiAP.h"
+#include "PositionedBeacon.h"
+#include "MACAddress.h"
+
+#include <unordered_map>
+
+#include "../Helper.h"
+
+class Settings {
+
+private:
+
+	std::unordered_map<MACAddress, PositionedWifiAP*> aps;
+	std::unordered_map<MACAddress, PositionedBeacon*> beacons;
+
+public:
+
+	Settings() {
+
+		const double pl = 2.7;		// 2.7
+		const double tx = -46;
+		const float ibPLE = 1.5;
+
+		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  Helper::getHeight(3), tx, pl);
+		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  Helper::getHeight(3), tx, pl);
+		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl);
+		addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
+		addAP(("00:04:96:77:ed:69"), "i.3.3",  6460, 3400, Helper::getHeight(3), tx, pl);
+
+		// 2nd floor (vague AP position)
+		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:6b:bf:f9"), "I.2.9",  3000, 3350, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:77:ec:a9"), "I.2.15",  290,  750, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, Helper::getHeight(2), tx, pl);
+		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
+
+		// 1st floor (vague AP position)
+		addAP(("00:04:96:6c:cf:19"), "I.1.2",  6150, 3420, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:7d:07:79"), "I.1.9",  1800, 3300, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:6b:45:59"), "I.1.30",  800, 4800, Helper::getHeight(1), tx, pl);
+		addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
+
+		// 0th floor (exact AP position)
+		addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530,  4970, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030,  270, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:A4:A9"), "I.0.9",  1660, 2780, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:77:EE:69"), "I.0.7",  3560, 3380, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6B:46:09"), "I.0.xx",  6860, 3690, Helper::getHeight(0), tx, pl);
+		addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl);		// vague!!
+
+		addBeacon("48:EF:8D:77:66:DF", -81, ibPLE, 6984, 4526, Helper::getHeight(2));
+		addBeacon("6F:5F:39:0C:51:E4", -81, ibPLE, 7829, 3916, 200);
+		addBeacon("49:23:D8:7F:E8:D2", -81, ibPLE, 6946, 4536, Helper::getHeight(1));
+
+		// OLD
+
+//		const double pl = 2.7;		// 2.7
+//		const double tx = -46;
+
+//		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  Helper::getHeight(3), tx, pl-1);
+//		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  Helper::getHeight(3), tx, pl);
+//		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1);
+//		addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
+//		addAP(("00:04:96:77:ed:69"), "i.3.3",  6460, 3400, Helper::getHeight(3), tx, pl);
+
+//		// 2nd floor (vague AP position)
+//		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, Helper::getHeight(2), tx, pl);
+//		addAP(("00:04:96:6b:bf:f9"), "I.2.9",  3000, 3350, Helper::getHeight(2), tx, pl);
+//		addAP(("00:04:96:77:ec:a9"), "I.2.15",  290,  750, Helper::getHeight(2), tx, pl);
+//		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, Helper::getHeight(2), tx, pl);
+//		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
+
+//		// 1st floor (vague AP position)
+//		addAP(("00:04:96:6c:cf:19"), "I.1.2",  6150, 3420, Helper::getHeight(1), tx, pl);
+//		addAP(("00:04:96:7d:07:79"), "I.1.9",  1800, 3300, Helper::getHeight(1), tx, pl);
+//		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, Helper::getHeight(1), tx, pl);
+//		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, Helper::getHeight(1), tx, pl);
+//		addAP(("00:04:96:6b:45:59"), "I.1.30",  800, 4800, Helper::getHeight(1), tx, pl);
+//		addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
+
+//		// 0th floor (exact AP position)
+//		addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530,  4970, Helper::getHeight(0), tx, pl);
+//		addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030,  270, Helper::getHeight(0), tx, pl);
+//		addAP(("00:04:96:6C:A4:A9"), "I.0.9",  1660, 2780, Helper::getHeight(0), tx, pl);
+//		addAP(("00:04:96:77:EE:69"), "I.0.7",  3560, 3380, Helper::getHeight(0), tx, pl);
+//		addAP(("00:04:96:6B:46:09"), "I.0.xx",  6860, 3690, Helper::getHeight(0), tx, pl);
+//		addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl);		// vague!!
+
+
+//		const int ibOff = +2;
+//		const float ibPLE = 1.9;
+//		addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3));		// id:16
+//		addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2));		// id:18
+//		addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1));		// id:11
+//		addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0));		// id:20
+
+//		addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2));	// idis switchboard
+//		addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2));		// idis outside
+//		addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2));		// idis toni
+
+//		addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1));		// i.1.47
+//		addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1));		// fachschaft
+//		addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1));		// neben fachschaft
+
+////		addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
+////		addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
+//		addBeacon("78:A5:04:1F:91:08", -88, ibPLE, 1448, 4538, 3);
+//		addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 2028, 4528, 3);
+
+	}
+
+	/** get the AP behind the given MAC (if any) */
+	const PositionedWifiAP* getAP(const MACAddress& mac) const {
+		auto it = aps.find(mac);
+		if (it == aps.end()) {return nullptr;}
+		return (it->second);
+	}
+
+	/** get the Beacon behind the given MAC (if any) */
+	const PositionedBeacon* getBeacon(const MACAddress& mac) const {
+		auto it = beacons.find(mac);
+		if (it == beacons.end()) {return nullptr;}
+		return (it->second);
+	}
+
+private:
+
+	/** add a new known AP */
+	void addAP(const std::string& mac, const std::string& room, const double x_cm, const double y_cm, const int floor, const double tx, const double pl) {
+		std::string mac2 = mac;
+		//mac2[mac2.length()-1] = '9';
+		PositionedWifiAP* pap = new PositionedWifiAP(MACAddress(mac2), room, tx, pl, x_cm, y_cm, floor);
+		aps[mac2] = pap;
+	}
+
+	/** add a new known Beacon */
+	void addBeacon(const std::string& mac, const double tx, const double pl, const double x_cm, const double y_cm, const int floor) {
+		PositionedBeacon* pap = new PositionedBeacon(MACAddress(mac), tx, pl, x_cm, y_cm, floor);
+		beacons[mac] = pap;
+	}
+
+//	// access points
+//	PositionedWifiAP aps[] = {
+
+////		// 3rd floor (excat AP position)
+////		PositionedWifiAP(MACAddress("00:04:96:6b:64:90"), "i.3.20", 290, 1300, 3),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:70:c0"), "i.3.25", 290, 3930, 3),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:82:70"), "i.3.16", 1860, 3400, 3),
+////		PositionedWifiAP(MACAddress("00:04:96:77:ed:f0"), "i.3.39", 4700, 4850, 3),
+////		PositionedWifiAP(MACAddress("00:04:96:77:ed:60"), "i.3.3", 6460, 3400, 3),
+
+////		// 2nd floor (vague AP position)
+////		PositionedWifiAP(MACAddress("00:04:96:6c:3a:a9"), "I.2.1", 6300, 3600, 2),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:bf:89"), "I.2.8", 3300, 3500, 2),
+////		PositionedWifiAP(MACAddress("00:04:96:77:ec:a9"), "I.2.15", 300, 1300, 2),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:db:69"), "I.2.34", 4400, 4800, 2),
+
+////		// 1st floor (vague AP position)
+////		PositionedWifiAP(MACAddress("00:04:96:6c:cf:19"), "I.1.2", 5700, 3500, 1),
+////		PositionedWifiAP(MACAddress("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1),
+////		PositionedWifiAP(MACAddress("00:04:96:69:48:89"), "I.1.17", 1500, 300, 1),
+////		PositionedWifiAP(MACAddress("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1),
+////		PositionedWifiAP(MACAddress("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1),
+////		PositionedWifiAP(MACAddress("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1),
+
+//	};
+
+};
+
+extern Settings settings;
+
+#endif // SETTINGS_H

code/frank/WiFiAP.h

@@ -0,0 +1,45 @@
+#ifndef WIFIAP_H
+#define WIFIAP_H
+
+#include "MACAddress.h"
+#include <ostream>
+
+
+/**
+ * represents a WiFi-AccessPoint
+ * an AP is represented by its MAC-Address and
+ * may provide a readably SSID
+ */
+class WiFiAP {
+
+public:
+
+	/** the AP's MAC-Address */
+	const MACAddress mac;
+
+	/** the AP's readable SSID */
+	const std::string ssid;
+
+	/** AP tx-power */
+	const float tx;
+
+	/** path loss for this ap. for testing */
+	const float pl;
+
+
+
+public:
+
+	/** ctor */
+	WiFiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) {
+		;
+	}
+
+	/** ctor */
+	WiFiAP(const std::string& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) {
+		;
+	}
+
+};
+
+#endif // WIFIAP_H

code/frank/WiFiEvaluation.h

@@ -0,0 +1,128 @@
+#ifndef WIFIEVALUATION_H
+#define WIFIEVALUATION_H
+
+#include "../particles/MyState.h"
+#include "WiFiObservation.h"
+#include "PositionedWiFiAP.h"
+#include "Settings.h"
+#include "../particles/MyObservation.h"
+
+#include <KLib/math/distribution/Normal.h>
+
+
+
+class WiFiEvaluation {
+
+private:
+
+	Settings settings;
+	WiFiObservation obs;
+	WiFiObservationEntry strongest;
+
+public:
+
+	void nextObservation(const WiFiObservation& _obs) {
+
+		if (_obs.entries.empty()) {return;}
+
+		obs = filter(_obs);
+		strongest = getStrongest(&obs);
+
+	}
+
+	double getProbability(const MyState& state, const MyObservation& observation) const {
+
+		if (obs.entries.empty()) {return 1.0;}
+		double prob = 0;//1.0;
+
+		//const double tx = -48;		// tablet
+		//const double pl = 3.15;
+		const float waf = 8;//10.0;		// was 7 before?! has something todo with the floor heights / levels
+
+		// get the ap the client had the strongest measurement for
+		//const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
+		//const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
+		//const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
+		//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
+
+		// process each detected AP
+		for (const WiFiObservationEntry& entry : obs.entries) {
+
+			// get the AP data from the settings
+			const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);
+
+			// distance (in meter) between particle and AP
+			const float distToAP_m = state.pCur.getDistance(*ap) / 100.0;
+
+			// floor difference?
+			const float floorDiff = std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
+			//const float floorDiff = std::round(std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)));
+
+			//const float floorDiff = std::abs(ap->z - state.pCur.z) / 340;
+
+			// estimate the rssi depending on above distance
+			const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);
+
+			// the measured rssi
+			const double realRSSI = entry.rssi;
+
+			// the measured relative rssi
+			//const double realRelRSSI = strongest.rssi - realRSSI;
+			//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
+
+			// probability? (sigma grows with measurement's age)
+			const double sigma = (8) + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.5;
+			const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);			// absolute
+			//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);	// relative
+
+			//prob *= p;
+			prob += std::log(p);
+
+		}
+
+		//const double lambda = 0.05;//0.15; //0.12;
+		//return lambda * exp(- lambda * (-prob));
+		return std::pow(std::exp(prob), 0.1);
+//		if (prob < -30)		{return 0.01;}
+//		if (prob < -15)		{return 0.50;}
+//		else				{return 1.00;}
+
+
+		//return prob;
+
+	}
+
+	WiFiObservation filter(const WiFiObservation& obs) const {
+
+		WiFiObservation out;
+		for (const WiFiObservationEntry& entry : obs.entries) {
+			// alter the mac
+			WiFiObservationEntry ne = entry;
+			//ne.mac[ne.mac.length()-1] = '0';		// enabled = VAP grouping. also adjust settings to use ending "0"
+			if (settings.getAP(ne.mac)) {out.entries.push_back(ne);}
+		}
+		return out;
+
+	}
+
+	/** get the strongest AP within all measurements */
+	WiFiObservationEntry getStrongest(const WiFiObservation* obs) const {
+		WiFiObservationEntry max = obs->entries.front();
+		for (const WiFiObservationEntry& entry : obs->entries) {
+			if (entry.rssi > max.rssi) {max = entry;}
+		}
+		return max;
+	}
+
+	static double rssiToDistance(double txPower, double rssi, double pathLoss) {
+		return pow(10, (txPower - rssi) / (10 * pathLoss));
+	}
+
+	static double distanceToRssi(double txPower, double distance, double pathLoss) {
+		if (distance <= 1) {return txPower;}
+		return (txPower - (10 * pathLoss * log10(distance)));
+	}
+
+};
+
+#endif // WIFIEVALUATION_H

code/frank/WiFiHelper.h

@@ -0,0 +1,4 @@
+#ifndef WIFIHELPER_H
+#define WIFIHELPER_H
+
+#endif // WIFIHELPER_H

code/frank/WiFiObservation.h

@@ -0,0 +1,23 @@
+#ifndef WIFIOBSERVATION_H
+#define WIFIOBSERVATION_H
+
+#include "MACAddress.h"
+#include <vector>
+
+/** one observed AP and its signal strength */
+struct WiFiObservationEntry {
+	uint64_t ts;
+	std::string mac;
+	int freq;
+	int rssi;
+	WiFiObservationEntry() {;}
+	WiFiObservationEntry(const uint64_t ts, const std::string& mac, const int freq, const int rssi) : ts(ts), mac(mac), freq(freq), rssi(rssi) {;}
+};
+
+/** all APs observed during one scan */
+struct WiFiObservation {
+
+	std::vector<WiFiObservationEntry> entries;
+};
+
+#endif // WIFIOBSERVATION_H

code/frank/WiFiSensorReader.h

@@ -0,0 +1,46 @@
+#ifndef WIFISENSORREADER_H
+#define WIFISENSORREADER_H
+
+#include "../reader/SensorReader.h"
+#include "WiFiObservation.h"
+
+#include <cassert>
+
+class WiFiSensorReader {
+
+public:
+
+	/** get wifi observation data from one CSV entry */
+	static WiFiObservation readWifi(const SensorEntry& se) {
+
+		std::string tmp = se.data;
+		WiFiObservation obs;
+
+		// process all APs
+		while(!tmp.empty()) {
+
+			std::string mac = tmp.substr(0, 17);
+			tmp = tmp.substr(17);
+			assert(tmp[0] == ';');	tmp = tmp.substr(1);
+
+			std::string freq = tmp.substr(0, 4);
+			tmp = tmp.substr(4);
+			assert(tmp[0] == ';');	tmp = tmp.substr(1);
+
+			int pos = tmp.find(';');
+			std::string rssi = tmp.substr(0, pos);
+			tmp = tmp.substr(pos);
+			assert(tmp[0] == ';'); tmp = tmp.substr(1);
+
+			WiFiObservationEntry e(se.ts, mac, std::stoi(freq), std::stoi(rssi));
+			obs.entries.push_back(e);
+
+		}
+
+		return obs;
+
+	}
+
+};
+
+#endif // WIFISENSORREADER_H