From a35a22e676857271d6cfc2447c9c3e3b7f0e1490 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 4 Nov 2016 17:25:49 +0100 Subject: [PATCH] added beacon stuff similiar architecture then wifi \n added activity percentage stuff \n added testcases --- floorplan/v2/Floorplan.h | 1 + .../WalkModuleActivityControlPercent.h | 91 ++++++ .../v2/modules/WalkModuleButterActivity.h | 86 ------ main.cpp | 6 +- sensors/beacon/Beacon.h | 59 ++++ sensors/beacon/BeaconMeasurement.h | 44 +++ sensors/beacon/BeaconMeasurements.h | 26 ++ sensors/beacon/BeaconProbability.h | 15 + sensors/beacon/BeaconProbabilityFree.h | 93 ++++++ sensors/beacon/model/BeaconModel.h | 46 +++ sensors/beacon/model/BeaconModelLogDist.h | 92 ++++++ .../beacon/model/BeaconModelLogDistCeiling.h | 208 ++++++++++++++ sensors/pressure/ActivityButterPressure.h | 24 +- .../pressure/ActivityButterPressurePercent.h | 270 ++++++++++++++++++ sensors/radio/LocatedAccessPoint.h | 27 -- sensors/radio/WiFiMeasurement.h | 22 +- sensors/radio/WiFiProbabilityFree.h | 1 + sensors/radio/WiFiProbabilityGrid.h | 4 +- sensors/radio/model/WiFiModel.h | 3 +- sensors/radio/model/WiFiModelLogDist.h | 2 + sensors/radio/setup/WiFiFingerprint.h | 6 +- sensors/radio/setup/WiFiOptimizer.h | 2 +- tests/Tests.h | 4 +- .../beacon/TestLogDistanceCeilingModel.cpp | 121 ++++++++ tests/sensors/beacon/TestProbabilityFree.cpp | 7 + tests/sensors/pressure/TestBarometer.cpp | 91 +++++- 26 files changed, 1207 insertions(+), 144 deletions(-) create mode 100644 grid/walk/v2/modules/WalkModuleActivityControlPercent.h delete mode 100644 grid/walk/v2/modules/WalkModuleButterActivity.h create mode 100644 sensors/beacon/Beacon.h create mode 100644 sensors/beacon/BeaconMeasurement.h create mode 100644 sensors/beacon/BeaconMeasurements.h create mode 100644 sensors/beacon/BeaconProbability.h create mode 100644 sensors/beacon/BeaconProbabilityFree.h create mode 100644 sensors/beacon/model/BeaconModel.h create mode 100644 sensors/beacon/model/BeaconModelLogDist.h create mode 100644 sensors/beacon/model/BeaconModelLogDistCeiling.h create mode 100644 sensors/pressure/ActivityButterPressurePercent.h delete mode 100644 sensors/radio/LocatedAccessPoint.h create mode 100644 tests/sensors/beacon/TestLogDistanceCeilingModel.cpp create mode 100644 tests/sensors/beacon/TestProbabilityFree.cpp diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 95648d0..0668f03 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -186,6 +186,7 @@ namespace Floorplan { Beacon() : name(), mac(), pos() {;} Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;} bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);} + Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground }; diff --git a/grid/walk/v2/modules/WalkModuleActivityControlPercent.h b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h new file mode 100644 index 0000000..0af1136 --- /dev/null +++ b/grid/walk/v2/modules/WalkModuleActivityControlPercent.h @@ -0,0 +1,91 @@ +#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H +#define WALKMODULEACTIVITYCONTROLPERCENT_H + +#include "WalkModule.h" +#include "WalkStateHeading.h" + +#include "../../../../geo/Heading.h" +#include "../../../../math/Distributions.h" +#include "../../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../../grid/GridNode.h" + + +/** favor z-transitions */ +template class WalkModuleActivityControlPercent : public WalkModule { + +private: + + Control* ctrl; + +public: + + /** ctor */ + WalkModuleActivityControlPercent(Control* ctrl) : ctrl(ctrl) { + ; + } + + virtual void updateBefore(WalkState& state, const Node& startNode) override { + (void) state; + (void) startNode; + } + + virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { + (void) state; + (void) startNode; + (void) endNode; + + } + + virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { + (void) state; + (void) curNode; + (void) nextNode; + } + + double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { + + (void) state; + (void) startNode; + + + const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; + + //floor and doors + if(potentialNode.getType() == 0 || potentialNode.getType() == 3){ + return ctrl->activityPercent.stay; + } + + //stairs + if(potentialNode.getType() == 1){ + +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown + ctrl->barometer.actProbs.stairsUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;} + return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp); + } + + //elevators + if(potentialNode.getType() == 2){ + +// //we need to do this, that particles are able to walk into an elevator even if the prob for that is low, +// //that happens often since the activity has some delay. +// if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown + ctrl->barometer.actProbs.elevatorUp){ +// return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp); +// } + + if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;} + if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;} + + //for walking out of the elevator + return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp); + } + + std::cout << "Node has unknown Type" << std::endl; + return 1.0; + } +}; + +#endif // WALKMODULEACTIVITYCONTROLPERCENT_H diff --git a/grid/walk/v2/modules/WalkModuleButterActivity.h b/grid/walk/v2/modules/WalkModuleButterActivity.h deleted file mode 100644 index bc3f37c..0000000 --- a/grid/walk/v2/modules/WalkModuleButterActivity.h +++ /dev/null @@ -1,86 +0,0 @@ -#ifndef WALKMODULEBUTTERACTIVITY_H -#define WALKMODULEBUTTERACTIVITY_H - -#include "WalkModule.h" - -#include "../../../../geo/Heading.h" -#include "../../../../math/Distributions.h" -#include "../../../../sensors/pressure/ActivityButterPressure.h" - -DEPREACTED -SEE WalkModuleActivityControl - -struct WalkStateBarometerActivity { - - /** innser-struct to prevent name-clashes */ - struct Barometer { - - /** activity currently detected from the baromter */ - ActivityButterPressure::Activity activity; - - Barometer() : activity(ActivityButterPressure::Activity::STAY) {;} - - } barometer; - - /** ctor */ - WalkStateBarometerActivity() : barometer() {;} - -}; - -/** favor z-transitions */ -template class WalkModuleButterActivity : public WalkModule { - -public: - - /** ctor */ - WalkModuleButterActivity() { - - // ensure templates WalkState inherits from 'WalkStateBarometerActivity' - StaticAssert::AinheritsB(); - - } - - virtual void updateBefore(WalkState& state, const Node& startNode) override { - (void) state; - (void) startNode; - } - - virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override { - (void) state; - (void) startNode; - (void) endNode; - - } - - virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override { - (void) state; - (void) curNode; - (void) nextNode; - } - - double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override { - - (void) state; - (void) startNode; - - const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm; - - if(state.barometer.activity == ActivityButterPressure::Activity::DOWN){ - if (deltaZ_cm < 0) {return 0;} - if (deltaZ_cm == 0) {return 0.1;} - return 0.9; - } else if (state.barometer.activity == ActivityButterPressure::Activity::UP){ - if (deltaZ_cm > 0) {return 0;} - if (deltaZ_cm == 0) {return 0.1;} - return 0.9; - } else { - if (deltaZ_cm == 0) {return 0.9;} - return 0.1; - } - - } - - -}; - -#endif // WALKMODULEBUTTERACTIVITY_H diff --git a/main.cpp b/main.cpp index 255a441..03d1e84 100755 --- a/main.cpp +++ b/main.cpp @@ -25,12 +25,12 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModel*"; - ::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; + ::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + ::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/beacon/Beacon.h b/sensors/beacon/Beacon.h new file mode 100644 index 0000000..1604932 --- /dev/null +++ b/sensors/beacon/Beacon.h @@ -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 diff --git a/sensors/beacon/BeaconMeasurement.h b/sensors/beacon/BeaconMeasurement.h new file mode 100644 index 0000000..0bc3ea9 --- /dev/null +++ b/sensors/beacon/BeaconMeasurement.h @@ -0,0 +1,44 @@ +#ifndef BEACONMEASUREMENT_H +#define BEACONMEASUREMENT_H + +#include "../MACAddress.h" +#include "../../data/Timestamp.h" +#include "Beacon.h" + +#include + + +/** 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 diff --git a/sensors/beacon/BeaconMeasurements.h b/sensors/beacon/BeaconMeasurements.h new file mode 100644 index 0000000..dbdf489 --- /dev/null +++ b/sensors/beacon/BeaconMeasurements.h @@ -0,0 +1,26 @@ +#ifndef BEACONMEASUREMENTS_H +#define BEACONMEASUREMENTS_H + +#include + +#include "BeaconMeasurement.h" + +/** + * group of several beacon measurements + */ +struct BeaconMeasurements { + + std::vector 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 diff --git a/sensors/beacon/BeaconProbability.h b/sensors/beacon/BeaconProbability.h new file mode 100644 index 0000000..66c0707 --- /dev/null +++ b/sensors/beacon/BeaconProbability.h @@ -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 diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h new file mode 100644 index 0000000..35e902e --- /dev/null +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -0,0 +1,93 @@ +#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 + +/** + * 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::getProbability(modelRSSI, sigma, scanRSSI); + //prob *= Distribution::Region::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 diff --git a/sensors/beacon/model/BeaconModel.h b/sensors/beacon/model/BeaconModel.h new file mode 100644 index 0000000..c9ba73f --- /dev/null +++ b/sensors/beacon/model/BeaconModel.h @@ -0,0 +1,46 @@ +#ifndef BEACONMODEL_H +#define BEACONMODEL_H + +#include "../Beacon.h" +#include "../BeaconMeasurement.h" +#include "../../../geo/Point3.h" + +#include + +/** + * 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 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 diff --git a/sensors/beacon/model/BeaconModelLogDist.h b/sensors/beacon/model/BeaconModelLogDist.h new file mode 100644 index 0000000..460cac8 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDist.h @@ -0,0 +1,92 @@ +#ifndef BEACONMODELLOGDIST_H +#define BEACONMODELLOGDIST_H + +#include "BeaconModel.h" +#include "../../radio/model/LogDistanceModel.h" + +#include + +/** + * 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 beacons; + +public: + + /** ctor */ + BeaconModelLogDist() { + ; + } + + /** get a list of all beacons known to the model */ + std::vector getAllBeacons() const { + std::vector 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(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 diff --git a/sensors/beacon/model/BeaconModelLogDistCeiling.h b/sensors/beacon/model/BeaconModelLogDistCeiling.h new file mode 100644 index 0000000..a10a128 --- /dev/null +++ b/sensors/beacon/model/BeaconModelLogDistCeiling.h @@ -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 + +/** + * 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 beacons; + + /** position (height) of all ceilings (in meter) */ + std::vector 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 getAllBeacons() const { + std::vector 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, -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(beacons.find(beacon), beacons.end(), "AccessPoint already present!"); + + // add + beacons.insert( std::pair(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 diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/pressure/ActivityButterPressure.h index 5940ae6..2d1617c 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/pressure/ActivityButterPressure.h @@ -38,12 +38,20 @@ public: Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); - /** change this values for much success */ + /** change this values for much success + * + * Nexus 6: + * butter = Filter::ButterworthLP(10,0.1f,2); + * threshold = 0.025; + * diffSize = 20; + * FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + */ const bool additionalLowpassFilter = false; - const int diffSize = 20; //the number values used for finding the activity. + 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 butter = Filter::ButterworthLP(10,0.1f,2); - Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.1f,2); + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); public: @@ -68,14 +76,14 @@ public: return STAY; } - input.push_back(History(ts, baro)); + //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))); + //inputInterp.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -113,7 +121,7 @@ public: }else{ actValue = sum; } - sumHist.push_back(actValue); + //sumHist.push_back(actValue); if(actValue > threshold){ currentActivity = DOWN; @@ -127,7 +135,7 @@ public: } } - actHist.push_back(History(ts, BarometerData(currentActivity))); + //actHist.push_back(History(ts, BarometerData(currentActivity))); return currentActivity; diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/pressure/ActivityButterPressurePercent.h new file mode 100644 index 0000000..85290e2 --- /dev/null +++ b/sensors/pressure/ActivityButterPressurePercent.h @@ -0,0 +1,270 @@ +#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 +#include + + +#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) + */ +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: + //just for debugging and plotting + std::vector input; + std::vector inputInterp; + std::vector output; + std::vector sumHist; + std::vector mvAvgHist; + std::vector actHist; + + bool initialize; + + ActivityProbabilities currentActivity; + + /** change this values for much success */ + const int diffSize = 20; //the number values used for finding the activity. + Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); + FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); + + const float variance = 0.02f; + + const float muStairs = 0.04f; + const float muStay = 0.00f; + const float muEleveator = 0.08; + + std::vector densities = std::vector(5, 1); + std::vector densitiesOld = std::vector(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; + } + + //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); + 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 diff; + for(int 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; + //sumHist.push_back(actValue); + + //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::getProbability(muEleveator, variance, actValue); + float densityStairsDown = Distribution::Normal::getProbability(muStairs, variance, actValue); + float densityStay = Distribution::Normal::getProbability(muStay, variance, actValue); + float densityStairsUp = Distribution::Normal::getProbability(-muStairs, variance, actValue); + float densityElevatorUp = Distribution::Normal::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), "the probability of densityElevatorDown is null!"); + //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!"); + //_assertTrue( (densityStay != 0), "the probability of densityStay is null!"); + //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!"); + //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!"); + + + //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt, + //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV? + + //const Timestamp age = ts - ap.getTimestamp(); + + //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; + + //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end()))); + // if an activity other then staying is detected with a high probability, we are using the previous probability + // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation + // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange. + // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the + // given activity! Since i had no time, this was the fastest solution for now. +// if(highestValueIdx != 2){ +// for(int i = 0; i < densities.size(); ++i){ +// densities[i] *= densitiesOld[i]; +// } +// } + + + //normalize + float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4]; + + for(int i = 0; i < densities.size(); ++i){ + densities[i] /= densitySum; + + //values cant be zero! + densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f); + } + +// densityElevatorDown /= densitySum; +// densityStairsDown /= densitySum; +// densityStay /= densitySum; +// densityStairsUp /= densitySum; +// densityElevatorUp /= densitySum; + + // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility +// densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f); +// densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f); +// densityStay = (densityStay > 0.0f ? densityStay : 0.01f); +// densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f); +// densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f); + + currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]); + + } + + //actHist.push_back(currentActivity); + } + + //retruns for every call, indepedent of callback. + return currentActivity; + } + + /** get the current Activity */ + ActivityProbabilities getCurrentActivity() { + return currentActivity; + } + + std::vector getSensorHistory(){ + std::vector tmp; + + for(History val : input){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getInterpolatedHistory(){ + std::vector tmp; + + for(History val : inputInterp){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getOutputHistory(){ + + std::vector tmp; + + for(History val : output){ + tmp.push_back(val.data.hPa); + } + + return tmp; + } + + std::vector getSumHistory(){ + return sumHist; + } + + + std::vector getActHistory(){ + + return actHist; + } + + +}; + +#endif // ACTIVITYBUTTERPRESSUREPERCENT_H diff --git a/sensors/radio/LocatedAccessPoint.h b/sensors/radio/LocatedAccessPoint.h deleted file mode 100644 index ddd4693..0000000 --- a/sensors/radio/LocatedAccessPoint.h +++ /dev/null @@ -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 diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index 3a07386..0b63d75 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -9,7 +9,7 @@ */ class WiFiMeasurement { -public: +private: friend class VAPGrouper; @@ -19,6 +19,9 @@ public: /** the measured signal strength */ float rssi; + /** OPTIONAL. frequence the signal was received */ + float freq; + /** OPTIONAL. timestamp the measurement was recorded at */ Timestamp ts; @@ -29,11 +32,21 @@ public: ; } + /** 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 */ @@ -45,6 +58,13 @@ public: /** 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;} }; + + #endif // WIFIMEASUREMENT_H diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index a0783f5..0ffc045 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -5,6 +5,7 @@ #include "model/WiFiModel.h" #include "../../math/Distributions.h" #include "VAPGrouper.h" +#include "../../floorplan/v2/Floorplan.h" #include diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index e2cfce2..1ac2189 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -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(); diff --git a/sensors/radio/model/WiFiModel.h b/sensors/radio/model/WiFiModel.h index 05f336c..bebfce4 100644 --- a/sensors/radio/model/WiFiModel.h +++ b/sensors/radio/model/WiFiModel.h @@ -1,7 +1,8 @@ #ifndef WIFIMODEL_H #define WIFIMODEL_H -#include "../LocatedAccessPoint.h" +#include "../AccessPoint.h" +#include "../../../geo/Point3.h" /** * interface for signal-strength prediction models. diff --git a/sensors/radio/model/WiFiModelLogDist.h b/sensors/radio/model/WiFiModelLogDist.h index 417f19c..e30158b 100644 --- a/sensors/radio/model/WiFiModelLogDist.h +++ b/sensors/radio/model/WiFiModelLogDist.h @@ -4,6 +4,8 @@ #include "WiFiModel.h" #include "LogDistanceModel.h" +#include + /** * signal-strength estimation using log-distance model */ diff --git a/sensors/radio/setup/WiFiFingerprint.h b/sensors/radio/setup/WiFiFingerprint.h index d0a2d6b..a92652b 100644 --- a/sensors/radio/setup/WiFiFingerprint.h +++ b/sensors/radio/setup/WiFiFingerprint.h @@ -46,9 +46,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 } @@ -62,7 +62,7 @@ struct WiFiFingerprint { 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"; } } diff --git a/sensors/radio/setup/WiFiOptimizer.h b/sensors/radio/setup/WiFiOptimizer.h index bcd84d6..5ac22b6 100644 --- a/sensors/radio/setup/WiFiOptimizer.h +++ b/sensors/radio/setup/WiFiOptimizer.h @@ -89,7 +89,7 @@ public: // add each available AP to its slot (lookup map) for (const WiFiMeasurement& m : measurements.entries) { - const RSSIatPosition rap(fp.pos_m, m.rssi); + const RSSIatPosition rap(fp.pos_m, m.getRSSI()); apMap[m.getAP().getMAC()].push_back(rap); } diff --git a/tests/Tests.h b/tests/Tests.h index fbe9932..3639618 100755 --- a/tests/Tests.h +++ b/tests/Tests.h @@ -6,8 +6,8 @@ #include static inline std::string getDataFile(const std::string& name) { - return "/mnt/data/workspaces/Indoor/tests/data/" + name; - //return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; + //return "/mnt/data/workspaces/Indoor/tests/data/" + name; + return "/home/toni/Documents/programme/localization/Indoor/tests/data/" + name; } diff --git a/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp new file mode 100644 index 0000000..856394c --- /dev/null +++ b/tests/sensors/beacon/TestLogDistanceCeilingModel.cpp @@ -0,0 +1,121 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../sensors/beacon/model/BeaconModelLogDistCeiling.h" + +TEST(LogDistanceCeilingModelBeacon, calc) { + + // dummy floorplan + Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0; + Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3; + Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7; + + Floorplan::IndoorMap map; + map.floors.push_back(f0); + map.floors.push_back(f1); + map.floors.push_back(f2); + + //LocatedAccessPoint ap0("00:00:00:00:00:00", Point3(0,0,0)); + //LocatedAccessPoint ap25("00:00:00:00:00:00", Point3(0,0,2.5)); + + BeaconModelLogDistCeiling model(&map); + + const MACAddress ap0 = MACAddress("00:00:00:00:00:00"); + const MACAddress ap25 = MACAddress("00:00:00:00:00:01"); + + model.addBeacon(ap0, BeaconModelLogDistCeiling::APEntry( Point3(0,0,0), -40, 1.0, -8.0 )); + model.addBeacon(ap25, BeaconModelLogDistCeiling::APEntry( Point3(0,0,2.5), -40, 1.0, -8.0 )); + + + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(1,0,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,1,0))); + ASSERT_EQ(-40, model.getRSSI(ap0, Point3(0,0,1))); + + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(1,0,2.5))); + ASSERT_EQ(-40, model.getRSSI(ap25, Point3(0,1,2.5))); + ASSERT_EQ(-40-8, model.getRSSI(ap25, Point3(0,0,3.5))); // one floor within + + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,8,0))); + ASSERT_EQ(model.getRSSI(ap0, Point3(8,0,0)), model.getRSSI(ap0, Point3(0,0,8))+8+8); // two ceilings within + +} + +TEST(LogDistanceCeilingModelBeacon, numCeilings) { + + // dummy floorplan + Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0; + Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3; + Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7; + + Floorplan::IndoorMap map; + map.floors.push_back(f0); + map.floors.push_back(f1); + map.floors.push_back(f2); + + BeaconModelLogDistCeiling model(&map); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,0)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,-1)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,1)) ); + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,1), Point3(0,0,0)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,-0.01), Point3(0,0,+0.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,+0.01), Point3(0,0,-0.01)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,2.99), Point3(0,0,3.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,3.01), Point3(0,0,2.99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,6.99), Point3(0,0,7.01)) ); + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,7.01), Point3(0,0,6.99)) ); + + ASSERT_EQ(0, model.numCeilingsBetween(Point3(0,0,7.00), Point3(0,0,99)) ); + + ASSERT_EQ(1, model.numCeilingsBetween(Point3(0,0,0), Point3(0,0,7)) ); + ASSERT_EQ(3, model.numCeilingsBetween(Point3(0,0,-1), Point3(0,0,8)) ); + +} + +TEST(LogDistanceCeilingModelBeacon, numCeilingsFloat) { + + // dummy floorplan + Floorplan::Floor* f0 = new Floorplan::Floor(); f0->atHeight = 0; + Floorplan::Floor* f1 = new Floorplan::Floor(); f1->atHeight = 3; + Floorplan::Floor* f2 = new Floorplan::Floor(); f2->atHeight = 7; + + Floorplan::IndoorMap map; + map.floors.push_back(f0); + map.floors.push_back(f1); + map.floors.push_back(f2); + + BeaconModelLogDistCeiling model(&map); + + const float d = 0.01; + + ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,0)), d ); + ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,-1)), d ); + + ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,1)), d ); + ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,1), Point3(0,0,0)), d ); + + ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,-0.01), Point3(0,0,+0.50)), d ); + ASSERT_NEAR(0.5, model.numCeilingsBetweenFloat(Point3(0,0,+0.50), Point3(0,0,-0.01)), d ); + + ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,2.99), Point3(0,0,3.20)), d ); + ASSERT_NEAR(0.2, model.numCeilingsBetweenFloat(Point3(0,0,3.20), Point3(0,0,2.99)), d ); + + ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,6.99), Point3(0,0,8.33)), d ); + ASSERT_NEAR(1.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,6.99)), d ); + ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,0.00), Point3(0,0,8.33)), d ); + ASSERT_NEAR(2.0, model.numCeilingsBetweenFloat(Point3(0,0,8.33), Point3(0,0,0.00)), d ); + + ASSERT_NEAR(0, model.numCeilingsBetweenFloat(Point3(0,0,7.00), Point3(0,0,99)), d ); + + ASSERT_NEAR(1, model.numCeilingsBetweenFloat(Point3(0,0,0), Point3(0,0,7)), d ); + ASSERT_NEAR(3, model.numCeilingsBetweenFloat(Point3(0,0,-1), Point3(0,0,8)), d ); + +} + + +#endif diff --git a/tests/sensors/beacon/TestProbabilityFree.cpp b/tests/sensors/beacon/TestProbabilityFree.cpp new file mode 100644 index 0000000..8466c2b --- /dev/null +++ b/tests/sensors/beacon/TestProbabilityFree.cpp @@ -0,0 +1,7 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +//todo + +#endif diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 5dcdfe1..1fef297 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -5,6 +5,7 @@ #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" #include "../../../sensors/pressure/ActivityButterPressure.h" +#include "../../../sensors/pressure/ActivityButterPressurePercent.h" #include @@ -78,7 +79,7 @@ TEST(Barometer, LIVE_tendence) { } - sleep(1000); + sleep(1); } @@ -114,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) { } - sleep(1000); + sleep(1); // tendence must be clear and smaller than the sigma @@ -130,6 +131,9 @@ TEST(Barometer, Activity) { std::string filename = getDataFile("barometer/baro1.dat"); std::ifstream infile(filename); + std::vector actHist; + std::vector rawHist; + while (std::getline(infile, line)) { std::istringstream iss(line); @@ -138,35 +142,102 @@ TEST(Barometer, Activity) { while (iss >> ts >> value) { ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); } } - std::vector sum = act.getSumHistory(); - std::vector interpolated = act.getInterpolatedHistory(); - std::vector raw = act.getSensorHistory(); - std::vector butter = act.getOutputHistory(); - std::vector actHist = act.getActHistory(); - K::Gnuplot gp; + K::Gnuplot gpRaw; K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; for(int i=0; i < actHist.size()-1; ++i){ + //raw + K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa); + K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa); + + rawLines.addSegment(raw_p1, raw_p2); + + //results K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); - rawLines.addSegment(input_p1, input_p2); + resultLines.addSegment(input_p1, input_p2); } - plot.add(&rawLines); + plotRaw.add(&rawLines); + plot.add(&resultLines); gp.draw(plot); gp.flush(); + gpRaw.draw(plotRaw); + gpRaw.flush(); + sleep(1); } +TEST(Barometer, ActivityPercent) { + + ActivityButterPressurePercent act; + + //read file + std::string line; + std::string filename = getDataFile("barometer/baro1.dat"); + std::ifstream infile(filename); + + std::vector actHist; + std::vector rawHist; + + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; + + while (iss >> ts >> value) { + ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(value); + actHist.push_back(activity); + } + } + + K::Gnuplot gp; + K::Gnuplot gpRaw; + K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; + K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; + + for(int i=0; i < actHist.size()-1; ++i){ + + K::GnuplotPoint2 raw_p1(i, rawHist[i]); + K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]); + + rawLines.addSegment(raw_p1, raw_p2); + + K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown); + K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown); + + resultLines.addSegment(input_p1, input_p2); + } + + plotRaw.add(&rawLines); + plot.add(&resultLines); + + gp.draw(plot); + gp.flush(); + + gpRaw.draw(plotRaw); + gpRaw.flush(); + + sleep(1000); +} + #endif