FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

merge

Browse Source
This commit is contained in:
FrankE
2017-06-06 15:03:49 +02:00
parent ba9559e5df 9ea7da557b
commit 8ed4305d2a
122 changed files with 7794 additions and 1045 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
sensors/MACAddress.h
View File

@@ -24,7 +24,7 @@ private:
} fields;
/** store as 64-bit integer */
uint64_t mac;
uint64_t mac = 0;
public:
@@ -47,15 +47,27 @@ public:
MACAddress(const std::string& str) {
// sanity check
if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");}
mac = 0; // all zeros
fields.h5 = hexWordToInt(str[ 0], str[ 1]);
fields.h4 = hexWordToInt(str[ 3], str[ 4]);
fields.h3 = hexWordToInt(str[ 6], str[ 7]);
fields.h2 = hexWordToInt(str[ 9], str[10]);
fields.h1 = hexWordToInt(str[12], str[13]);
fields.h0 = hexWordToInt(str[15], str[16]);
if (str.size() == 17 ){
mac = 0; // all zeros
fields.h5 = hexWordToInt(str[ 0], str[ 1]);
fields.h4 = hexWordToInt(str[ 3], str[ 4]);
fields.h3 = hexWordToInt(str[ 6], str[ 7]);
fields.h2 = hexWordToInt(str[ 9], str[10]);
fields.h1 = hexWordToInt(str[12], str[13]);
fields.h0 = hexWordToInt(str[15], str[16]);
}
else if (str.size() == 12){
mac = 0; // all zeros
fields.h5 = hexWordToInt(str[ 0], str[ 1]);
fields.h4 = hexWordToInt(str[ 2], str[ 3]);
fields.h3 = hexWordToInt(str[ 4], str[ 5]);
fields.h2 = hexWordToInt(str[ 6], str[ 7]);
fields.h1 = hexWordToInt(str[ 8], str[ 9]);
fields.h0 = hexWordToInt(str[10], str[11]);
}
else{
throw Exception("invalid hex string length. must be 17 or 12 (without :)");
}
}
@@ -77,7 +89,9 @@ public:
/** get the mac address as a long-int value */
uint64_t asLong() const {
return mac;
// even if we initialized all 8 bytes with zero
// we mask the 2 unsued bytes to be absolutely safe
return mac & 0x0000FFFFFFFFFFFF;
}
/** equal? */
@@ -93,13 +107,19 @@ public:
private:
/** convert the given hex char [0-F] to an integer [0-15] */
static uint8_t hexCharToInt(const char _hex) {
// to upper case
const char hex = (_hex >= 'a') ? (_hex - ('a' - 'A')) : (_hex);
static uint8_t hexCharToInt(const char hex) {
// convert
return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10);
if (hex >= '0' && hex <= '9') { // digits
return (hex - '0');
} else if (hex >= 'a' && hex <= 'f') { // lower case
return (hex - 'a' + 10);
} else if (hex >= 'A' && hex <= 'F') { // upper case
return (hex - 'A' + 10);
}
// found an invalid character
throw Exception("invalid character within MAC address");
}

20
sensors/activity/Activity.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef ACTIVITY_H
#define ACTIVITY_H
enum class Activity {
STANDING,
WALKING,
WALKING_UP,
WALKING_DOWN,
ELEVATOR_UP,
ELEVATOR_DOWN,
};
#endif // ACTIVITY_H

158
sensors/activity/ActivityDetector.h Normal file
View File

@@ -0,0 +1,158 @@
#ifndef ACTIVITYDETECTOR_H
#define ACTIVITYDETECTOR_H
#include "../imu/AccelerometerData.h"
#include "../pressure/BarometerData.h"
#include "../../data/Timestamp.h"
#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingStdDevTS.h"
#include "../../data/HistoryTS.h"
#include "../activity/Activity.h"
//#define ACT_DET_DEBUG_PLOT
#ifdef ACT_DET_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#endif
/**
* simple step detection based on accelerometer magnitude.
* magnitude > threshold? -> step!
* block for several msec until detecting the next one
*/
class ActivityDetector {
private:
MovingAverageTS<float> avgLong;
MovingAverageTS<float> avgShort;
MovingStdDevTS<float> stdDev;
MovingStdDevTS<float> stdDev2;
MovingAverageTS<float> baroAvgSlow;
MovingAverageTS<float> baroAvgFast;
MovingAverageTS<float> baroAvg;
HistoryTS<float> baroHistory;
Activity current;
public:
#ifdef ACT_DET_DEBUG_PLOT
K::Gnuplot gp;
K::GnuplotPlot gplot;
K::GnuplotPlotElementLines l1;
K::GnuplotPlotElementLines l2;
#endif
/** ctor */
ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0),
stdDev(Timestamp::fromMS(150), 0), stdDev2(Timestamp::fromMS(2000), 0),
baroAvg(Timestamp::fromMS(500), 0), baroHistory(Timestamp::fromMS(4000)) {
;
#ifdef ACT_DET_DEBUG_PLOT
gplot.add(&l1);
gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000");
#endif
}
//int xx = 0;
/** add barometer data */
void add(const Timestamp ts, const BarometerData& baro) {
if (baro.isValid()) {
baroAvg.add(ts, baro.hPa);
const float avg = baroAvg.get();
baroHistory.add(ts, avg);
//l1.add(K::GnuplotPoint2(xx, avg));
update();
}
}
/** get the currently detected activity */
Activity get() const {
return current;
}
/** does the given data indicate a step? */
void add(const Timestamp ts, const AccelerometerData& acc) {
// update averages
avgLong.add(ts, acc.magnitude());
avgShort.add(ts, acc.magnitude());
stdDev.add(ts, acc.magnitude());
stdDev2.add(ts, acc.magnitude());
// const float delta = std::abs(avgLong.get() - avgShort.get());
// static int x = 0; ++x;
// if (delta < 0.3) {
// return Activity::STANDING;
// }
// if (avgLong.get() > 9.81+0.5) {
// return Activity::WALKING_UP;
// } else if (avgLong.get() < 9.81-0.5) {
// return Activity::WALKING_DOWN;
// }
// return Activity::WALKING;
}
private:
/** estimate the current activity based on the sensor data */
void update() {
// delta in acceleration
const float delta_acc = std::abs(avgLong.get() - avgShort.get());
if (delta_acc < 0.015) {
current = Activity::STANDING;
return;
}
// delta in pressure
const float delta_hPa = baroHistory.getMostRecent() - baroHistory.getOldest();
#ifdef ACT_DET_DEBUG_PLOT
l2.add(K::GnuplotPoint2(xx, delta_hPa));
gp.draw(gplot);
gp.flush();
++xx;
#endif
if (std::abs(delta_hPa) < 0.042) {
current = Activity::WALKING;
return;
} else if (delta_hPa > 0) {
current = Activity::WALKING_DOWN;
return;
} else {
current = Activity::WALKING_UP;
return;
}
}
};
#endif // ACTIVITYDETECTOR_H

59
sensors/beacon/Beacon.h Normal file
View File

@@ -0,0 +1,59 @@
#ifndef BEACON_H
#define BEACON_H
#include "../MACAddress.h"
/**
* represents a single beacon
* a beacon is represented by its MAC-Address and
* may provide a sending power TXP
*/
class Beacon {
private:
/** the AP's MAC-Address */
MACAddress mac;
/** OPTIONAL the beacons sending power */
float txp;
public:
/** empty ctor */
Beacon() {
;
}
/** ctor with MAC and TXP */
Beacon(const MACAddress& mac, const float& txp) : mac(mac), txp(txp) {
;
}
/** ctor with MAC and TXP */
Beacon(const std::string& mac, const float& txp) : mac(mac), txp(txp) {
;
}
/** ctor with MAC and without TXP */
Beacon(const MACAddress& mac) : mac(mac), txp() {
;
}
/** ctor with MAC and without TXP */
Beacon(const std::string& mac) : mac(mac), txp() {
;
}
public:
/** get the AP's MAC address */
inline const MACAddress& getMAC() const {return mac;}
/** OPTIONAL: get the AP's ssid (if any) */
inline const float& getTXP() const {return txp;}
};
#endif // BEACON_H

44
sensors/beacon/BeaconMeasurement.h Normal file
View File

@@ -0,0 +1,44 @@
#ifndef BEACONMEASUREMENT_H
#define BEACONMEASUREMENT_H
#include "../MACAddress.h"
#include "../../data/Timestamp.h"
#include "Beacon.h"
#include <vector>
/** one observed AP and its signal strength */
class BeaconMeasurement {
private:
/** the timestamp this beacon was discovered at */
Timestamp ts;
/** the beacon's mac address */
Beacon beacon;
/** signal strength */
float rssi;
public:
/** ctor */
BeaconMeasurement(const Timestamp ts, const Beacon& beacon, const float rssi) : ts(ts), beacon(beacon), rssi(rssi) {;}
public:
/** get the beacon */
const Beacon& getBeacon() const {return beacon;}
/** get the measurements timestamp */
const Timestamp& getTimestamp() const {return ts;}
/** get the rssi */
float getRSSI() const {return rssi;}
};
#endif // BEACONMEASUREMENT_H

26
sensors/beacon/BeaconMeasurements.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef BEACONMEASUREMENTS_H
#define BEACONMEASUREMENTS_H
#include <vector>
#include "BeaconMeasurement.h"
/**
* group of several beacon measurements
*/
struct BeaconMeasurements {
std::vector<BeaconMeasurement> entries;
/** remove entries older then 3000 ms*/
void removeOld(const Timestamp latestTS) {
auto lambda = [latestTS] (const BeaconMeasurement& e) {
Timestamp age = latestTS - e.getTimestamp();
return age > Timestamp::fromMS(1000*3);
};
entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end());
}
};
#endif // BEACONMEASUREMENTS_H

15
sensors/beacon/BeaconProbability.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef BEACONPROBABILITY_H
#define BEACONPROBABILITY_H
#include "BeaconMeasurements.h"
/**
* base class for all Beacon probability calculators.
* such a calculator determines the probabilty for a location (e.g. x,y,z)
* given BeaconMeasurements
*/
class BeaconProbability {
};
#endif // BEACONPROBABILITY_H

94
sensors/beacon/BeaconProbabilityFree.h Normal file
View File

@@ -0,0 +1,94 @@
#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

46
sensors/beacon/model/BeaconModel.h Normal file
View File

@@ -0,0 +1,46 @@
#ifndef BEACONMODEL_H
#define BEACONMODEL_H
#include "../Beacon.h"
#include "../BeaconMeasurement.h"
#include "../../../geo/Point3.h"
#include <vector>
/**
* interface for signal-strength prediction models.
*
* the model is passed a MAC-address of an AP in question, and a position.
* hereafter the model returns the RSSI for this AP at the questioned location.
*/
class BeaconModel {
public:
// /** get the given access-point's RSSI at the provided location */
// virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
/** get a list of all APs known to the model */
virtual std::vector<Beacon> getAllBeacons() const = 0;
/**
* update the beacons signal strength using the current measurement
* this could happen if the txp is not updated within the floorplan
*
* be careful and don't use fantasy values, this could ruin your localitions
* completely
*/
virtual void updateBeacon(const BeaconMeasurement beacon) = 0;
/**
* get the RSSI expected at the given location (in meter)
* for an beacon identified by the given MAC.
*
* if the model can not predict the RSSI for an beacon, it returns NaN!
*/
virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0;
};
#endif // BEACONMODEL_H

92
sensors/beacon/model/BeaconModelLogDist.h Normal file
View File

@@ -0,0 +1,92 @@
#ifndef BEACONMODELLOGDIST_H
#define BEACONMODELLOGDIST_H
#include "BeaconModel.h"
#include "../../radio/model/LogDistanceModel.h"
#include <unordered_map>
/**
* signal-strength estimation using log-distance model
*/
class BeaconModelLogDist : public BeaconModel {
public:
/** parameters describing one beacons to the model */
struct APEntry {
Point3 position_m; // the AP's position (in meter)
float txp; // sending power (-40)
float exp; // path-loss-exponent (~2.0 - 4.0)
/** ctor */
APEntry(const Point3 position_m, const float txp, const float exp) :
position_m(position_m), txp(txp), exp(exp) {;}
};
private:
/** map of all beacons (and their parameters) known to the model */
std::unordered_map<MACAddress, APEntry> beacons;
public:
/** ctor */
BeaconModelLogDist() {
;
}
/** get a list of all beacons known to the model */
std::vector<Beacon> getAllBeacons() const {
std::vector<Beacon> aps;
for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
return aps;
}
/** make the given beacon (and its parameters) known to the model */
void addAP(const MACAddress& beacon, const APEntry& params) {
// sanity check
Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-90:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
// add
beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
}
void updateBeacon(const BeaconMeasurement beacon) override{
// try to get the corresponding parameters
const auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
// beacon unknown? -> NAN
if (it == beacons.end()) {return;}
it->second.txp = beacon.getBeacon().getTXP();
}
virtual float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
// try to get the corresponding parameters
const auto it = beacons.find(beacon);
// AP unknown? -> NAN
if (it == beacons.end()) {return NAN;}
// the beacons' parameters
const APEntry& params = it->second;
// free-space (line-of-sight) RSSI
const float distance_m = position_m.getDistance(params.position_m);
const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
// done
return rssiLOS;
}
};
#endif // BEACONMODELLOGDIST_H

208
sensors/beacon/model/BeaconModelLogDistCeiling.h Normal file
View File

