121 lines
3.5 KiB
C++
Executable File
121 lines
3.5 KiB
C++
Executable File
#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 double waf = 7;//10.0;
|
|
const double floor_height_cm = 350;
|
|
|
|
// 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 double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
|
|
|
|
// floor difference?
|
|
const double floorDist = std::abs(ap->zNr - state.z_nr);
|
|
|
|
// estimate the rssi depending on above distance
|
|
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->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); // absolute
|
|
//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); // relative
|
|
|
|
//prob *= p;
|
|
prob += std::log(p);
|
|
|
|
}
|
|
|
|
const double lambda = 0.25; //0.12;
|
|
return lambda * exp(- lambda * (-prob));
|
|
//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
|