101 lines
2.0 KiB
C++
101 lines
2.0 KiB
C++
#ifndef WIFIOBSERVATION_H
|
|
#define WIFIOBSERVATION_H
|
|
|
|
#include <vector>
|
|
#include <fstream>
|
|
|
|
#include "../MACAddress.h"
|
|
#include "../../math/Distributions.h"
|
|
|
|
|
|
/** observed one AP with the given signal strength */
|
|
struct WiFiObservationEntry {
|
|
|
|
/** AP's MAC address */
|
|
MACAddress mac;
|
|
|
|
/** AP's RSSI */
|
|
float rssi;
|
|
|
|
/** ctor */
|
|
WiFiObservationEntry(const MACAddress& mac, const float rssi) : mac(mac), rssi(rssi) {;}
|
|
|
|
};
|
|
|
|
/** all observed APs and their signal strength */
|
|
struct WiFiObservation {
|
|
|
|
/** one entry per AP */
|
|
std::vector<WiFiObservationEntry> entries;
|
|
|
|
};
|
|
|
|
|
|
class WiFiObserver {
|
|
|
|
private:
|
|
|
|
float sigma = 8.0f;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
WiFiObserver(const float sigma) : sigma(sigma) {
|
|
;
|
|
}
|
|
|
|
/**
|
|
* get the given GridNode's probability based on the provided WiFi measurements
|
|
*/
|
|
template <typename Node> double getProbability(const Node& n, const WiFiObservation& obs, const int age_ms = 0) {
|
|
|
|
double prob = 0;
|
|
|
|
// sigma grows with measurement age
|
|
const double sigma = this->sigma * (1 + age_ms / 500.0f);
|
|
|
|
// process each observed AP
|
|
for (const WiFiObservationEntry& ap : obs.entries) {
|
|
|
|
// the RSSI from the scan
|
|
const float measuredRSSI = ap.rssi;
|
|
|
|
// the RSSI from the model (if available!)
|
|
const float modelRSSI = n.getRSSI(ap.mac);
|
|
|
|
// no model RSSI available?
|
|
if (modelRSSI == 0) {continue;}
|
|
|
|
// compare both
|
|
const double p = Distribution::Normal<double>::getProbability(measuredRSSI, sigma, modelRSSI);
|
|
|
|
// adjust using log
|
|
prob += std::log(p);
|
|
|
|
}
|
|
|
|
//return std::pow(std::exp(prob), 0.1);
|
|
return std::exp(prob);
|
|
|
|
}
|
|
|
|
/** gnuplot debug dump */
|
|
template <typename Node> void dump(Grid<Node>& grid, const WiFiObservation& obs, const std::string& fileName) {
|
|
|
|
std::ofstream out(fileName);
|
|
out << "splot '-' with points palette\n";
|
|
|
|
for (const Node& n : grid) {
|
|
const float p = getProbability(n, obs);
|
|
out << n.x_cm << " " << n.y_cm << " " << n.z_cm << " " << p << "\n";
|
|
}
|
|
out << "e\n";
|
|
|
|
out.close();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // WIFIOBSERVATION_H
|