@@ -0,0 +1,208 @@
#ifndef BEACONMODELLOGDISTCEILING_H
#define BEACONMODELLOGDISTCEILING_H
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../Assertions.h"
#include "BeaconModel.h"
#include "../../radio/model/LogDistanceModel.h"
#include "../BeaconMeasurement.h"
#include <unordered_map>
/**
* signal-strength estimation using log-distance model
* including ceilings between beacon and position
*/
class BeaconModelLogDistCeiling : public BeaconModel {
public:
/** parameters describing one beacon to the model */
struct APEntry {
Point3 position_m; // the beacon's position (in meter)
float txp; // sending power (-40)
float exp; // path-loss-exponent (~2.0 - 4.0)
float waf; // attenuation per ceiling/floor (~-8.0)
/** ctor */
APEntry(const Point3 position_m, const float txp, const float exp, const float waf) :
position_m(position_m), txp(txp), exp(exp), waf(waf) {;}
};
private:
/** map of all beacons (and their parameters) known to the model */
std::unordered_map<MACAddress, APEntry> beacons;
/** position (height) of all ceilings (in meter) */
std::vector<float> ceilingsAtHeight_m;
public:
/** ctor with floorplan (needed for ceiling position) */
BeaconModelLogDistCeiling(const Floorplan::IndoorMap* map) {
// sanity checks
Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
// position of all ceilings
for (Floorplan::Floor* f : map->floors) {
ceilingsAtHeight_m.push_back(f->atHeight);
}
}
/** get a list of all beacons known to the model */
std::vector<Beacon> getAllBeacons() const {
std::vector<Beacon> aps;
for (const auto it : beacons) {aps.push_back(Beacon(it.first));}
return aps;
}
/** load beacon information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadBeaconsFromMap(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::Beacon* beacon : floor->beacons) {
APEntry ape(beacon->getPos(floor), txp, exp, waf);
addBeacon(MACAddress(beacon->mac), ape);
}
}
}
/** load beacon information from a vector. use the given fixed TXP/EXP/WAF for all APs */
void loadBeaconsFromVector(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::Beacon* beacon : floor->beacons) {
APEntry ape(beacon->getPos(floor), txp, exp, waf);
addBeacon(MACAddress(beacon->mac), ape);
}
}
}
/** make the given beacon (and its parameters) known to the model */
void addBeacon(const MACAddress& beacon, const APEntry& params) {
// sanity check
Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
Assert::isBetween(params.txp, -90.0f, -30.0f, "TXP out of bounds [-50:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
//Assert::equal(beacons.find(beacon), beacons.end(), "AccessPoint already present!");
// add
beacons.insert( std::pair<MACAddress, APEntry>(beacon, params) );
}
void updateBeacon(const BeaconMeasurement beacon) override {
// try to get the corresponding parameters
auto it = beacons.find(MACAddress(beacon.getBeacon().getMAC()));
// beacon unknown? -> NAN
if (it == beacons.end()) {return;}
// TODO: Check if this works as expected
it->second.txp = beacon.getBeacon().getTXP();
}
/** remove all added APs */
void clear() {
beacons.clear();
}
float getRSSI(const MACAddress& beacon, const Point3 position_m) const override {
// try to get the corresponding parameters
const auto it = beacons.find(beacon);
// beacon unknown? -> NAN
if (it == beacons.end()) {return NAN;}
// the access-points' parameters
const APEntry& params = it->second;
// free-space (line-of-sight) RSSI
const float distance_m = position_m.getDistance(params.position_m);
const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
// WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value
const float wafLoss = params.waf * numCeilingsBetween(position_m, params.position_m);
// combine
return rssiLOS + wafLoss;
}
protected:
FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilings);
FRIEND_TEST(LogDistanceCeilingModelBeacon, numCeilingsFloat);
/** get the number of ceilings between z1 and z2 */
float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
float cnt = 0;
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {
const float dmax = zMax - z;
cnt += (dmax > 1) ? (1) : (dmax);
}
}
return cnt;
}
/** get the number of ceilings between z1 and z2 */
int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
int cnt = 0;
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
#ifdef WITH_ASSERTIONS
static int numNear = 0;
static int numFar = 0;
for (const float z : ceilingsAtHeight_m) {
const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
if (diff < 0.1) {++numNear;} else {++numFar;}
}
if ((numNear + numFar) > 150000) {
Assert::isTrue(numNear < numFar*0.1,
"many requests to the WiFiModel address nodes (very) near to a ground! \
due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
expect very wrong outputs! \
consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
);
}
#endif
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {++cnt;}
}
return cnt;
}
};
#endif // WIFIMODELLOGDISTCEILING_H

30
sensors/gps/GPSData.h
View File

