95 lines
2.6 KiB
C++
95 lines
2.6 KiB
C++
#ifndef BEACONPROBABILITYFREE_H
|
|
#define BEACONPROBABILITYFREE_H
|
|
|
|
#include "BeaconProbability.h"
|
|
#include "BeaconMeasurements.h"
|
|
#include "model/BeaconModel.h"
|
|
#include "../../math/Distributions.h"
|
|
#include "../../data/Timestamp.h"
|
|
|
|
#include "../../floorplan/v2/Floorplan.h"
|
|
|
|
#include <unordered_map>
|
|
|
|
/**
|
|
* compare BeaconMeasurements within predictions of a given model.
|
|
* predictions are just based on the distance to the observed beacon.
|
|
*/
|
|
class BeaconObserverFree : public BeaconProbability {
|
|
|
|
private:
|
|
|
|
const float sigma = 8.0f;
|
|
const float sigmaPerSecond = 3.0f;
|
|
|
|
/** the RSSI prediction model */
|
|
BeaconModel& model;
|
|
|
|
/** the map's floorplan */
|
|
Floorplan::IndoorMap* map;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
BeaconObserverFree(const float sigma, BeaconModel& model) : sigma(sigma), model(model) {
|
|
|
|
}
|
|
|
|
/** provides the probability for a specific point in space */
|
|
double getProbability(const Point3& pos_m, const Timestamp curTime, const BeaconMeasurements& obs) const {
|
|
|
|
double prob = 1.0;
|
|
int numMatchingBeacons = 0;
|
|
|
|
// process each measured AP
|
|
for (const BeaconMeasurement& entry : obs.entries) {
|
|
|
|
// sanity check
|
|
Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");
|
|
|
|
// updating the beacons sended txp if available
|
|
if(entry.getBeacon().getTXP() != 0.0f){
|
|
//TODO: check if this works
|
|
model.updateBeacon(entry);
|
|
}
|
|
|
|
// get the model's RSSI (if possible!)
|
|
const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m);
|
|
|
|
// NaN? -> AP not known to the model -> skip
|
|
if (modelRSSI != modelRSSI) {
|
|
continue;
|
|
}
|
|
|
|
|
|
// the scan's RSSI
|
|
const float scanRSSI = entry.getRSSI();
|
|
|
|
// the measurement's age
|
|
const Timestamp age = curTime - entry.getTimestamp();
|
|
|
|
Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
|
|
Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?");
|
|
|
|
// sigma grows with measurement age
|
|
const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
|
|
|
|
// update probability
|
|
prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
|
|
//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
|
|
|
|
++numMatchingBeacons;
|
|
|
|
}
|
|
|
|
// sanity check
|
|
//Assert::isTrue(numMatchingBeacons > 0, "not a single measured Beacon was matched against known ones. coding error? model error?");
|
|
|
|
return prob;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // WIFIPROBABILITYFREE_H
|