added code from fusion2016

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