@@ -2,6 +2,7 @@
#define GPSDATA_H
#include "../../data/Timestamp.h"
#include "../../geo/EarthPos.h"
struct GPSData {
@@ -15,12 +16,41 @@ struct GPSData {
float accuracy; // m [might be NAN]
float speed; // m/s [might be NAN]
/** ctor for invalid/unknown data */
GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
/** ctor */
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(NAN), speed(NAN) {;}
/** ctor */
GPSData(const Timestamp tsReceived, const float lat, const float lon, const float alt, const float accuracy) : tsReceived(tsReceived), lat(lat), lon(lon), alt(alt), accuracy(accuracy), speed(NAN) {;}
/** get as EarthPos struct */
EarthPos toEarthPos() const {
return EarthPos(lat, lon, alt);
}
/** data valid? */
bool isValid() const {
return (lat == lat) && (lon == lon);
}
bool operator == (const GPSData& o) const {
return EQ_OR_NAN(lat, o.lat) &&
EQ_OR_NAN(lon, o.lon) &&
EQ_OR_NAN(alt, o.alt) &&
EQ_OR_NAN(accuracy, o.accuracy) &&
EQ_OR_NAN(speed, o.speed);
}
std::string asString() const {
return "(lat: " + std::to_string(lat) + ", lon: " + std::to_string(lon) + ", alt: " + std::to_string(alt) + " acur: " + std::to_string(accuracy) + ")";
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // GPSDATA_H

65
sensors/gps/GPSProbability.h Normal file
View File

@@ -0,0 +1,65 @@
#ifndef GPSPROBABILITY_H
#define GPSPROBABILITY_H
#include "GPSData.h"
#include "../../geo/Point3.h"
#include "../../math/distribution/Region.h"
#include "../../geo/EarthMapping.h"
class GPSProbability {
private:
/** convert between map and earth */
const EarthMapping& em;
public:
/** ctor with the map<->earth translator */
GPSProbability(const EarthMapping& em) : em(em) {
;
}
/** get the probability for residing at pos_m [in meter, map-coordinates] given some GPS measurement */
double getProbability(const Point3 pos_m, const GPSData& d) const {
// pad GPS? -> no gps eval
if (isBad(d)) {return 1.0;}
// adjust accuracy [sometimes strange values are provided here!]
float accuracy = d.accuracy;
if (accuracy < 3.0) {
std::cout << "note: adjusting gps accuracy as '" << accuracy << "'' seems invalid" << std::endl;
accuracy = 3.0;
}
// convert GPS to map coordinats
const Point3 gpsToMap_m = em.worldToMap(d.toEarthPos());
// distance between given point and GPS's estimation
const float dist = pos_m.getDistance(gpsToMap_m);
// calculate probability
//const double prob = Distribution::Region<double>::getProbability(0, d.accuracy, dist);
const double prob = Distribution::Normal<double>::getProbability(0, accuracy, dist);
// sanity checks
Assert::isNot0(prob, "gps probability is 0.0");
Assert::isNotNaN(prob, "gps probability is NaN");
// done
return prob;
}
private:
/** returns true if the given GPS reading is bad [inaccurate, invalid, ...] */
static inline bool isBad(const GPSData& d) {
return (!d.isValid()) || (d.accuracy == 0) || (d.accuracy > 25);
}
};
#endif // GPSPROBABILITY_H

24
sensors/imu/AccelerometerData.h
View File

@@ -42,11 +42,25 @@ struct AccelerometerData {
return AccelerometerData(x/val, y/val, z/val);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const AccelerometerData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};

56
sensors/imu/CompassData.h Normal file
View File

@@ -0,0 +1,56 @@
#ifndef COMPASSDATA_H
#define COMPASSDATA_H
#include <cmath>
#include <sstream>
/** data received from a compass sensor */
struct CompassData {
/** azimuth angle. NAN if not available */
float azimuth = NAN;
/** describes the sensor's quality */
float quality01 = 0;
/** empty ctor */
CompassData() : azimuth(NAN) {;}
/** data ctor */
CompassData(const float azimuth) : azimuth(azimuth), quality01(0) {;}
/** data ctor */
CompassData(const float azimuth, const float quality01) : azimuth(azimuth), quality01(quality01) {;}
/** get an instance describing invalid data */
static CompassData INVALID() {
return CompassData(NAN);
}
/** convert to string */
std::string asString() const {
std::stringstream ss;
ss << "(" << azimuth << ")";
return ss.str();
}
/** is the compass data valid? [compass present] */
bool isValid() const {
return azimuth == azimuth;
}
bool operator == (const CompassData& o) const {
return EQ_OR_NAN(azimuth, o.azimuth) &&
EQ_OR_NAN(quality01, o.quality01);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // COMPASSDATA_H

67
sensors/imu/GravityData.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef GRAVITYDATA_H
#define GRAVITYDATA_H
#include <cmath>
#include <sstream>
/** data received from an accelerometer sensor */
struct GravityData {
float x;
float y;
float z;
GravityData() : x(0), y(0), z(0) {;}
GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
float magnitude() const {
return std::sqrt( x*x + y*y + z*z );
}
GravityData& operator += (const GravityData& o) {
this->x += o.x;
this->y += o.y;
this->z += o.z;
return *this;
}
GravityData& operator -= (const GravityData& o) {
this->x -= o.x;
this->y -= o.y;
this->z -= o.z;
return *this;
}
GravityData operator - (const GravityData& o) const {
return GravityData(x-o.x, y-o.y, z-o.z);
}
GravityData operator / (const float val) const {
return GravityData(x/val, y/val, z/val);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const GravityData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // GRAVITYDATA_H

24
sensors/imu/GyroscopeData.h
View File

@@ -23,11 +23,25 @@ struct GyroscopeData {
return std::sqrt( x*x + y*y + z*z );
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const GyroscopeData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};

67
sensors/imu/LinearAccelerationData.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef LINEARACCELERATIONDATA_H
#define LINEARACCELERATIONDATA_H
#include <cmath>
#include <sstream>
/** data received from an accelerometer sensor */
struct LinearAccelerationData {
float x;
float y;
float z;
LinearAccelerationData() : x(0), y(0), z(0) {;}
LinearAccelerationData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
float magnitude() const {
return std::sqrt( x*x + y*y + z*z );
}
LinearAccelerationData& operator += (const LinearAccelerationData& o) {
this->x += o.x;
this->y += o.y;
this->z += o.z;
return *this;
}
LinearAccelerationData& operator -= (const LinearAccelerationData& o) {
this->x -= o.x;
this->y -= o.y;
this->z -= o.z;
return *this;
}
LinearAccelerationData operator - (const LinearAccelerationData& o) const {
return LinearAccelerationData(x-o.x, y-o.y, z-o.z);
}
LinearAccelerationData operator / (const float val) const {
return LinearAccelerationData(x/val, y/val, z/val);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
bool isValid() const {
return (x == x) && (y == y) && (z == z);
}
bool operator == (const LinearAccelerationData& o ) const {
return EQ_OR_NAN(x, o.x) &&
EQ_OR_NAN(y, o.y) &&
EQ_OR_NAN(z, o.z);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // LINEARACCELERATIONDATA_H

163
sensors/imu/MotionDetection.h Normal file
View File

@@ -0,0 +1,163 @@
#ifndef MOTIONDETECTION_H
#define MOTIONDETECTION_H
#include "GravityData.h"
#include "LinearAccelerationData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../misc/Debug.h"
#include <eigen3/Eigen/Dense>
#include <cmath>
#include <vector>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include "../../Assertions.h"
class MotionDetection {
private:
bool newAccelerationMeasurementArrived = false;
bool newGravityMeasurementArrived = false;
Eigen::Vector3f curGravity;
Eigen::Vector3f curLinearAcceleration;
//fast algo
Eigen::Matrix2f sumProjectedCov = Eigen::Matrix2f::Identity(); //sum of the projection of curLinearAcceleartion into perpendicular plane of curGravity as semmetric matrix
int numMeasurementsPerInterval, updateCnt;
int updateInterval; //defines how often a new motion axis is calculated in milliseconds. default = 500ms
struct Motion{
Eigen::Vector2f vec = Eigen::Vector2f::Identity();
Timestamp lastEstimation;
};
Motion curMotion;
Motion prevMotion;
const char* name = "MotionDetection";
public:
/** ctor */
MotionDetection(int updateInterval = 500) : updateInterval(updateInterval), numMeasurementsPerInterval(0), updateCnt(0) {
;
}
void addGravity(const Timestamp& ts, const GravityData& grav){
curGravity << grav.x, grav.y, grav.z;
newGravityMeasurementArrived = true;
updateProjectionVectorFast(ts);
}
void addLinearAcceleration(const Timestamp& ts, const LinearAccelerationData& acc) {
curLinearAcceleration << acc.x, acc.y, acc.z;
newAccelerationMeasurementArrived = true;
updateProjectionVectorFast(ts);
}
/** return the current motion axis
* NOTE: if no data is available, this vector is the Identity
*/
Eigen::Vector2f getCurrentMotionAxis(){
return curMotion.vec;
}
/** returns the radians between [-pi, pi] between successive motion axis estimations */
float getMotionChangeInRad(){
//TODO: put this in an EigenHelper Class within geo
const float crossProduct = curMotion.vec.x() * prevMotion.vec.y() - curMotion.vec.y() * prevMotion.vec.x();
const float ang = (crossProduct < 0 ? 1:-1) * std::acos(std::min(std::max(curMotion.vec.dot(prevMotion.vec) / curMotion.vec.norm() * prevMotion.vec.norm(), -1.0f), 1.0f));
//nan?
if(std::isnan(ang)){
Log::add(name, "The motion change angle is nAn, this is not correct!");
}
if(updateCnt < 2){
return 0;
}
return ang;
}
private:
FRIEND_TEST(MotionDetection, motionAngle);
FRIEND_TEST(MotionDetection, motionAxis);
Eigen::Vector2f updateMotionAxis(Eigen::Matrix2f covarianceMatrix){
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver(covarianceMatrix);
return solver.eigenvectors().col(1); //returns the eigenvector corresponding to the biggest eigenvalue
}
void updateProjectionVectorFast(const Timestamp& ts){
//make sure we have both measurements for calculation
if(newGravityMeasurementArrived && newAccelerationMeasurementArrived){
numMeasurementsPerInterval++;
//project acc into perpendicular plane of grav (using standard vector projection)
Eigen::Vector3f proj = (curLinearAcceleration.dot(curGravity) / curGravity.dot(curGravity)) * curGravity;
//if the acc vector is perpendicular to the gravity vector, the dot product results in 0, therefore, we need to do this
if(proj.isZero()){
proj = curLinearAcceleration;
Log::add(name, "The LinearAcceleration vector is perpendicular to the gravity, is this correct?");
}
//we are only interested in x,y
Eigen::Vector2f vec;
vec << proj.x(), proj.y();
// sum this up for later averaging.
sumProjectedCov += vec*vec.transpose();
// start with the first available timestamp
if (curMotion.lastEstimation.isZero()) {curMotion.lastEstimation = ts;}
//update the motion axis depending on the update interval
if(ts - curMotion.lastEstimation > Timestamp::fromMS(updateInterval)){
prevMotion = curMotion;
//calculate the average of the coveriance matrix
Eigen::Matrix2f Q = sumProjectedCov / numMeasurementsPerInterval;
curMotion.vec = updateMotionAxis(Q);
curMotion.lastEstimation = ts;
reset();
}
newGravityMeasurementArrived = false;
newAccelerationMeasurementArrived = false;
}
//do nothing
}
void reset(){
numMeasurementsPerInterval = 0;
sumProjectedCov = Eigen::Matrix2f::Zero();
++updateCnt;
}
};
#endif // MOTIONDETECTION_H

123
sensors/offline/FilePlayer.h Normal file
View File

@@ -0,0 +1,123 @@
#ifndef FILEPLAYER_H
#define FILEPLAYER_H
#include "FileReader.h"
#include <thread>
namespace Offline {
/**
* this class can be used to "play" previously recorded [so-called "offline"] files.
* one can attach itself as listener and is informed whenever new sensor data is available.
* one may choose whether to use full-speed playback [as many events as possible] or
* live-speed playback with the same timing the values were recorded with
*/
class FilePlayer {
private:
FileReader* reader;
bool realtime = false;
bool enabled;
std::thread thread;
/** the listener to inform */
Listener* listener = nullptr;
public:
/** empty ctor */
FilePlayer() : reader(nullptr), listener(nullptr) {
;
}
/** ctor */
FilePlayer(FileReader* reader, Listener* l) : reader(reader), listener(l) {
;
}
/** whether to use realtime playback or "as fast as possible" */
void setRealtime(const bool rt) {
this->realtime = rt;
}
/** set the offline-file-reader to use as data source */
void setReader(FileReader* r) {
this->reader = r;
}
/** set the event listener to inform */
void setListener(Listener* l) {
this->listener = l;
}
/** start playback */
void start() {
// sanity check
Assert::isNotNull(reader, "call FilePlayer::setReader() first");
Assert::isNotNull(listener, "call FilePlayer::setListener() first");
Assert::isFalse(reader->getEntries().empty(), "FileReader has no loaded entries for playback within the FilePlayer!");
enabled = true;
thread = std::thread(&FilePlayer::loop, this);
}
/** stop playback */
void stop() {
enabled = false;
}
/** wait for termination */
void join() {
thread.join();
}
private:
/** background loop */
void loop() {
// get all sensor events from the offline file
const std::vector<Entry> events = reader->getEntries();
// process every event
for (const Entry& e : events) {
// aborted?
if (!enabled) {break;}
// timestamp
const Timestamp ts = Timestamp::fromMS(e.ts);
// event index
const size_t idx = e.idx;
#warning "some sensors todo:"
switch(e.type) {
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break;
case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
default: throw Exception("code error. found not-yet-implemented sensor");
}
}
// done
enabled = false;
}
};
}
#endif // FILEPLAYER_H

414
sensors/offline/FileReader.h Normal file
View File

@@ -0,0 +1,414 @@
#ifndef FILEREADER_H
#define FILEREADER_H
#include <fstream>
#include <Indoor/Exception.h>
#include <vector>
#include <unordered_map>
#include "../../math/Interpolator.h"
#include "../../sensors/radio/WiFiMeasurements.h"
#include "../../sensors/pressure/BarometerData.h"
#include "../../sensors/imu/AccelerometerData.h"
#include "../../sensors/imu/GyroscopeData.h"
#include "../../sensors/imu/GravityData.h"
#include "../../sensors/imu/LinearAccelerationData.h"
#include "../../sensors/beacon/BeaconMeasurements.h"
#include "../../sensors/gps/GPSData.h"
#include "../../sensors/imu/CompassData.h"
#include "../../geo/Point2.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/Importance.h"
#include "../../floorplan/v2/Floorplan.h"
#include "../../floorplan/v2/FloorplanHelper.h"
#include "Splitter.h"
#include "Sensors.h"
#include "Listener.h"
#warning "adjust to to use the new splitter for all parsers [gps, compass, etc. already do!]"
namespace Offline {
/**
* read and parse previously recorded ["offline"] files
*/
class FileReader {
public:
using GroundTruth = Interpolator<Timestamp, Point3>;
/** all entries grouped by sensor */
std::vector<TS<int>> groundTruth;
std::vector<TS<WiFiMeasurements>> wifi;
std::vector<TS<BeaconMeasurement>> beacon;
std::vector<TS<AccelerometerData>> acc;
std::vector<TS<GyroscopeData>> gyro;
std::vector<TS<BarometerData>> barometer;
std::vector<TS<LinearAccelerationData>> lin_acc;
std::vector<TS<GravityData>> gravity;
std::vector<TS<GPSData>> gps;
std::vector<TS<CompassData>> compass;
/** all entries in linear order as they appeared while recording */
std::vector<Entry> entries;
static constexpr char sep = ';';
public:
/** empty ctor. call open() */
FileReader() {
;
}
/** ctor with filename */
FileReader(const std::string& file) {
open(file);
}
/** open the given file */
void open(const std::string& file) {
clear();
parse(file);
}
/** remove all parsed entries */
void clear() {
entries.clear();
groundTruth.clear();
wifi.clear();
beacon.clear();
acc.clear();
gyro.clear();
gps.clear();
compass.clear();
barometer.clear();
lin_acc.clear();
gravity.clear();
}
const std::vector<Entry>& getEntries() const {return entries;}
const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
const std::vector<TS<GPSData>>& getGPS() const {return gps;}
const std::vector<TS<CompassData>>& getCompass() const {return compass;}
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
const std::vector<TS<LinearAccelerationData>>& getLinearAcceleration() const {return lin_acc;}
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
/** get an interpolateable ground-truth based on the time-clicks during recording */
GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
// sanity check: given path [indices to ground-truth points within the map]
// must have the same size as the number of clicks during recording
Assert::equal(groundTruthPoints.size(), groundTruth.size(), "mismatch of ground-truth points between given path and recording");
// allows getting a position on the ground-truth given a timestamp
GroundTruth interpol;
// all ground-truth points within the map
static std::unordered_map<int, Point3> gt = FloorplanHelper::getGroundTruthPoints(map);
// process each "tap smartphone when reaching ground-truth-point"
for (const TS<int>& entry : groundTruth) {
const Timestamp ts = Timestamp::fromMS(entry.ts);
const int idx = entry.data; // starting at 0, incrementing over time [1st point, 2nd points, 3d points, ...]
const int id = groundTruthPoints[idx]; // convert point number to point-id within floorplan
const auto& it = gt.find(id);
if (it == gt.end()) {throw Exception("missing ground-truth point ID:" + std::to_string(id));}
const Point3 pos = it->second;
interpol.add(ts, pos);
}
// done
return interpol;
}
private:
void parse(const std::string& file) {
std::ifstream inp(file);
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
while(!inp.eof() && !inp.bad()) {
uint64_t ts;
char delim;
int idx = -1;
std::string data;
inp >> ts;
inp >> delim;
inp >> idx;
inp >> delim;
inp >> data;
if (idx == (int)Sensor::WIFI) {parseWiFi(ts, data);}
else if (idx == (int)Sensor::BEACON) {parseBeacons(ts, data);}
else if (idx == (int)Sensor::GROUND_TRUTH) {parseGroundTruth(ts, data);}
else if (idx == (int)Sensor::ACC) {parseAccelerometer(ts, data);}
else if (idx == (int)Sensor::GYRO) {parseGyroscope(ts, data);}
else if (idx == (int)Sensor::BARO) {parseBarometer(ts, data);}
else if (idx == (int)Sensor::LIN_ACC) {parseLinearAcceleration(ts,data);}
else if (idx == (int)Sensor::GRAVITY) {parseGravity(ts,data);}
else if (idx == (int)Sensor::COMPASS) {parseCompass(ts,data);}
else if (idx == (int)Sensor::GPS) {parseGPS(ts,data);}
// TODO: this is a hack...
// the loop is called one additional time after the last entry
// and keeps the entries of entry
}
inp.close();
}
void parseLinearAcceleration(const uint64_t ts, const std::string& data){
const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<LinearAccelerationData> elem(ts, LinearAccelerationData(std::stof(x), std::stof(y), std::stof(z)));
lin_acc.push_back(elem);
entries.push_back(Entry(Sensor::LIN_ACC, ts, lin_acc.size()-1));
}
void parseGravity(const uint64_t ts, const std::string& data){
GravityData gravData;
const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
gravData.x = std::stof(data.substr(0, pos1));
gravData.y = std::stof(data.substr(pos1+1, pos2-pos1-1));
gravData.z = std::stof(data.substr(pos2+1));
TS<GravityData> elem(ts, gravData);
gravity.push_back(elem);
entries.push_back(Entry(Sensor::GRAVITY, ts, gravity.size()-1));
// inform listener
//if (listener) {listener->onGravity(Timestamp::fromMS(ts), gravData);}
}
void parseAccelerometer(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
const AccelerometerData accData(std::stof(x), std::stof(y), std::stof(z));
TS<AccelerometerData> elem(ts, accData);
acc.push_back(elem);
entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
// inform listener
//if (listener) {listener->onAccelerometer(Timestamp::fromMS(ts), accData);}
}
void parseGyroscope(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
const GyroscopeData gyroData(std::stof(x), std::stof(y), std::stof(z));
TS<GyroscopeData> elem(ts, gyroData);
gyro.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
// inform listener
//if (listener) {listener->onGyroscope(Timestamp::fromMS(ts), gyroData);}
}
void parseWiFi(const uint64_t ts, const std::string& data) {
WiFiMeasurements wifi;
Splitter s(data, sep);
// the -1 is due to some old files containing a trailing ";" resulting in one additional stray column
for (size_t i = 0; i < s.size()-1; i += 3) {
const std::string mac = s.get(i+0);
const float freq = s.getFloat(i+1);
const float rssi = s.getFloat(i+2);
// append AP to current scan-entry
WiFiMeasurement e(AccessPoint(mac), rssi, freq, Timestamp::fromMS(ts));
wifi.entries.push_back(e);
}
// add new wifi reading
this->wifi.push_back(TS<WiFiMeasurements>(ts, wifi));
entries.push_back(Entry(Sensor::WIFI, ts, this->wifi.size()-1));
// inform listener
//if (listener) {listener->onWiFi(Timestamp::fromMS(ts), wifi);}
}
void parseBeacons(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';');
const auto pos2 = data.find(';', pos1+1);
const auto pos3 = data.find(';', pos2+1);
const std::string mac = data.substr(0, pos1);
const std::string rssi = data.substr(pos1+1, pos2);
const std::string txp = data.substr(pos2+1, pos3);
//yes the timestamp is redundant here, but in case of multiusage...
TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
beacon.push_back(e);
entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
}
void parseGroundTruth(const uint64_t ts, const std::string& data) {
const auto pos1 = data.find(';');
std::string gtIndex = data.substr(0, pos1);
TS<int> elem(ts, std::stoi(gtIndex));
groundTruth.push_back(elem);
}
void parseBarometer(const uint64_t ts, const std::string& data) {
BarometerData baro;
Splitter s(data, sep);
baro.hPa = s.has(0) ? (s.getFloat(0)) : (NAN);
TS<BarometerData> elem(ts, baro);
barometer.push_back(elem);
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
// inform listener
//if (listener) {listener->onBarometer(Timestamp::fromMS(ts), baro);}
}
void parseCompass(const uint64_t ts, const std::string& data) {
CompassData compass;
Splitter s(data, sep);
compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
TS<CompassData> elem(ts, compass);
this->compass.push_back(elem);
entries.push_back(Entry(Sensor::COMPASS, ts, this->compass.size()-1));
// inform listener
//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
}
/** parse the given GPS entry */
void parseGPS(const uint64_t ts, const std::string& data) {
GPSData gps;
Splitter s(data, sep);
gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
TS<GPSData> elem(ts, gps);
this->gps.push_back(elem);
entries.push_back(Entry(Sensor::GPS, ts, this->gps.size()-1));
// inform listener
//if (listener) {listener->onGPS(Timestamp::fromMS(ts), gps);}
}
public:
const Interpolator<uint64_t, Point3> getGroundTruthPath(Floorplan::IndoorMap* map, std::vector<int> gtPath) const {
// finde alle positionen der waypoints im gtPath aus map
std::unordered_map<int, Point3> waypointsMap;
for(Floorplan::Floor* f : map->floors){
float h = f->atHeight;
for (Floorplan::GroundTruthPoint* gtp : f->gtpoints){
//wenn die gleiche id 2x vergeben wurde, knallt es
if(waypointsMap.find(gtp->id) == waypointsMap.end()){
waypointsMap.insert({gtp->id, Point3(gtp->pos.x,gtp->pos.y, h)});
}
else{
throw std::string("the floorplan's ground truth contains two points with identical id's!");
}
}
}
// bringe diese in richtige reihenfolge und füge timestamp hinzu
Interpolator<uint64_t, Point3> interpol;
int it = 0;
for(int id : gtPath){
auto itMap = waypointsMap.find(id);
if(itMap == waypointsMap.end()) {throw std::string("waypoint not found in xml");}
//the time, when the gt button was clicked on the app
uint64_t tsGT = groundTruth[it++].ts;
interpol.add(tsGT, itMap->second);
}
if(gtPath.empty() || waypointsMap.empty() || groundTruth.empty()){
throw std::string("No Ground Truth points found within the map.xml file");
}
return interpol;
}
};
}
#endif // FILEREADER_H

92
sensors/offline/FileWriter.h Normal file
View File

@@ -0,0 +1,92 @@
#ifndef FILEWRITER_H
#define FILEWRITER_H
#include "../gps/GPSData.h"
#include "../imu/CompassData.h"
#include "../imu/LinearAccelerationData.h"
#include "../imu/GravityData.h"
#include "../pressure/BarometerData.h"
#include "../imu/GyroscopeData.h"
#include "../imu/AccelerometerData.h"
#include "../radio/WiFiMeasurements.h"
#include "Sensors.h"
#include <fstream>
namespace Offline {
class FileWriter {
private:
std::ofstream out;
static constexpr char sep = ';';
const std::string nl = "\n";
public:
FileWriter() {
;
}
~FileWriter() {
close();
}
void open(const std::string& file) {
out.open(file);
if (!out) {throw Exception("error opening file: " + file);}
}
void close() {
out.flush();
out.close();
}
void flush() {
out.flush();
}
void add(const Timestamp ts, const AccelerometerData& data) {
out << ts.ms() << sep << (int) Sensor::ACC << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const LinearAccelerationData& data) {
out << ts.ms() << sep << (int) Sensor::LIN_ACC << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const GravityData& data) {
out << ts.ms() << sep << (int) Sensor::GRAVITY << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const GyroscopeData& data) {
out << ts.ms() << sep << (int) Sensor::GYRO << sep << data.x << sep << data.y << sep << data.z << nl;
}
void add(const Timestamp ts, const BarometerData& data) {
out << ts.ms() << sep << (int) Sensor::BARO << sep << data.hPa << nl;
}
void add(const Timestamp ts, const GPSData& data) {
out << ts.ms() << sep << (int) Sensor::GPS << sep << data.lat << sep << data.lon << sep << data.alt << sep << data.accuracy << sep << data.speed << nl;
}
void add(const Timestamp ts, const CompassData& data) {
out << ts.ms() << sep << (int) Sensor::COMPASS << sep << data.azimuth << sep << data.quality01 << nl;
}
void add(const Timestamp ts, const WiFiMeasurements& data) {
out << ts.ms() << sep << (int) Sensor::WIFI;
for (const WiFiMeasurement& m : data.entries) {
out << sep << m.getAP().getMAC().asString();
out << sep << m.getFrequency();
out << sep << m.getRSSI();
}
out << "\n";
}
};
}
#endif // FILEWRITER_H

33
sensors/offline/Listener.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef OFFLINE_LISTENER_H
#define OFFLINE_LISTENER_H
#include "../gps/GPSData.h"
#include "../imu/CompassData.h"
#include "../imu/GravityData.h"
#include "../pressure/BarometerData.h"
#include "../imu/GyroscopeData.h"
#include "../imu/AccelerometerData.h"
#include "../radio/WiFiMeasurements.h"
namespace Offline {
/**
* listen for events/callbacks while parsing offline files
*/
class Listener {
public:
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0;
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onGravity(const Timestamp ts, const GravityData data) = 0;
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
};
}
#endif // OFFLINE_LISTENER_H

117
sensors/offline/OfflineAndroid.h
View File

@@ -12,8 +12,14 @@
#include "../radio/WiFiMeasurements.h"
#include "../imu/AccelerometerData.h"
#include "../imu/GyroscopeData.h"
#include "../imu/CompassData.h"
#include "../gps/GPSData.h"
#include "../pressure/BarometerData.h"
#include "Splitter.h"
#include "Listener.h"
#include "Sensors.h"
template <typename SensorData> struct OfflineEntry {
Timestamp ts;
@@ -35,20 +41,12 @@ struct WalkedPath {
};
/** listener for event callbacks */
class OfflineAndroidListener {
public:
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) = 0;
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onGravity(const Timestamp ts, const AccelerometerData data) = 0;
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) = 0;
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
};
/** read recorded android sensor data files */
/**
* read sensor data files that were recorded using
* the old java android app
*/
class OfflineAndroid {
private:
std::vector<OfflineEntry<WiFiMeasurements>> wifi;
@@ -56,12 +54,16 @@ private:
std::vector<OfflineEntry<GyroscopeData>> gyro;
std::vector<OfflineEntry<AccelerometerData>> accel;
std::vector<OfflineEntry<AccelerometerData>> gravity;
std::vector<OfflineEntry<GravityData>> gravity;
std::vector<OfflineEntry<CompassData>> compass;
std::vector<OfflineEntry<BarometerData>> barometer;
std::vector<OfflineEntry<GPSData>> gps;
WalkedPath walkedPath;
static constexpr char sep = ';';
const char* name = "OfflineData";
public:
@@ -84,11 +86,17 @@ public:
const std::vector<OfflineEntry<AccelerometerData>>& getAccelerometer() const {return accel;}
/** get all gravity readings */
const std::vector<OfflineEntry<AccelerometerData>>& getGravity() const {return gravity;}
const std::vector<OfflineEntry<GravityData>>& getGravity() const {return gravity;}
/** get all barometer readings */
const std::vector<OfflineEntry<BarometerData>>& getBarometer() const {return barometer;}
/** get all compass readings */
const std::vector<OfflineEntry<CompassData>>& getCompass() const {return compass;}
/** get all gps readings */
const std::vector<OfflineEntry<GPSData>>& getGPS() const {return gps;}
/** get the walked path */
const WalkedPath& getWalkedPath() const {return walkedPath;}
@@ -105,7 +113,7 @@ public:
public:
void parse(const std::string& file, OfflineAndroidListener* listener = nullptr) {
void parse(const std::string& file, Offline::Listener* listener = nullptr) {
Log::add(name, "parsing data file: " + file , false);
Log::tick();
@@ -152,47 +160,61 @@ public:
private:
/** parse the given data */
void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, OfflineAndroidListener* listener) {
void parse(const Timestamp ts, const int32_t sensorID, const std::string& sensorData, Offline::Listener* listener) {
// how to parse
switch(sensorID) {
case 0: {
case (int) Offline::Sensor::ACC: {
const AccelerometerData data = parseAccelerometer(sensorData);
accel.push_back(OfflineEntry<AccelerometerData>(ts, data));
if (listener) {listener->onAccelerometer(ts, data);}
break;
}
case 1: {
const AccelerometerData data = parseAccelerometer(sensorData);
gravity.push_back(OfflineEntry<AccelerometerData>(ts, data));
case (int) Offline::Sensor::GRAVITY: {
const GravityData data = parseGravity(sensorData);
gravity.push_back(OfflineEntry<GravityData>(ts, data));
if (listener) {listener->onGravity(ts, data);}
break;
}
case 3: {
case (int) Offline::Sensor::GYRO: {
const GyroscopeData data = parseGyroscope(sensorData);
gyro.push_back(OfflineEntry<GyroscopeData>(ts, data));
if (listener) {listener->onGyroscope(ts, data);}
break;
}
case 5: {
case (int) Offline::Sensor::BARO: {
const BarometerData data = parseBarometer(sensorData);
barometer.push_back(OfflineEntry<BarometerData>(ts, data));
if (listener) {listener->onBarometer(ts, data);}
break;
}
case 8: {
case (int) Offline::Sensor::WIFI: {
const WiFiMeasurements data = parseWiFi(ts, sensorData);
wifi.push_back(OfflineEntry<WiFiMeasurements>(ts, data));
if (listener) {listener->onWiFi(ts, data);}
break;
}
case 99: {
case (int) Offline::Sensor::COMPASS: {
const CompassData data = parseCompass(sensorData);
compass.push_back(OfflineEntry<CompassData>(ts, data));
if (listener) {listener->onCompass(ts, data);}
break;
}
case (int) Offline::Sensor::GPS: {
const GPSData data = parseGPS(sensorData);
gps.push_back(OfflineEntry<GPSData>(ts, data));
if (listener) {listener->onGPS(ts, data);}
break;
}
case (int) Offline::Sensor::GROUND_TRUTH: {
const GroundTruthID data = parseGroundTruthTick(sensorData);
groundTruth.push_back(OfflineEntry<GroundTruthID>(ts, data));
// TODO listener
@@ -279,6 +301,24 @@ private:
}
static inline GravityData parseGravity(const std::string& data) {
const size_t pos1 = data.find(';', 0);
const size_t pos2 = data.find(';', pos1+1);
const size_t pos3 = data.find(';', pos2+1);
Assert::isTrue(pos1 != std::string::npos, "format error");
Assert::isTrue(pos2 != std::string::npos, "format error");
Assert::isTrue(pos3 != std::string::npos, "format error");
const std::string sx = data.substr(0, pos1);
const std::string sy = data.substr(pos1+1, pos2-pos1-1);
const std::string sz = data.substr(pos2+1, pos3-pos2-1);
return GravityData(std::stof(sx), std::stof(sy), std::stof(sz));
}
/** parse the given Barometer entry */
static inline BarometerData parseBarometer(const std::string& data) {
@@ -326,6 +366,35 @@ private:
}
/** parse the given Compass entry */
static inline CompassData parseCompass(const std::string& data) {
CompassData compass;
Splitter s(data, sep);
compass.azimuth = s.has(0) ? (s.getFloat(0)) : (NAN);
compass.quality01 = s.has(1) ? (s.getFloat(1)) : (NAN);
return compass;
}
/** parse the given GPS entry */
static inline GPSData parseGPS(const std::string& data) {
GPSData gps;
Splitter s(data, sep);
gps.lat = s.has(0) ? (s.getFloat(0)) : (NAN);
gps.lon = s.has(1) ? (s.getFloat(1)) : (NAN);
gps.alt = s.has(2) ? (s.getFloat(2)) : (NAN);
gps.accuracy = s.has(3) ? (s.getFloat(3)) : (NAN);
gps.speed = s.has(4) ? (s.getFloat(4)) : (NAN);
return gps;
}
};
#endif // OFFLINEANDROID_H

38
sensors/offline/Sensors.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef OFFLINE_SENSORS_H
#define OFFLINE_SENSORS_H
namespace Offline {
enum class Sensor {
ACC = 0,
GRAVITY = 1,
LIN_ACC = 2,
GYRO = 3,
BARO = 5,
COMPASS = 6, // also called "orientatioN"
WIFI = 8,
BEACON = 9,
GPS = 16,
GROUND_TRUTH = 99,
POS = 1001, // IPIN2016
};
template <typename T> struct TS {
const uint64_t ts;
T data;
TS(const uint64_t ts) : ts(ts) {;}
TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
};
/** entry for one sensor */
struct Entry {
Sensor type;
uint64_t ts;
int idx;
Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
};
}
#endif // OFFLINE_SENSORS_H

53
sensors/offline/Splitter.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef DATA_SPLITTER_H
#define DATA_SPLITTER_H
#include <string>
#include <vector>
/**
* split an input-file into various tokens
*/
class Splitter {
std::string str;
char sep = ';';
std::vector<std::string> split;
public:
/** ctor */
Splitter(const std::string& str, const char sep = ';') : str(str), sep(sep) {
build();
}
bool has(const int idx) const {return split.size() > idx;}
const std::string& get(const int idx) const {return split.at(idx);}
const float getFloat(const int idx) const {return std::stof(get(idx));}
size_t size() const {return split.size();}
private:
void build() {
std::string cur;
for (char c : str) {
if (c == sep) {
split.push_back(cur);
cur = "";
} else {
cur += c;
}
}
split.push_back(cur);
}
};
#endif // DATA_SPLITTER_H

81
sensors/pressure/ActivityButterPressure.h
View File

@@ -18,8 +18,6 @@ public:
enum Activity {DOWN, STAY, UP};
struct History {
Timestamp ts;
BarometerData data;
@@ -27,23 +25,25 @@ public:
};
private:
//just for debugging and plotting
std::vector<History> input;
std::vector<History> inputInterp;
std::vector<History> output;
std::vector<float> sumHist;
std::vector<float> mvAvgHist;
std::vector<History> actHist;
std::vector<History> output;
Activity currentActivity;
MovingAVG<float> mvAvg = MovingAVG<float>(20);
/** change this values for much success */
/** change this values for much success
*
* Nexus 6:
* butter = Filter::ButterworthLP<float>(10,0.1f,2);
* threshold = 0.025;
* diffSize = 20;
* FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
*/
const bool additionalLowpassFilter = false;
const int diffSize = 20; //the number values used for finding the activity.
const float threshold = 0.025; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.1f,2);
Filter::ButterworthLP<float> butter2 = Filter::ButterworthLP<float>(10,0.1f,2);
const unsigned long diffSize = 20; //the number values used for finding the activity.
const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.05f,2);
Filter::ButterworthLP<float> butter2 = Filter::ButterworthLP<float>(10,0.05f,2);
FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
public:
@@ -68,14 +68,11 @@ public:
return STAY;
}
input.push_back(History(ts, baro));
bool newInterpolatedValues = false;
//interpolate & butter
auto callback = [&] (const Timestamp ts, const float val) {
float interpValue = val;
inputInterp.push_back(History(ts, BarometerData(interpValue)));
//butter
float butterValue = butter.process(interpValue);
@@ -89,10 +86,10 @@ public:
if(newInterpolatedValues == true){
//getActivity
if((int)output.size() > diffSize){
if(output.size() > diffSize){
//diff
std::vector<float> diff;
for(int i = output.size() - diffSize; i < output.size() - 1; ++i){
for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
float diffVal = output[i+1].data.hPa - output[i].data.hPa;
@@ -113,7 +110,6 @@ public:
}else{
actValue = sum;
}
sumHist.push_back(actValue);
if(actValue > threshold){
currentActivity = DOWN;
@@ -127,8 +123,6 @@ public:
}
}
actHist.push_back(History(ts, BarometerData(currentActivity)));
return currentActivity;
}
@@ -137,49 +131,6 @@ public:
Activity getCurrentActivity() {
return currentActivity;
}
std::vector<float> getSensorHistory(){
std::vector<float> tmp;
for(History val : input){
tmp.push_back(val.data.hPa);
}
return tmp;
}
std::vector<float> getInterpolatedHistory(){
std::vector<float> tmp;
for(History val : inputInterp){
tmp.push_back(val.data.hPa);
}
return tmp;
}
std::vector<float> getOutputHistory(){
std::vector<float> tmp;
for(History val : output){
tmp.push_back(val.data.hPa);
}
return tmp;
}
std::vector<float> getSumHistory(){
return sumHist;
}
std::vector<History> getActHistory(){
return actHist;
}
};
#endif // ACTIVITYBUTTERPRESSURE_H

187
sensors/pressure/ActivityButterPressurePercent.h Normal file
View File

@@ -0,0 +1,187 @@
#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H
#define ACTIVITYBUTTERPRESSUREPERCENT_H
#include "../../data/Timestamp.h"
#include "../../math/filter/Butterworth.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/distribution/Normal.h"
#include <KLib/Assertions.h>
#include <vector>
#include "BarometerData.h"
/**
* receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
* activity recognition based on a small window given by matlabs diff(window)
*
* todo: if an elevator is detected, first we have a short time the stairs are more prober.
*/
class ActivityButterPressurePercent {
public:
struct ActivityProbabilities{
float elevatorDown;
float stairsDown;
float stay;
float stairsUp;
float elevatorUp;
ActivityProbabilities(float elevatorDown, float stairsDown,
float stay, float stairsUp, float elevatorUp) :
elevatorDown(elevatorDown), stairsDown(stairsDown),
stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;}
ActivityProbabilities() :
elevatorDown(0.01f), stairsDown(0.01f),
stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;}
};
struct History {
Timestamp ts;
BarometerData data;
History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;}
};
private:
std::vector<History> output;
bool initialize;
ActivityProbabilities currentActivity;
/** change this values for much success */
const unsigned long diffSize = 20; //the number values used for finding the activity.
Filter::ButterworthLP<float> butter = Filter::ButterworthLP<float>(10,0.05f,2);
FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
const float variance = 0.02f;
const float muStairs = 0.04f;
const float muStay = 0.00f;
const float muEleveator = 0.08f;
std::vector<float> densities = std::vector<float>(5, 1);
std::vector<float> densitiesOld = std::vector<float>(5, 1);
public:
/** ctor */
ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){
initialize = true;
}
/** add new sensor readings that were received at the given timestamp */
ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) {
//init
if(initialize){
butter.stepInitialization(baro.hPa);
initialize = false;
return currentActivity;
}
bool newInterpolatedValues = false;
//interpolate & butter
auto callback = [&] (const Timestamp ts, const float val) {
float interpValue = val;
//butter
float butterValue = butter.process(interpValue);
output.push_back(History(ts, BarometerData(butterValue)));
newInterpolatedValues = true;
};
ffi.add(ts, baro.hPa, callback);
if(newInterpolatedValues == true){
//getActivity
if(output.size() > diffSize){
//diff
std::vector<float> diff;
for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){
float diffVal = output[i+1].data.hPa - output[i].data.hPa;
diff.push_back(diffVal);
}
float sum = 0;
for(float val : diff){
sum += val;
}
float actValue = sum;
//calculate the probabilites of walking down/up etc...
densitiesOld = densities;
//in one building there is an ultra fast elevator, therefore we need to clip the activity value...
if(actValue > muEleveator){
actValue = muEleveator;
}
if(actValue < -muEleveator){
actValue = -muEleveator;
}
float densityElevatorDown = Distribution::Normal<float>::getProbability(muEleveator, variance, actValue);
float densityStairsDown = Distribution::Normal<float>::getProbability(muStairs, variance, actValue);
float densityStay = Distribution::Normal<float>::getProbability(muStay, variance, actValue);
float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);
_assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
_assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
_assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
_assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
_assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");
_assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
_assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
_assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
_assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
_assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");
//wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
densities[0] = densityElevatorDown;
densities[1] = densityStairsDown;
densities[2] = densityStay;
densities[3] = densityStairsUp;
densities[4] = densityElevatorUp;
//normalize
float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4];
for(unsigned long i = 0; i < densities.size(); ++i){
densities[i] /= densitySum;
//values cant be zero!
densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f);
}
currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]);
}
}
//retruns for every call, indepedent of callback.
return currentActivity;
}
/** get the current Activity */
ActivityProbabilities getCurrentActivity() {
return currentActivity;
}
};
#endif // ACTIVITYBUTTERPRESSUREPERCENT_H

13
sensors/pressure/BarometerData.h
View File

@@ -13,6 +13,19 @@ struct BarometerData {
explicit BarometerData(const float hPa) : hPa(hPa) {;}
/** valid data? */
bool isValid() const {
return hPa == hPa;
}
bool operator == (const BarometerData& o ) const {
return EQ_OR_NAN(hPa, o.hPa);
}
private:
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
};
#endif // BAROMETERDATA_H

6
sensors/pressure/PressureTendence.h
View File

@@ -51,9 +51,9 @@ public:
K::GnuplotPlotElementLines tendence;
Debug() {
plot.add(&raw); raw.setColorHex("#999999");
plot.add(&avg); avg.setColorHex("#000000");
plot.add(&tendence); tendence.setLineWidth(2);
plot.add(&raw); raw.getStroke().getColor().setHexStr("#999999");
plot.add(&avg); avg.getStroke().getColor().setHexStr("#000000");
plot.add(&tendence); tendence.getStroke().setWidth(2);
tendence.setCustomAttr(" axes x1y2 ");
gp << "set y2tics\n";
gp << "set y2range[-0.3:+0.3]\n";

5
sensors/radio/AccessPoint.h
View File

@@ -45,6 +45,11 @@ public:
;
}
/** equals? */
bool operator == (const AccessPoint& o) {
return (o.mac == mac) && (o.ssid == ssid);
}
public:
/** get the AP's MAC address */

27
sensors/radio/LocatedAccessPoint.h
View File

@@ -1,27 +0,0 @@
#ifndef LOCATEDACCESSPOINT_H
#define LOCATEDACCESSPOINT_H
#include "AccessPoint.h"
#include "../../geo/Point3.h"
#include "../../floorplan/v2/Floorplan.h"
/**
* describes an access-point including its position (in meter)
*/
class LocatedAccessPoint : public AccessPoint, public Point3 {
public:
/** ctor */
LocatedAccessPoint(const std::string& mac, const Point3 pos_m) : AccessPoint(mac, ""), Point3(pos_m) {
;
}
/** ctor */
LocatedAccessPoint(const Floorplan::AccessPoint& ap) : AccessPoint(ap.mac, ap.name), Point3(ap.pos) {
;
}
};
#endif // LOCATEDACCESSPOINT_H

30
sensors/radio/VAPGrouper.h
View File

@@ -7,6 +7,8 @@
#include "../../math/Stats.h"
#include "../../misc/Debug.h"
#include "WiFiMeasurements.h"
class VAPGrouper {
@@ -40,24 +42,35 @@ public:
private:
static constexpr const char* name = "VAPGrp";
/** the mode to use for grouping VAPs */
const Mode mode;
/** the signal-strength aggregation algorithm to use */
const Aggregation agg;
/** respect only outputs with at-least X occurences of one physical hardware [can be used to prevent issues] */
int minOccurences;
public:
/** ctor */
VAPGrouper(const Mode mode, const Aggregation agg) : mode(mode), agg(agg) {
VAPGrouper(const Mode mode, const Aggregation agg, const int minOccurences = 2) :
mode(mode), agg(agg), minOccurences(minOccurences) {
;
}
/** set the number of needed occurences per VAP-group to be accepted */
void setMinOccurences(const int min) {
this->minOccurences = min;
}
/** get a vap-grouped version of the given input */
WiFiMeasurements group(const WiFiMeasurements& original) const {
// first, group all VAPs into a vector [one vector per VAP-group]
// by using the modified MAC address that all VAPs have in common
std::unordered_map<MACAddress, std::vector<WiFiMeasurement>> grouped;
for (const WiFiMeasurement& m : original.entries) {
@@ -68,8 +81,9 @@ public:
}
// output
// to-be-constructed output
WiFiMeasurements result;
int skipped = 0;
// perform aggregation on each VAP-group
for (auto it : grouped) {
@@ -77,6 +91,9 @@ public:
const MACAddress& base = it.first;
const std::vector<WiFiMeasurement>& vaps = it.second;
// remove entries that do not satify the min-occurences metric
if ((int)vaps.size() < minOccurences) {++skipped; continue;}
// group all VAPs into one measurement
const WiFiMeasurement groupedMeasurement = groupVAPs(base, vaps);
@@ -85,6 +102,13 @@ public:
}
// debug
Log::add(name,
"grouped " + std::to_string(original.entries.size()) + " measurements " +
"into " + std::to_string(result.entries.size()) + " [omitted: " + std::to_string(skipped) + "]",
true
);
// done
return result;

47
sensors/radio/WiFiMeasurement.h
View File

@@ -9,7 +9,7 @@
*/
class WiFiMeasurement {
public:
private:
friend class VAPGrouper;
@@ -19,32 +19,55 @@ public:
/** the measured signal strength */
float rssi;
/** OPTIONAL. frequence the signal was received */
float freq = NAN;
/** OPTIONAL. timestamp the measurement was recorded at */
Timestamp ts;
public:
/** ctor */
WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {
WiFiMeasurement(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi), freq(NAN) {
;
}
/** ctor with timestamp */
WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
;
}
/** ctor with freq*/
WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) {
;
}
/** ctor with timestamp */
WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) {
;
}
/** ctor with timestamp and freq*/
WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) {
;
}
public:
/** get the AP we got the measurement for */
const AccessPoint& getAP() const {return ap;}
/** get the AP we got the measurement for */
const AccessPoint& getAP() const {return ap;}
/** get the measurement's signal strength */
float getRSSI() const {return rssi;}
/** get the measurement's signal strength */
float getRSSI() const {return rssi;}
/** OPTIONAL: get the measurement's timestamp (if known!) */
const Timestamp& getTimestamp() const {return ts;}
/** OPTIONAL: get the measurement's timestamp (if known!) */
const Timestamp& getTimestamp() const {return ts;}
/** OPTIONAL: get the measurement's frequence (if known!) */
float getFrequency() const {return freq;}
/** set another signal strength */
void setRssi(float value){rssi = value;}
/** set the timestamp */
void setTimestamp(const Timestamp& val){ts = val;}
};
#endif // WIFIMEASUREMENT_H

50
sensors/radio/WiFiMeasurements.h
View File

@@ -22,6 +22,56 @@ struct WiFiMeasurements {
return res;
}
/** get the measurements for the given MAC [if available] otherwise null */
const WiFiMeasurement* getForMac(const MACAddress& mac) const {
for (const WiFiMeasurement& m : entries) {
if (m.getAP().getMAC() == mac) {
return &m;
}
}
return nullptr;
}
/** remove the entry for the given MAC (if any) */
void remove(const MACAddress& mac) {
for (size_t i = 0; i < entries.size(); ++i) {
if (entries[i].getAP().getMAC() == mac) {
entries.erase(entries.begin() + i);
break;
}
}
}
/** create a combination */
static WiFiMeasurements mix(const WiFiMeasurements& a, const WiFiMeasurements& b, float sec = 3) {
Timestamp max;
WiFiMeasurements res;
for (const WiFiMeasurement& m : a.entries) {
res.entries.push_back(m);
if (m.getTimestamp() > max) {max = m.getTimestamp();}
}
for (const WiFiMeasurement& m : b.entries) {
res.entries.push_back(m);
if (m.getTimestamp() > max) {max = m.getTimestamp();}
}
std::vector<WiFiMeasurement> tmp;
std::swap(res.entries, tmp);
for (const WiFiMeasurement& m : tmp) {
if ((max - m.getTimestamp()).sec() < sec) {
res.entries.push_back(m);
}
}
return res;
}
};
#endif // WIFIMEASUREMENTS_H

81
sensors/radio/WiFiProbabilityFree.h
View File

@@ -5,6 +5,7 @@
#include "model/WiFiModel.h"
#include "../../math/Distributions.h"
#include "VAPGrouper.h"
#include "../../floorplan/v2/Floorplan.h"
#include <unordered_map>
@@ -16,7 +17,7 @@ class WiFiObserverFree : public WiFiProbability {
private:
const float sigma = 8.0f;
const float sigma;
const float sigmaPerSecond = 3.0f;
@@ -26,15 +27,25 @@ private:
/** the map's floorplan */
Floorplan::IndoorMap* map;
std::vector<AccessPoint> allAPs;
bool useError = false;
public:
WiFiObserverFree(const float sigma, WiFiModel& model) : sigma(sigma), model(model) {
allAPs = model.getAllAPs();
}
void setUseError(const bool useError) {
this->useError = useError;
}
double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
double prob = 1.0;
//double prob = 0;
int numMatchingAPs = 0;
// process each measured AP
@@ -49,7 +60,6 @@ public:
// NaN? -> AP not known to the model -> skip
if (modelRSSI != modelRSSI) {continue;}
// the scan's RSSI
const float scanRSSI = entry.getRSSI();
@@ -57,28 +67,85 @@ public:
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?");
Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?");
Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range");
//Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range");
// sigma grows with measurement age
const float sigma = this->sigma + this->sigmaPerSecond * age.sec();
// probability for this AP
double local = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
//double local = Distribution::Exponential<double>::getProbability(0.1, std::abs(modelRSSI-scanRSSI));
// also add the error value? [location is OK but model is wrong]
if (useError) {
local = 0.95 * local + 0.05 * (1.0-local);
//local = 0.95 * local + 0.05;
}
// update probability
prob *= Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI);
//prob *= Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI);
prob *= local;
++numMatchingAPs;
}
// sanity check
Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
//Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
return prob;
}
/**
* for some locations it might make sense to also look at APs
* that have NOT been discovered by the smartphone but SHOULD
* be very well receivable at a location.
* such locations can be opted-out by using this veto-probability
*/
double getVeto(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {
(void) curTime;
struct APR {
AccessPoint ap;
float rssi;
APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;}
};
std::vector<APR> all;
for (const AccessPoint& ap : allAPs) {
const float rssi = model.getRSSI(ap.getMAC(), pos_m);
if (rssi != rssi) {continue;} // unknown to the model
all.push_back(APR(ap, rssi));
}
// stort by RSSI
auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;};
std::sort(all.begin(), all.end(), comp);
int numVetos = 0;
for (int i = 0; i < 3; ++i) {
const APR& apr = all[i];
const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
const float rssiScan = (mes) ? (mes->getRSSI()) : (-100);
const float rssiModel = apr.rssi;
const float diff = std::abs(rssiScan - rssiModel);
if (diff > 20) {++numVetos;}
}
if (numVetos == 0) {return 0.70;}
if (numVetos == 1) {return 0.29;}
else {return 0.01;}
}
template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {
throw Exception("todo??");
return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs);
}
};

