added code from fusion2016

2016-03-01 15:04:46 +01:00
commit 8d2be0f8a0
97 changed files with 19831 additions and 0 deletions
--- a/code/frank/BeaconEvaluation.h
+++ b/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