109 lines
3.3 KiB
C++
Executable File
109 lines
3.3 KiB
C++
Executable File
#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 = 20.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;
|
|
|
|
// floor difference?
|
|
const double floorDist = std::abs(beacon->zNr - state.z_nr);
|
|
|
|
// 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) * 2.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
|