7
sensors/radio/WiFiProbabilityGrid.h
View File

@@ -59,7 +59,7 @@ public:
// after some runtime, check whether the wifi timestamps make sense
// those must not be zero, otherwise something is wrong!
if (!obs.entries.empty()) {
Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().getTimestamp().isZero(), "WiFiMeasurement timestamp is 0. this does not make sense...");
}
// process each observed measurement
@@ -73,7 +73,7 @@ public:
const Timestamp age = curTime - measurement.getTimestamp();
// sigma grows with measurement age
float sigma = this->sigma + this->sigmaPerSecond * age.sec();
float sigma = this->sigma + this->sigmaPerSecond * age.sec();
// the RSSI from the scan
const float measuredRSSI = measurement.getRSSI();
@@ -102,8 +102,7 @@ public:
}
// sanity check
// Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
// if (numMatchingAPs == 0) {return 0;}
//Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");
// as not every node has the same number of visible/matching APs
// we MUST return something like the average probability

106
sensors/radio/WiFiQualityAnalyzer.h Normal file
View File

@@ -0,0 +1,106 @@
#ifndef WIFIQUALITYANALYZER_H
#define WIFIQUALITYANALYZER_H
#include "WiFiMeasurements.h"
#include <unordered_set>
class WiFiQualityAnalyzer {
private:
int historySize = 3;
std::vector<WiFiMeasurements> history;
float quality = 0;
public:
/** attach the current measurement and infer the quality */
void add(const WiFiMeasurements& mes) {
// update the history
history.push_back(mes);
while(history.size() > historySize) {
history.erase(history.begin());
}
// recalculate
update();
}
/** get a quality score for the WiFi */
float getQuality() const {
return quality;
}
private:
/** re-calculate the current quality */
void update() {
const float qCnt = getQualityByCount();
const float qAvgdB = getQualityByAvgDB();
quality = qAvgdB;
}
/** score [0:1] based on the average sig-strength. the higher the better */
float getQualityByAvgDB() const {
// stats
float sum = 0;
float sum2 = 0;
float cnt = 0;
for (const WiFiMeasurements& mes : history) {
for (const WiFiMeasurement& m : mes.entries) {
const float rssi = m.getRSSI();
sum += rssi;
sum2 += rssi*rssi;
++cnt;
}
}
// average sig strength
const float avg = sum/cnt;
const float avg2 = sum2/cnt;
const float stdDev = std::sqrt(avg2 - avg*avg);
// avg rssi score
const float minRSSI = -85;
const float maxRSSI = -70;
float score1 = (avg-minRSSI) / (maxRSSI-minRSSI); // min = 0; max = 1
if (score1 > 1) {score1 = 1;}
if (score1 < 0) {score1 = 0;}
//const float score1 = 1.0 - std::exp(-0.07 * (avg-minRSSI));
// std dev score
const float score2 = 1.0 - std::exp(-1.0 * stdDev);
return score1 * score2;
}
/** quality score [0:1] according to the number of seen APs */
float getQualityByCount() const {
// distinct macs
std::unordered_set<MACAddress> macs;
for (const WiFiMeasurements& mes : history) {
for (const WiFiMeasurement& m : mes.entries) {
macs.insert(m.getAP().getMAC());
}
}
// number of distinct macs
const int cnt = macs.size();
// see function plot. function between [0.0:1.0] has 0.5 around 7 seen APs
return 1.0 - std::exp(-0.1 * cnt);
}
};
#endif // WIFIQUALITYANALYZER_H

14
sensors/radio/model/WiFiModel.h
View File

@@ -1,7 +1,13 @@
#ifndef WIFIMODEL_H
#define WIFIMODEL_H
#include "../LocatedAccessPoint.h"
#include "../AccessPoint.h"
#include "../../../geo/Point3.h"
#include <vector>
#include "../../../data/XMLserialize.h"
/**
* interface for signal-strength prediction models.
@@ -9,10 +15,13 @@
* the model is passed a MAC-address of an AP in question, and a position.
* hereafter the model returns the RSSI for this AP at the questioned location.
*/
class WiFiModel {
class WiFiModel : public XMLserialize {
public:
/** dtor */
virtual ~WiFiModel() {;}
// /** get the given access-point's RSSI at the provided location */
// virtual float getRSSI(const LocatedAccessPoint& ap, const Point3 p) = 0;
@@ -27,6 +36,7 @@ public:
*/
virtual float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const = 0;
};
#endif // WIFIMODEL_H

32
sensors/radio/model/WiFiModelFactory.h Normal file
View File

@@ -0,0 +1,32 @@
#ifndef WIFIMODELFACTORY_H
#define WIFIMODELFACTORY_H
#include "WiFiModel.h"
#include "../../../floorplan/v2/Floorplan.h"
/**
* this class can instantiate WiFiModels based on serialized XML files
*/
class WiFiModelFactory {
private:
Floorplan::IndoorMap* map;
public:
/** ctor. needs the floorplan for some models */
WiFiModelFactory(Floorplan::IndoorMap* map) : map(map) {
;
}
/** parse model from XML file */
WiFiModel* loadXML(const std::string& file);
/** parse model from XML node */
WiFiModel* readFromXML(XMLDoc* doc, XMLElem* src);
};
#endif // WIFIMODELFACTORY_H

42
sensors/radio/model/WiFiModelFactoryImpl.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef WIFIMODELFACTORYIMPL_H
#define WIFIMODELFACTORYIMPL_H
#include "WiFiModelFactory.h"
#include "WiFiModelLogDist.h"
#include "WiFiModelLogDistCeiling.h"
#include "WiFiModelPerFloor.h"
#include "WiFiModelPerBBox.h"
WiFiModel* WiFiModelFactory::loadXML(const std::string& file) {
XMLDoc doc;
XMLserialize::assertOK(doc.LoadFile(file.c_str()), doc, "error while loading file");
XMLElem* root = doc.FirstChildElement("root");
return readFromXML(&doc, root);
}
WiFiModel* WiFiModelFactory::readFromXML(XMLDoc* doc, XMLElem* src) {
// each model attaches its "type" during serialization
const std::string type = src->Attribute("type");
WiFiModel* mdl = nullptr;
// create an instance for the model
if (type == "WiFiModelLogDist") {mdl = new WiFiModelLogDist();}
else if (type == "WiFiModelLogDistCeiling") {mdl = new WiFiModelLogDistCeiling(map);}
else if (type == "WiFiModelPerFloor") {mdl = new WiFiModelPerFloor(map);}
else if (type == "WiFiModelPerBBox") {mdl = new WiFiModelPerBBox(map);}
else {throw Exception("invalid model type given: " + type);}
// load the model from XML
mdl->readFromXML(doc, src);
// done
return mdl;
}
#endif // WIFIMODELFACTORYIMPL_H

10
sensors/radio/model/WiFiModelLogDist.h
View File

@@ -4,6 +4,8 @@
#include "WiFiModel.h"
#include "LogDistanceModel.h"
#include <unordered_map>
/**
* signal-strength estimation using log-distance model
*/
@@ -86,6 +88,14 @@ public:
}
void readFromXML(XMLDoc* doc, XMLElem* src) override {
throw Exception("not yet implemented");
}
void writeToXML(XMLDoc* doc, XMLElem* dst) override {
throw Exception("not yet implemented");
}
};
#endif // WIFIMODELLOGDIST_H

154
sensors/radio/model/WiFiModelLogDistCeiling.h
View File

@@ -2,6 +2,7 @@
#define WIFIMODELLOGDISTCEILING_H
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanCeilings.h"
#include "../../../Assertions.h"
#include "WiFiModel.h"
@@ -9,6 +10,8 @@
#include "../VAPGrouper.h"
#include "../../../misc/Debug.h"
#include "../../../data/XMLserialize.h"
/**
* signal-strength estimation using log-distance model
* including ceilings between AP and position
@@ -36,24 +39,28 @@ private:
/** map of all APs (and their parameters) known to the model */
std::unordered_map<MACAddress, APEntry> accessPoints;
/** position (height) of all ceilings (in meter) */
std::vector<float> ceilingsAtHeight_m;
// /** position (height) of all ceilings (in meter) */
// std::vector<float> ceilingsAtHeight_m;
Floorplan::Ceilings ceilings;
public:
/** ctor with floorplan (needed for ceiling position) */
WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) {
WiFiModelLogDistCeiling(const Floorplan::IndoorMap* map) : ceilings(map) {
// sanity checks
Assert::isTrue(map->floors.size() >= 1, "map has no floors?!");
// position of all ceilings
for (Floorplan::Floor* f : map->floors) {
ceilingsAtHeight_m.push_back(f->atHeight);
}
}
/** get the entry for the given mac. exception if missing */
const APEntry& getAP(const MACAddress& mac) const {
const auto& it = accessPoints.find(mac);
if (it == accessPoints.end()) {throw Exception("model does not contain an entry for " + mac.asString());}
return it->second;
}
/** get a list of all APs known to the model */
std::vector<AccessPoint> getAllAPs() const {
std::vector<AccessPoint> aps;
@@ -62,38 +69,47 @@ public:
}
/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
void loadAPs(const Floorplan::IndoorMap* map, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
const APEntry ape(ap->getPos(floor), txp, exp, waf);
addAP(MACAddress(ap->mac), ape);
addAP(MACAddress(ap->mac), ape, assertSafe);
}
}
}
/** load AP information from the floorplan. use the given fixed TXP/EXP/WAF for all APs */
void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f) {
/**
* load AP information from the floorplan.
* use the given fixed TXP/EXP/WAF for all APs.
* usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
*/
void loadAPs(const Floorplan::IndoorMap* map, const VAPGrouper& vg, const float txp = -40.0f, const float exp = 2.5f, const float waf = -8.0f, const bool assertSafe = true) {
for (const Floorplan::Floor* floor : map->floors) {
for (const Floorplan::AccessPoint* ap : floor->accesspoints) {
const APEntry ape(ap->getPos(floor), txp, exp, waf);
const MACAddress mac = vg.getBaseMAC(MACAddress(ap->mac));
Log::add("WiModLDC", "AP: " + ap->mac + " -> " + mac.asString());
addAP(MACAddress(mac), ape);
Log::add("WiModLDC", "AP added! given: " + ap->mac + " -> after VAP: " + mac.asString());
addAP(MACAddress(mac), ape, assertSafe);
}
}
}
/** make the given AP (and its parameters) known to the model */
void addAP(const MACAddress& accessPoint, const APEntry& params) {
/**
* make the given AP (and its parameters) known to the model
* usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
*/
void addAP(const MACAddress& accessPoint, const APEntry& params, const bool assertSafe = true) {
// sanity check
Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
if (assertSafe) {
Assert::isBetween(params.waf, -99.0f, 0.0f, "WAF out of bounds [-99:0]");
Assert::isBetween(params.txp, -50.0f, -30.0f, "TXP out of bounds [-50:-30]");
Assert::isBetween(params.exp, 1.0f, 4.0f, "EXP out of bounds [1:4]");
}
Assert::equal(accessPoints.find(accessPoint), accessPoints.end(), "AccessPoint already present! VAP-Grouping issue?");
@@ -102,6 +118,14 @@ public:
}
/**
* make the given AP (and its parameters) known to the model
* usually txp,exp,waf are checked for a sane range. if you know what you are doing, set assertSafe to false!
*/
void addAP(const MACAddress& accessPoint, const Point3 pos_m, const float txp, const float exp, const float waf, const bool assertSafe = true) {
addAP(accessPoint, APEntry(pos_m, txp, exp, waf), assertSafe);
}
/** remove all added APs */
void clear() {
accessPoints.clear();
@@ -134,71 +158,71 @@ public:
const float rssiLOS = LogDistanceModel::distanceToRssi(params.txp, params.exp, distance_m);
// WAF loss (params.waf is a negative value!) -> WAF loss is also a negative value
const float wafLoss = params.waf * numCeilingsBetween(position_m, params.position_m);
//const float wafLoss = params.waf * ceilings.numCeilingsBetween(position_m, params.position_m);
const float wafLoss = params.waf * ceilings.numCeilingsBetweenFloat(position_m, params.position_m);
//const float wafLoss = params.waf * ceilings.numCeilingsBetweenLinearInt(position_m, params.position_m);
// combine
return rssiLOS + wafLoss;
const float res = rssiLOS + wafLoss;
// sanity check
Assert::isNotNaN(res, "detected NaN within WiFiModelLogDistCeiling::getRSSI()");
// ok
return res;
}
void writeToXML(XMLDoc* doc, XMLElem* dst) override {
// set my type
dst->SetAttribute("type", "WiFiModelLogDistCeiling");
protected:
FRIEND_TEST(LogDistanceCeilingModel, numCeilings);
FRIEND_TEST(LogDistanceCeilingModel, numCeilingsFloat);
/** get the number of ceilings between z1 and z2 */
float numCeilingsBetweenFloat(const Point3 pos1, const Point3 pos2) const {
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
float cnt = 0;
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {
const float dmax = zMax - z;
cnt += (dmax > 1) ? (1) : (dmax);
}
for (const auto& it : accessPoints) {
const MACAddress& mac = it.first;
const APEntry& ape = it.second;
XMLElem* xap = doc->NewElement("ap");
xap->SetAttribute("mac", mac.asString().c_str());
xap->SetAttribute("px", ape.position_m.x);
xap->SetAttribute("py", ape.position_m.y);
xap->SetAttribute("pz", ape.position_m.z);
xap->SetAttribute("txp", ape.txp);
xap->SetAttribute("exp", ape.exp);
xap->SetAttribute("waf", ape.waf);
dst->InsertEndChild(xap);
}
return cnt;
for (const float atHeight_m : ceilings.getCeilings()) {
XMLElem* xceil = doc->NewElement("ceiling");
xceil->SetAttribute("atHeight", atHeight_m);
dst->InsertEndChild(xceil);
}
}
/** get the number of ceilings between z1 and z2 */
int numCeilingsBetween(const Point3 pos1, const Point3 pos2) const {
void readFromXML(XMLDoc* doc, XMLElem* src) override {
int cnt = 0;
const float zMin = std::min(pos1.z, pos2.z);
const float zMax = std::max(pos1.z, pos2.z);
// check type
if (std::string("WiFiModelLogDistCeiling") != src->Attribute("type")) {throw Exception("invalid model type");}
#ifdef WITH_ASSERTIONS
accessPoints.clear();
ceilings.clear();
static int numNear = 0;
static int numFar = 0;
for (const float z : ceilingsAtHeight_m) {
const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) );
if (diff < 0.1) {++numNear;} else {++numFar;}
}
if ((numNear + numFar) > 150000) {
Assert::isTrue(numNear < numFar*0.1,
"many requests to the WiFiModel address nodes (very) near to a ground! \
due to rounding issues, determining the number of floors between AP and point-in-question is NOT possible! \
expect very wrong outputs! \
consider adding the person's height to the questioned positions: p += Point3(0,0,1.3) "
XML_FOREACH_ELEM_NAMED("ap", xap, src) {
MACAddress mac = MACAddress(xap->Attribute("mac"));
APEntry ape(
Point3(xap->FloatAttribute("px"), xap->FloatAttribute("py"), xap->FloatAttribute("pz")),
xap->FloatAttribute("txp"),
xap->FloatAttribute("exp"),
xap->FloatAttribute("waf")
);
}
#endif
for (const float z : ceilingsAtHeight_m) {
if (zMin < z && zMax > z) {++cnt;}
accessPoints.insert( std::make_pair(mac, ape) );
}
return cnt;
XML_FOREACH_ELEM_NAMED("ceiling", xceil, src) {
const float atHeight_m = xceil->FloatAttribute("atHeight");
ceilings.addCeiling(atHeight_m);
}
}

167
sensors/radio/model/WiFiModelPerBBox.h Normal file
View File

@@ -0,0 +1,167 @@
#ifndef WIFIMODELPERBBOX_H
#define WIFIMODELPERBBOX_H
#include "../AccessPoint.h"
#include "../../../geo/Point3.h"
#include "../../../geo/BBoxes3.h"
#include <vector>
#include "WiFiModelFactory.h"
/**
* FOR TESTING
*
* this model allows using a different sub-models
* identified by a bound-box to reduce the error
*/
class WiFiModelPerBBox : public WiFiModel {
struct ModelForBBoxes {
WiFiModel* mdl;
BBoxes3 bboxes;
/** ctor */
ModelForBBoxes(WiFiModel* mdl, BBoxes3 bboxes) : mdl(mdl), bboxes(bboxes) {;}
/** does this entry apply to the given position? */
bool matches(const Point3 pt) const {
if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");}
return bboxes.contains(pt);
}
};
Floorplan::IndoorMap* map;
/** all contained models [one per bbox] */
std::vector<ModelForBBoxes> models;
public:
WiFiModelPerBBox(Floorplan::IndoorMap* map) : map(map) {
;
}
/** dtor */
virtual ~WiFiModelPerBBox() {
}
/** get a list of all APs known to the model */
std::vector<AccessPoint> getAllAPs() const override {
// combine all submodels
std::vector<AccessPoint> res;
for (const ModelForBBoxes& sub : models) {
for (const AccessPoint& ap : sub.mdl->getAllAPs()) {
if (std::find(res.begin(), res.end(), ap) == res.end()) { // TODO use map instead?
res.push_back(ap);
}
}
}
return res;
}
void add(WiFiModel* mdl, const BBoxes3 bboxes) {
if (bboxes.get().empty()) {throw Exception("no bbox(es) given for model!");}
ModelForBBoxes mfb(mdl, bboxes);
models.push_back(mfb);
}
float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
for (const ModelForBBoxes& mfb : models) {
if (mfb.matches(position_m)) {return mfb.mdl->getRSSI(accessPoint, position_m);}
}
return -120;
}
void readFromXML(XMLDoc* doc, XMLElem* src) override {
// check type
if (std::string("WiFiModelPerBBox") != src->Attribute("type")) {throw Exception("invalid model type");}
models.clear();
// model factory [create models based on XMl content]
WiFiModelFactory fac(map);
// parse all contained models [one per floor]
XML_FOREACH_ELEM_NAMED("entry", xentry, src) {
// all bboxes
BBoxes3 bboxes;
XML_FOREACH_ELEM_NAMED("bbox", xbbox, xentry) {
const float x1 = xbbox->FloatAttribute("x1");
const float y1 = xbbox->FloatAttribute("y1");
const float z1 = xbbox->FloatAttribute("z1");
const float x2 = xbbox->FloatAttribute("x2");
const float y2 = xbbox->FloatAttribute("y2");
const float z2 = xbbox->FloatAttribute("z2");
const BBox3 bbox(Point3(x1,y1,z1), Point3(x2,y2,z2));
bboxes.add(bbox);
}
// node for the model
XMLElem* xmodel = xentry->FirstChildElement("model");
// let the factory instantiate the model
WiFiModel* mdl = fac.readFromXML(doc, xmodel);
// add
models.push_back(ModelForBBoxes(mdl, bboxes));
}
}
void writeToXML(XMLDoc* doc, XMLElem* dst) override {
// set my type
dst->SetAttribute("type", "WiFiModelPerBBox");
for (const ModelForBBoxes& mfb : models) {
// all models
XMLElem* xentry = doc->NewElement("entry"); {
// each bbox
for (const BBox3& bbox : mfb.bboxes.get()) {
XMLElem* xbbox = doc->NewElement("bbox"); {
xbbox->SetAttribute("x1", bbox.getMin().x);
xbbox->SetAttribute("y1", bbox.getMin().y);
xbbox->SetAttribute("z1", bbox.getMin().z);
xbbox->SetAttribute("x2", bbox.getMax().x);
xbbox->SetAttribute("y2", bbox.getMax().y);
xbbox->SetAttribute("z2", bbox.getMax().z);
}; xentry->InsertFirstChild(xbbox);
}
// the corresponding model
XMLElem* xmodel = doc->NewElement("model"); {
mfb.mdl->writeToXML(doc, xmodel);
}; xentry->InsertEndChild(xmodel);
}; dst->InsertEndChild(xentry);
}
}
};
#endif // WIFIMODELPERBBOX_H

133
sensors/radio/model/WiFiModelPerFloor.h Normal file
View File

@@ -0,0 +1,133 @@
#ifndef WIFIMODELPERFLOOR_H
#define WIFIMODELPERFLOOR_H
#include "../AccessPoint.h"
#include "../../../geo/Point3.h"
#include <vector>
#include "WiFiModelFactory.h"
/**
* FOR TESTING
*
* this model allows using a different sub-models for each floor to reduce the error
*/
class WiFiModelPerFloor : public WiFiModel {
struct ModelForFloor {
float fromZ;
float toZ;
WiFiModel* mdl;
/** ctor */
ModelForFloor(const float fromZ, const float toZ, WiFiModel* mdl) : fromZ(fromZ), toZ(toZ), mdl(mdl) {;}
/** does this entry apply to the given z-position? */
bool matches(const float z) const {
return (fromZ <= z && z < toZ);
}
};
Floorplan::IndoorMap* map;
/** all contained models [one per floor] */
std::vector<ModelForFloor> models;
public:
WiFiModelPerFloor(Floorplan::IndoorMap* map) : map(map) {
;
}
/** dtor */
virtual ~WiFiModelPerFloor() {
}
/** get a list of all APs known to the model */
std::vector<AccessPoint> getAllAPs() const override {
// combine all submodels
std::vector<AccessPoint> res;
for (const ModelForFloor& sub : models) {
for (const AccessPoint& ap : sub.mdl->getAllAPs()) {
if (std::find(res.begin(), res.end(), ap) == res.end()) { // TODO use map instead?
res.push_back(ap);
}
}
}
return res;
}
void add(WiFiModel* mdl, const Floorplan::Floor* floor) {
ModelForFloor mff(floor->atHeight, floor->atHeight+floor->height, mdl);
models.push_back(mff);
}
float getRSSI(const MACAddress& accessPoint, const Point3 position_m) const override {
for (const ModelForFloor& mff : models) {
if (mff.matches(position_m.z)) {return mff.mdl->getRSSI(accessPoint, position_m);}
}
return -120;
}
void readFromXML(XMLDoc* doc, XMLElem* src) override {
// check type
if (std::string("WiFiModelPerFloor") != src->Attribute("type")) {throw Exception("invalid model type");}
models.clear();
// model factory [create models based on XMl content]
WiFiModelFactory fac(map);
// parse all contained models [one per floor]
XML_FOREACH_ELEM_NAMED("floor", xfloor, src) {
// floor params
const float z1 = xfloor->FloatAttribute("z1");
const float z2 = xfloor->FloatAttribute("z2");
// node for the model
XMLElem* xmodel = xfloor->FirstChildElement("model");
// let the factory instantiate the model
WiFiModel* mdl = fac.readFromXML(doc, xmodel);
// add
models.push_back(ModelForFloor(z1, z2, mdl));
}
}
void writeToXML(XMLDoc* doc, XMLElem* dst) override {
// set my type
dst->SetAttribute("type", "WiFiModelPerFloor");
for (const ModelForFloor& mff : models) {
XMLElem* xfloor = doc->NewElement("floor"); {
xfloor->SetAttribute("z1", mff.fromZ);
xfloor->SetAttribute("z2", mff.toZ);
XMLElem* xmodel = doc->NewElement("model"); {
mff.mdl->writeToXML(doc, xmodel);
}; xfloor->InsertEndChild(xmodel);
}; dst->InsertEndChild(xfloor);
}
}
};
#endif // WIFIMODELPERFLOOR_H

8
sensors/radio/model/WiFiModels.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef WIFIMODELS_H
#define WIFIMODELS_H
#include "WiFiModel.h"
#include "WiFiModelFactory.h"
#include "WiFiModelFactoryImpl.h"
#endif // WIFIMODELS_H

27
sensors/radio/setup/WiFiFingerprint.h
View File

@@ -8,10 +8,10 @@
#include <unordered_map>
/**
* denotes a wifi fingerprint
* known position and several measurements conducted at this position
* denotes a wifi fingerprint:
* known position and 1-n measurements [wifi-scan on all channels] conducted at this position
*
* as several measurements were conducted, each AP is usually contained more than once!
* if more than one measurement is conducted, each AP is contained more than once!
*/
struct WiFiFingerprint {
@@ -19,7 +19,7 @@ struct WiFiFingerprint {
/** real-world-position that was measured */
Point3 pos_m;
/** measurements (APs) at the given location */
/** seen APs at the given location */
WiFiMeasurements measurements;
@@ -29,9 +29,11 @@ struct WiFiFingerprint {
/** ctor */
WiFiFingerprint(const Point3 pos_m) : pos_m(pos_m) {;}
/** ctor */
WiFiFingerprint(const Point3 pos_m, const WiFiMeasurements& measurements) : pos_m(pos_m), measurements(measurements) {;}
/** as each AP is contained more than once (scanned more than once), group them by MAC and use the average RSSI */
/** as each AP might be contained more than once (scanned more than once), group them by MAC and use the average RSSI */
WiFiMeasurements average() {
// group scans by MAC (all measurements for one AP)
@@ -46,9 +48,9 @@ struct WiFiFingerprint {
const WiFiMeasurements& apMeasurements = it.second;
WiFiMeasurement avg = apMeasurements.entries.front(); // average starts with a copy of the first entry (to get all data-fields beside the rssi)
for (int i = 1; i < (int)apMeasurements.entries.size(); ++i) { // sum up all other entries [1:end]
avg.rssi += apMeasurements.entries[i].rssi;
avg.setRssi(avg.getRSSI() + apMeasurements.entries[i].getRSSI());
}
avg.rssi /= apMeasurements.entries.size();
avg.setRssi(avg.getRSSI() / apMeasurements.entries.size());
res.entries.push_back(avg); // add to output
}
@@ -57,12 +59,21 @@ struct WiFiFingerprint {
}
/** get all measurements for the given MAC */
std::vector<WiFiMeasurement> getAllForMAC(const MACAddress& mac) const {
std::vector<WiFiMeasurement> res;
for (const WiFiMeasurement& m : measurements.entries) {
if (m.getAP().getMAC() == mac) {res.push_back(m);}
}
return res;
}
/** serialize */
void write(std::ostream& out) const {
out << "pos: " << pos_m.x << " " << pos_m.y << " " << pos_m.z << "\n";
out << "num: " << measurements.entries.size() << "\n";
for (const WiFiMeasurement& wm : measurements.entries) {
out << wm.getTimestamp().ms() << " " << wm.ap.getMAC().asString() << " " << wm.getRSSI() << "\n";
out << wm.getTimestamp().ms() << " " << wm.getAP().getMAC().asString() << " " << wm.getRSSI() << "\n";
}
}

140
sensors/radio/setup/WiFiFingerprints.h Normal file
View File

@@ -0,0 +1,140 @@
#ifndef WIFIFINGERPRINTS_H
#define WIFIFINGERPRINTS_H
#include <fstream>
#include "WiFiFingerprint.h"
#include <iostream>
/**
* helper class to load and save fingerprints.
* fingerprints are wifi-measurements given at some known location
* those can be used to e.g. optimize access-point models etc.
*/
class WiFiFingerprints {
private:
// /** the file to save the calibration model to */
// std::string file;
/** all fingerprints (position -> measurements) within the model */
std::vector<WiFiFingerprint> fingerprints;
public:
/** empty ctor */
WiFiFingerprints() {
}
/** ctor from file */
WiFiFingerprints(const std::string& file) {
load(file);
}
const std::vector<WiFiFingerprint>& getFingerprints() const {
return fingerprints;
}
std::vector<WiFiFingerprint>& getFingerprints() {
return fingerprints;
}
/** attach the given fingerprint */
void add(const WiFiFingerprint& fp) {
fingerprints.push_back(fp);
}
/** get all fingerprints that measured exactly the given mac [no VAP grouping] */
const std::vector<WiFiFingerprint> getFingerprintsFor(const MACAddress& mac) const {
std::vector<WiFiFingerprint> res;
// process each fingerprint location
for (const WiFiFingerprint& _fp : fingerprints) {
// start with an empty copy
WiFiFingerprint fp = _fp; fp.measurements.entries.clear();
// only add the measurements that belong to the requested mac
for (const WiFiMeasurement& _m : _fp.measurements.entries) {
if (_m.getAP().getMAC() == mac) {
fp.measurements.entries.push_back(_m);
}
}
if (fp.measurements.entries.size() > 0) { // got some measurements for this AP?
res.push_back(fp);
}
}
return res;
}
/** deserialize the model */
void load(const std::string& file) {
// open and check
std::ifstream inp(file.c_str());
if (inp.bad() || inp.eof() || ! inp.is_open()) { return; }
// read all entries
while (!inp.eof()) {
// each section starts with [fingerprint]
std::string section;
inp >> section;
if (inp.eof()) {break;}
if (section != "[fingerprint]") {throw Exception("error!");}
// deserialize it
WiFiFingerprint wfp;
wfp.read(inp);
if (wfp.measurements.entries.empty()) {continue;}
fingerprints.push_back(wfp);
}
inp.close();
}
/** serialize the model */
void save(const std::string& file) {
// open and check
std::ofstream out(file.c_str());
if (out.bad()) {throw Exception("error while opening " + file + " for write");}
// write all entries
for (const WiFiFingerprint& wfp : fingerprints) {
out << "[fingerprint]\n";
wfp.write(out);
}
// done
out.close();
}
/** get the fingerprint for the given location. if no fingerprint exists, an empty one is created! */
WiFiFingerprint& getFingerprint(const Point3 pos_m) {
// try to find an existing one
for (WiFiFingerprint& wfp : fingerprints) {
// get within range of floating-point rounding issues
if (wfp.pos_m.getDistance(pos_m) < 0.01) {return wfp;}
}
// create a new one and return its reference
WiFiFingerprint wfp(pos_m);
fingerprints.push_back(wfp);
return fingerprints.back();
}
};
#endif // WIFIFINGERPRINTS_H

265
sensors/radio/setup/WiFiOptimizer.h
View File

@@ -1,247 +1,68 @@
#ifndef OPTIMIZER_H
#define OPTIMIZER_H
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
#ifndef WIFIOPTIMIZER_H
#define WIFIOPTIMIZER_H
#include "WiFiFingerprints.h"
#include "WiFiOptimizerStructs.h"
#include "../VAPGrouper.h"
#include "../../../geo/BBox3.h"
#include "../../../misc/Debug.h"
#include "WiFiFingerprint.h"
#include "../model/WiFiModel.h"
#include "../model/WiFiModelLogDistCeiling.h"
#include <unordered_map>
#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
namespace WiFiOptimizer {
#include <string>
#include <sstream>
/** base-class for all WiFiOptimizers */
class Base {
struct WiFiOptimizer {
protected:
private:
/** each MAC-Adress has several position->rssi entries */
std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
/** combine one RSSI measurement with the position the signal was measured at */
struct RSSIatPosition {
/** how to handle virtual access points [group, not-group, ..] */
const VAPGrouper vg;
/** real-world position (in meter) */
const Point3 pos_m;
/** measured signal strength (for one AP) */
const float rssi;
public:
/** ctor */
RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
Base(const VAPGrouper vg) : vg(vg) {;}
};
public:
struct APParams {
float x;
float y;
float z;
float txp;
float exp;
float waf;
Point3 getPos() const {return Point3(x,y,z);}
APParams() {;}
APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
std::string asString() {
std::stringstream ss;
ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
return ss.str();
}
};
/** add MAC-info to params */
struct APParamsMAC {
MACAddress mac;
APParams params;
APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
};
private:
Floorplan::IndoorMap* map;
const VAPGrouper vg;
/** each MAC-Adress has several position->rssi entries */
std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;
const char* name = "WiFiOptimizer";
public:
/** ctor */
WiFiOptimizer(Floorplan::IndoorMap* map, const VAPGrouper& vg) : map(map), vg(vg) {
;
}
/** add a new fingerprint to the optimizers data-source */
void addFingerprint(const WiFiFingerprint& fp) {
// group the fingerprint's measurements by VAP (if configured)
const WiFiMeasurements measurements = vg.group(fp.measurements);
// add each available AP to its slot (lookup map)
for (const WiFiMeasurement& m : measurements.entries) {
const RSSIatPosition rap(fp.pos_m, m.rssi);
apMap[m.getAP().getMAC()].push_back(rap);
/** get a list of all to-be-optimized access-points (given by their mac-address) */
virtual std::vector<MACAddress> getAllMACs() const {
std::vector<MACAddress> res;
for (const auto& it : apMap) {res.push_back(it.first);}
return res;
}
}
/** get a list of all to-be-optimized access-points (given by their mac-address) */
std::vector<MACAddress> getAllMACs() const {
std::vector<MACAddress> res;
for (const auto& it : apMap) {res.push_back(it.first);}
return res;
}
/** optimize all known APs */
std::vector<APParamsMAC> optimizeAll() const {
// sanity chekc
Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!");
float errSum = 0;
std::vector<APParamsMAC> res;
for (const MACAddress& mac : getAllMACs()) {
float err;
const APParams params = optimize(mac, err);
res.push_back(APParamsMAC(mac, params));
errSum += err;
/** get all [VAPGrouped, Averaged] fingerprints for the given mac */
virtual const std::vector<RSSIatPosition>& getFingerprintsFor(const MACAddress& mac) {
const auto& it = apMap.find(mac);
if (it == apMap.end()) {throw Exception("mac not found: " + mac.asString());}
return it->second;
}
const float avgErr = errSum / getAllMACs().size();
Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
/** add a new fingerprint to the optimizer as data-source */
virtual void addFingerprint(const WiFiFingerprint& fp) {
return res;
}
/** optimize the given AP */
APParams optimize(const MACAddress& mac, float& errResult) const {
// starting parameters do not matter for the current optimizer!
APParams params(0,0,0, -40, 2.5, -4.0);
constexpr float hugeError = 1e10;
// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
// signal-strength-prediction-model...
WiFiModelLogDistCeiling model(map);
auto func = [&] (const float* data) {
const APParams* params = (APParams*) data;
// some sanity checks
if (params->waf > 0) {return hugeError;}
if (params->txp < -50) {return hugeError;}
if (params->txp > -30) {return hugeError;}
if (params->exp > 4) {return hugeError;}
if (params->exp < 1) {return hugeError;}
// current position guess for the AP;
const Point3 apPos_m = params->getPos();
// add the AP [described by the current guess] to the signal-strength-prediction model
model.clear();
model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
float err = 0;
int cnt = 0;
// process each measurement
for (const RSSIatPosition& reading : entries) {
// get the model-estimation for the fingerprint's position
const float rssiModel = model.getRSSI(mac, reading.pos_m);
// difference between estimation and measurement
const float diff = std::abs(rssiModel - reading.rssi);
// adjust the error
err += diff*diff;
++cnt;
// max distance penality
// [unlikely to get a reading for this AP here!]
if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
// group the fingerprint's measurements by VAP (if configured)
const WiFiMeasurements measurements = vg.group(fp.measurements);
// add each available AP to its slot (lookup map)
for (const WiFiMeasurement& m : measurements.entries) {
const RSSIatPosition rap(fp.pos_m, m.getRSSI());
apMap[m.getAP().getMAC()].push_back(rap);
}
err /= cnt;
err = std::sqrt(err);
}
if (params->txp < -50) {err += 999999;}
if (params->txp > -35) {err += 999999;}
/** add new fingerprints to the optimizer as data-source */
virtual void addFingerprints(const WiFiFingerprints& fps) {
for (const WiFiFingerprint& fp : fps.getFingerprints()) {
addFingerprint(fp);
}
}
if (params->exp > 3.5) {err += 999999;}
if (params->exp < 1.0) {err += 999999;}
return err;
};
};
//
const BBox3 mapBBox = FloorplanHelper::getBBox(map);
}
using LeOpt = K::NumOptAlgoRangeRandom<float>;
const std::vector<LeOpt::MinMax> valRegion = {
LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x
LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y
LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z
LeOpt::MinMax(-50,-30), // txp
LeOpt::MinMax(1,3), // exp
LeOpt::MinMax(-10,-4), // waf
};
// log
Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
Log::tick();
LeOpt opt(valRegion);
opt.setPopulationSize(500); // USE MORE FOR PRODUCTION
opt.setNumIerations(150);
opt.calculateOptimum(func, (float*) &params);
// using LeOpt = K::NumOptAlgoGenetic<float>;
// LeOpt opt(6);
// opt.setPopulationSize(750);
// opt.setMaxIterations(50);
// opt.setElitism(0.05f);
// opt.setMutation(0.75f);
// //opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
// opt.setValRegion(valRegion);
// K::NumOptAlgoDownhillSimplex<float, 6> opt;
// opt.setMaxIterations(100);
// opt.setNumRestarts(10);
opt.calculateOptimum(func, (float*) &params);
errResult = func((float*)&params);
Log::tock();
Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err");
return params;
}
};
#endif // OPTIMIZER_H
#endif // WIFIOPTIMIZER_H

345
sensors/radio/setup/WiFiOptimizerLogDistCeiling.h Normal file
View File

@@ -0,0 +1,345 @@
#ifndef WIFI_OPTIMIZER_LOG_DIST_CEILING_H
#define WIFI_OPTIMIZER_LOG_DIST_CEILING_H
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
#include "../../../geo/BBox3.h"
#include "../../../misc/Debug.h"
#include "WiFiFingerprint.h"
#include "WiFiFingerprints.h"
#include "../model/WiFiModel.h"
#include "../model/WiFiModelLogDistCeiling.h"
#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>
#include <string>
#include <sstream>
#include "WiFiOptimizer.h"
#include "WiFiOptimizerStructs.h"
#include <functional>
namespace WiFiOptimizer {
/**
* optimize access-point parameters,
* given several fingerprints using the log-dist-ceiling model
*/
struct LogDistCeiling : public Base {
public:
enum class Mode {
FAST,
MEDIUM,
QUALITY,
};
/**
* resulting optimization stats for one AP
*/
struct Stats {
/** average model<->scan error after optimzing */
float error_db;
/** number of fingerprints [= locations] that were used for optimzing */
int usedFingerprins;
/** resulting model<->scan error after optimzing for each individual fingerprints [= location] */
std::vector<ErrorAtPosition> errors;
/** get the location where the model estimation reaches the highest negative value [model estimation too low] */
ErrorAtPosition getEstErrorMaxNeg() const {
auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
return *std::min_element(errors.begin(), errors.end(), cmpErrAtPos);
}
/** get the location where the model estimation reaches the highest positive value [model estimation too high] */
ErrorAtPosition getEstErrorMaxPos() const {
auto cmpErrAtPos = [] (const ErrorAtPosition& a, const ErrorAtPosition& b) {return a.getError_db() < b.getError_db();};
return *std::max_element(errors.begin(), errors.end(), cmpErrAtPos);
}
};
/** parameters for one AP when using the LogDistCeiling model */
struct APParams {
float x;
float y;
float z;
float txp;
float exp;
float waf;
Point3 getPos() const {return Point3(x,y,z);}
/** ctor */
APParams() {;}
/** ctor */
APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
std::string asString() const {
std::stringstream ss;
ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
return ss.str();
}
/** we add some constraints to the parameter range */
bool outOfRange() const {
return (waf > 0) ||
(txp < -50) ||
(txp > -30) ||
(exp > 4) ||
(exp < 1);
}
};
/** add MAC-info to params */
struct APParamsMAC {
MACAddress mac;
APParams params;
APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
};
class APParamsList {
std::vector<APParamsMAC> lst;
public:
/** ctor */
APParamsList(const std::vector<APParamsMAC>& lst) : lst(lst) {
}
/** get the list */
const std::vector<APParamsMAC>& get() const {
return lst;
}
/** get params for the given mac [if known, otherwise nullptr] */
const APParamsMAC* get (const MACAddress& mac) const {
for (const APParamsMAC& ap : lst) {
if (ap.mac == mac) {return &ap;}
}
return nullptr;
}
};
using APFilter = std::function<bool(const int numFingerprints, const MACAddress& mac)>;
const APFilter NONE = [] (const int numFingerprints, const MACAddress& mac) {
(void) numFingerprints; (void) mac;
return false;
};
const APFilter MIN_5_FPS = [] (const int numFingerprints, const MACAddress& mac) {
(void) mac;
return numFingerprints < 5;
};
private:
Floorplan::IndoorMap* map;
Mode mode = Mode::QUALITY;
const char* name = "WiFiOptLDC";
public:
/** ctor */
LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) {
;
}
/** ctor */
LogDistCeiling(Floorplan::IndoorMap* map, const VAPGrouper& vg, const WiFiFingerprints& fps, const Mode mode = Mode::QUALITY) : Base(vg), map(map), mode(mode) {
addFingerprints(fps);
}
/** optimize all known APs */
APParamsList optimizeAll(APFilter filter) const {
// sanity check
Assert::isFalse(getAllMACs().empty(), "no APs found for optimization! call addFingerprint() first!");
float errSum = 0; int errCnt = 0;
std::vector<APParamsMAC> res;
for (const MACAddress& mac : getAllMACs()) {
Stats stats;
const APParams params = optimize(mac, stats);
if (!filter(stats.usedFingerprins, mac)) {
res.push_back(APParamsMAC(mac, params));
errSum += stats.error_db;
++errCnt;
} else {
std::cout << "ignored due to filter!" << std::endl;
}
}
const float avgErr = errSum / errCnt;
Log::add(name, "optimized APs: " + std::to_string(errCnt));
Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");
// done
return APParamsList(res);
}
/** optimize the given AP */
APParams optimize(const MACAddress& mac, Stats& res) const {
// starting parameters do not matter for the current optimizer!
APParams params(0,0,0, -40, 2.5, -4.0);
// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;
// log
Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
Log::tick();
// get the map's size
const BBox3 mapBBox = FloorplanHelper::getBBox(map);
using LeOpt = K::NumOptAlgoRangeRandom<float>;
const std::vector<LeOpt::MinMax> valRegion = {
LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20), // x
LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20), // y
LeOpt::MinMax(mapBBox.getMin().z - 5, mapBBox.getMax().z + 5), // z
LeOpt::MinMax(-50, -30), // txp
LeOpt::MinMax(1, 4), // exp
LeOpt::MinMax(-15, -0), // waf
};
LeOpt opt(valRegion);
switch(mode) {
case Mode::FAST:
opt.setPopulationSize(100);
opt.setNumIerations(50);
break;
case Mode::MEDIUM:
opt.setPopulationSize(200);
opt.setNumIerations(100);
break;
case Mode::QUALITY:
opt.setPopulationSize(500);
opt.setNumIerations(150);
break;
}
// error function
auto func = [&] (const float* params) {
return getErrorLogDistCeiling(mac, entries, params, nullptr);
};
opt.calculateOptimum(func, (float*) &params);
// using LeOpt = K::NumOptAlgoGenetic<float>;
// LeOpt opt(6);
// opt.setPopulationSize(750);
// opt.setMaxIterations(50);
// opt.setElitism(0.05f);
// opt.setMutation(0.75f);
// //opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
// opt.setValRegion(valRegion);
// K::NumOptAlgoDownhillSimplex<float, 6> opt;
// opt.setMaxIterations(100);
// opt.setNumRestarts(10);
opt.calculateOptimum(func, (float*) &params);
res.error_db = getErrorLogDistCeiling(mac, entries, (float*)&params, &res);
res.usedFingerprins = entries.size();
Log::tock();
Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(res.error_db) +" dB err");
return params;
}
private:
float getErrorLogDistCeiling(const MACAddress& mac, const std::vector<RSSIatPosition>& entries, const float* data, Stats* stats = nullptr) const {
const APParams* params = (APParams*) data;
// some sanity checks
if (params->outOfRange()) {return 1e10;}
// current position guess for the AP;
const Point3 apPos_m = params->getPos();
// add the AP [described by the current guess] to the signal-strength-prediction model
// signal-strength-prediction-model...
WiFiModelLogDistCeiling model(map);
model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));
float err = 0;
int cnt = 0;
// process each measurement
for (const RSSIatPosition& reading : entries) {
// get the model-estimation for the fingerprint's position
const float rssiModel = model.getRSSI(mac, reading.pos_m);
// difference between estimation and measurement
const float diff = std::abs(rssiModel - reading.rssi);
// add error to stats object?
if (stats) {
stats->errors.push_back(ErrorAtPosition(reading.pos_m, reading.rssi, rssiModel));
}
// adjust the error
err += std::pow(std::abs(diff), 2.0);
++cnt;
// max distance penality
// [unlikely to get a reading for this AP here!]
if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}
}
err /= cnt;
err = std::sqrt(err);
if (params->txp < -50) {err += 999999;}
if (params->txp > -35) {err += 999999;}
if (params->exp > 3.5) {err += 999999;}
if (params->exp < 1.0) {err += 999999;}
return err;
}
};
}
#endif // WIFI_OPTIMIZER_LOG_DIST_CEILING_H

55
sensors/radio/setup/WiFiOptimizerStructs.h Normal file
View File

@@ -0,0 +1,55 @@
#ifndef WIFIOPTIMIZERSTRUCTS_H
#define WIFIOPTIMIZERSTRUCTS_H
#include "../../../geo/Point3.h"
namespace WiFiOptimizer {
/**
* one entry that is used during optimization:
* combine one RSSI measurement with the position the signal was measured at
*/
struct RSSIatPosition {
/** real-world position (in meter) */
const Point3 pos_m;
/** measured signal strength (for one AP) */
const float rssi;
/** ctor */
RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}
};
/**
* for statistics
* after optimiziaton
*
* denotes the difference [error] between one fingerprinted rssi
* at location (x,y,z) and the model estimation for this location
*/
struct ErrorAtPosition {
/** real-world position (in meter) */
const Point3 pos_m;
/** measured signal strength (for one AP) */
const float scan_rssi;
/** final model's prediction */
const float model_rssi;
/** ctor */
ErrorAtPosition(const Point3 pos_m, const float scan_rssi, const float model_rssi) : pos_m(pos_m), scan_rssi(scan_rssi), model_rssi(model_rssi) {;}
/** get the difference [error] between model-estimated-rssi and fingerprinted-rssi */
float getError_db() const {
return model_rssi - scan_rssi;
}
};
}
#endif // WIFIOPTIMIZERSTRUCTS_H