From 353bba83427f481679af71c9191c8f3dc291d7f6 Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 25 Jan 2016 17:57:49 +0100 Subject: [PATCH] initial commit before ownership transfer --- code/CMakeLists.txt | 91 + code/README.txt | 7 + code/Vis.h | 73 + code/frank/BeaconEvaluation.h | 108 + code/frank/BeaconObservation.h | 41 + code/frank/BeaconSensorReader.h | 60 + code/frank/MACAddress.h | 131 + code/frank/Position3D.h | 36 + code/frank/PositionedBeacon.h | 23 + code/frank/PositionedWiFiAP.h | 20 + code/frank/Settings.h | 136 + code/frank/WiFiAP.h | 44 + code/frank/WiFiEvaluation.h | 120 + code/frank/WiFiHelper.h | 4 + code/frank/WiFiObservation.h | 23 + code/frank/WiFiSensorReader.h | 46 + code/lukas/ReadMe.txt | 14 + code/lukas/StepDetector.py | 278 ++ code/lukas/StepEvaluation.h | 51 + code/lukas/StepObservation.h | 14 + code/lukas/StepReader.h | 25 + code/lukas/TurnDetector.py | 445 +++ code/lukas/TurnEvaluation.h | 76 + code/lukas/TurnObservation.h | 21 + code/lukas/TurnReader.h | 36 + code/lukas/detection.sh | 21 + code/main.cpp | 58 + code/plan.svg | 4645 +++++++++++++++++++++++++++++ code/reader/SensorReader.h | 66 + code/reader/SensorReaderStep.h | 51 + code/reader/SensorReaderTurn.h | 56 + code/toni/BarometerEvaluation.h | 58 + code/toni/BarometerObservation.h | 17 + code/toni/BarometerSensorReader.h | 91 + code/toni/TFRingBuffer.h | 52 + code/toni/barometric.h | 109 + code/toni/circular.h | 492 +++ 37 files changed, 7639 insertions(+) create mode 100755 code/CMakeLists.txt create mode 100644 code/README.txt create mode 100644 code/Vis.h create mode 100755 code/frank/BeaconEvaluation.h create mode 100755 code/frank/BeaconObservation.h create mode 100755 code/frank/BeaconSensorReader.h create mode 100755 code/frank/MACAddress.h create mode 100755 code/frank/Position3D.h create mode 100755 code/frank/PositionedBeacon.h create mode 100755 code/frank/PositionedWiFiAP.h create mode 100755 code/frank/Settings.h create mode 100755 code/frank/WiFiAP.h create mode 100755 code/frank/WiFiEvaluation.h create mode 100755 code/frank/WiFiHelper.h create mode 100755 code/frank/WiFiObservation.h create mode 100755 code/frank/WiFiSensorReader.h create mode 100755 code/lukas/ReadMe.txt create mode 100755 code/lukas/StepDetector.py create mode 100755 code/lukas/StepEvaluation.h create mode 100755 code/lukas/StepObservation.h create mode 100755 code/lukas/StepReader.h create mode 100755 code/lukas/TurnDetector.py create mode 100755 code/lukas/TurnEvaluation.h create mode 100755 code/lukas/TurnObservation.h create mode 100755 code/lukas/TurnReader.h create mode 100755 code/lukas/detection.sh create mode 100644 code/main.cpp create mode 100755 code/plan.svg create mode 100755 code/reader/SensorReader.h create mode 100755 code/reader/SensorReaderStep.h create mode 100755 code/reader/SensorReaderTurn.h create mode 100755 code/toni/BarometerEvaluation.h create mode 100755 code/toni/BarometerObservation.h create mode 100755 code/toni/BarometerSensorReader.h create mode 100755 code/toni/TFRingBuffer.h create mode 100755 code/toni/barometric.h create mode 100755 code/toni/circular.h diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt new file mode 100755 index 0000000..a138ae4 --- /dev/null +++ b/code/CMakeLists.txt @@ -0,0 +1,91 @@ +# Usage: +# Create build folder, like RC-build next to RobotControl and WifiScan folder +# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' +# make + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) + +# select build type +SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) + +PROJECT(Fusion2016) + +IF(NOT CMAKE_BUILD_TYPE) + MESSAGE(STATUS "No build type selected. Default to Debug") + SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../../ +) + + +FILE(GLOB HEADERS + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h +) + +FILE(GLOB SOURCES + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ../../KLib/inc/tinyxml/*.cpp +) + + +if(${CMAKE_GENERATOR} MATCHES "Visual Studio") + + SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") + SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") + + set(CMAKE_CONFIGURATION_TYPES Release Debug) + +else() + +# system specific compiler flags +ADD_DEFINITIONS( + + -std=gnu++11 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + + -g + -O0 + + -DWITH_TESTS + -DWITH_ASSERTIONS + +) + +endif() + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} + gtest + pthread +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/code/README.txt b/code/README.txt new file mode 100644 index 0000000..1c13966 --- /dev/null +++ b/code/README.txt @@ -0,0 +1,7 @@ +indoor Framework (https://git.frank-ebner.de/kazu/Indoor.git) must be relative to the "Fusion2016" folder: +/path/xyz/Fusion2016/code +/path/xyz/Indoor + +KLib (https://github.com/k-a-z-u/KLib.git) must be relative to the "Fusion2016" folder: +/path/xyz/Fusion2016/code +/path/xyz/KLib diff --git a/code/Vis.h b/code/Vis.h new file mode 100644 index 0000000..bbe6ea7 --- /dev/null +++ b/code/Vis.h @@ -0,0 +1,73 @@ +#ifndef VIS_H +#define VIS_H + +#include +#include +#include +#include + +#include +#include + +class Vis { + +public: + + K::Gnuplot gp; + K::GnuplotSplot splot; + K::GnuplotSplotElementLines floors; + K::GnuplotSplotElementPoints gridNodes; + K::GnuplotSplotElementLines gridEdges; + +public: + + Vis() { + + gp << "set hidden3d front\n"; + gp << "set view equal xy\n"; + gp << "set ticslevel 0\n"; + + // attach all layers + splot.add(&floors); + splot.add(&gridNodes); + splot.add(&gridEdges); + + } + + /** add all obstacles of the given floor to the provided height */ + Vis& addFloor(const Floor& f, const LengthF height) { + + // add each wall + for (const Line2& l : f.getObstacles()) { + const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm()); + const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm()); + floors.addSegment(p1, p2); + } + + return *this; + + } + + /** add the grid to the plot */ + template Vis& addGrid(Grid& grid) { + for (const T& n1 : grid) { + const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); + gridNodes.add(p1); + for (const T& n2 : grid.neighbors(n1)) { + const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm); + gridEdges.addSegment(p1, p2); + } + } + return *this; + } + + /** show (plot) the current setup */ + void show() { + gp.draw(splot); + gp.flush(); + } + + +}; + +#endif // VIS_H diff --git a/code/frank/BeaconEvaluation.h b/code/frank/BeaconEvaluation.h new file mode 100755 index 0000000..996461b --- /dev/null +++ b/code/frank/BeaconEvaluation.h @@ -0,0 +1,108 @@ +#ifndef BEACONEVALUATION_H +#define BEACONEVALUATION_H + +#include +#include "BeaconObservation.h" +#include "Settings.h" +#include "../particles/MyState.h" +#include "../particles/MyObservation.h" +#include "PositionedBeacon.h" + +class BeaconEvaluation { + +private: + + Settings settings; + //BeaconObservation obs; + +public: + + double getProbability(const MyState& state, const MyObservation& observation) const { + + //if (obs.entries.empty()) {return 1.0;} + double prob = 1.0; + +// const double tx = -74; + const double waf = 20.0; + +// // get the ap the client had the strongest measurement for +// const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); +// const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; +// const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); +// const double mdlStrongestRSSI = distanceToRssi(tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); + + // process each detected beacon + for (const BeaconObservationEntry& entry : observation.beacons.entries) { + + // get the AP data from the settings + const PositionedBeacon* beacon = settings.getBeacon(entry.mac); + if (!beacon) {continue;} + + // distance (in meter) between particle and AP + const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0; + + // floor difference? + const double floorDist = std::abs(beacon->zNr - state.z_nr); + + // estimate the rssi depending on above distance + const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf); + + // the measured rssi + const double realRSSI = entry.rssi; + +// // the measured relative rssi +// const double realRelRSSI = strongest.rssi - realRSSI; +// const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI; + + // probability? (sigma grows with measurement's age) + const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 2.0; + const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI); + //const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); + + //prob *= p; + prob += std::log(p); + + } + + const double lambda = 0.15; + const double res = lambda * exp(- lambda * (-prob)); + return res; + //return prob; + + } + +// WiFiObservation filter(const WiFiObservation* obs) const { + +// WiFiObservation out; +// out.ts = obs->ts; +// for (const WiFiObservationEntry& entry : obs->entries) { +// // alter the mac +// WiFiObservationEntry ne = entry; +// ne.mac[ne.mac.length()-1] = '0'; +// if (settings.getAP(ne.mac)) {out.entries.push_back(ne);} +// } +// return out; + +// } + +// /** get the strongest AP within all measurements */ +// WiFiObservationEntry getStrongest(const WiFiObservation* obs) const { +// WiFiObservationEntry max = obs->entries.front(); +// for (const WiFiObservationEntry& entry : obs->entries) { +// if (entry.rssi > max.rssi) {max = entry;} +// } +// return max; +// } + + static double rssiToDistance(double txPower, double rssi, double pathLoss) { + return pow(10, (txPower - rssi) / (10 * pathLoss)); + } + + static double distanceToRssi(double txPower, double distance, double pathLoss) { + if (distance <= 1) {return txPower;} + return (txPower - (10 * pathLoss * log10(distance))); + } + +}; + +#endif // BEACONEVALUATION_H diff --git a/code/frank/BeaconObservation.h b/code/frank/BeaconObservation.h new file mode 100755 index 0000000..c80f307 --- /dev/null +++ b/code/frank/BeaconObservation.h @@ -0,0 +1,41 @@ +#ifndef BEACONOBSERVATION_H +#define BEACONOBSERVATION_H + +#include "MACAddress.h" +#include + + +/** one observed AP and its signal strength */ +struct BeaconObservationEntry { + + /** the timestamp this beacon was discovered at */ + uint64_t ts; + + /** the beacon's mac address */ + std::string mac; + + /** the beacon's rssi */ + int rssi; + + BeaconObservationEntry() : ts(0), mac(), rssi(0) {;} + BeaconObservationEntry(const uint64_t ts, const std::string& mac, const int rssi) : ts(ts), mac(mac), rssi(rssi) {;} + +}; + +/** all APs observed during one scan */ +struct BeaconObservation { + + std::vector entries; + + void removeOld(uint64_t latestTS) { + auto lambda = [latestTS] (const BeaconObservationEntry& e) { + uint64_t age = latestTS - e.ts; + return age > 1000*3; + }; + entries.erase(std::remove_if(entries.begin(), entries.end(), lambda), entries.end()); + } + +}; + + +#endif // BEACONOBSERVATION_H diff --git a/code/frank/BeaconSensorReader.h b/code/frank/BeaconSensorReader.h new file mode 100755 index 0000000..d921c0d --- /dev/null +++ b/code/frank/BeaconSensorReader.h @@ -0,0 +1,60 @@ +#ifndef BEACONSENSORREADER_H +#define BEACONSENSORREADER_H + +#include "../SensorReader.h" +#include "BeaconObservation.h" +#include "Settings.h" +#include + +class BeaconSensorReader { + +public: + +// /** get wifi observation data from one CSV entry */ +// static BeaconObservation* readBeacons(const SensorEntry& se) { + +// std::string tmp = se.data; +// BeaconObservation* obs = new BeaconObservation(); +// obs->ts = se.ts; + +// std::string mac = tmp.substr(0, 17); +// tmp = tmp.substr(17); +// assert(tmp[0] == ';'); tmp = tmp.substr(1); + +// std::string rssi = tmp; + +// BeaconObservationEntry e(mac, std::stoi(rssi)); +// obs->entries.push_back(e); + +// /** skip unknown beacons */ +// if (settings.getBeacon(mac) == nullptr) {return nullptr;} + +// return obs; + +// } + + /** get wifi observation data from one CSV entry */ + static BeaconObservationEntry getBeacon(const SensorEntry& se) { + + BeaconObservationEntry boe; + std::string tmp = se.data; + + std::string mac = tmp.substr(0, 17); + tmp = tmp.substr(17); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + std::string rssi = tmp; + + BeaconObservationEntry e(se.ts, mac, std::stoi(rssi)); + + /** skip unknown beacons */ + if (settings.getBeacon(mac) == nullptr) {return BeaconObservationEntry();} + + return e; + + } + +}; + + +#endif // BEACONSENSORREADER_H diff --git a/code/frank/MACAddress.h b/code/frank/MACAddress.h new file mode 100755 index 0000000..760dfd2 --- /dev/null +++ b/code/frank/MACAddress.h @@ -0,0 +1,131 @@ +#ifndef MACADDRESS_H +#define MACADDRESS_H + +#include +#include + +/** + * describe a MAC-Address as 64-bit integer + * or 8-bit access to all fields + */ +union MACAddressValue { + + struct { + uint8_t h5; + uint8_t h4; + uint8_t h3; + uint8_t h2; + uint8_t h1; + uint8_t h0; + }; + + uint64_t mac; + + /** initialize everything with zeros */ + MACAddressValue() : mac(0) {;} + +}; + +class MACAddress { + +private: + + /** the address as integer value */ + MACAddressValue value; + +public: + + /** empty ctor */ + MACAddress() { + ; + } + + /** copy ctor */ + MACAddress(const MACAddress& o) : value(o.value) { + ; + } + + /** ctor form string (e.g. "xx:xx:xx:xx:xx:xx") */ + MACAddress(const std::string& str) { + + // sanity check + if (str.size() != 17) {throw "invalid hex string length. must be 17";} + + value.mac = 0; // all zeros + value.h5 = hexWordToInt(str[ 0], str[ 1]); + value.h4 = hexWordToInt(str[ 3], str[ 4]); + value.h3 = hexWordToInt(str[ 6], str[ 7]); + value.h2 = hexWordToInt(str[ 9], str[10]); + value.h1 = hexWordToInt(str[12], str[13]); + value.h0 = hexWordToInt(str[15], str[16]); + + } + + /** convert to hex-string ("xx:xx:xx:xx:xx:xx") */ + std::string asString() { + + std::string str = ":::::::::::::::::"; + + intToHexStr(value.h5, &str[ 0]); + intToHexStr(value.h4, &str[ 3]); + intToHexStr(value.h3, &str[ 6]); + intToHexStr(value.h2, &str[ 9]); + intToHexStr(value.h1, &str[12]); + intToHexStr(value.h0, &str[15]); + + return str; + + } + + /** get the mac address as a long-int value */ + uint64_t asLong() const { + return value.mac; + } + + /** equal? */ + bool operator == (const MACAddress& o) const { + return o.asLong() == asLong(); + } + +private: + + /** convert the given hex char [0-F] to an integer [0-15] */ + static uint8_t hexCharToInt(char hex) { + + // to upper case + if (hex >= 'a') {hex -= 'a' - 'A';} + + // convert + return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10); + + } + + /** convert the given hex-word to an integer */ + static uint8_t hexWordToInt(char hi, char lo) { + return hexCharToInt(hi) << 4 | hexCharToInt(lo); + } + + /** conver the given integer [0-15] to a hex char [0-F] */ + static char intToHexChar(const uint8_t val) { + return (val < 10) ? ('0' + val) : ('A' - 10 + val); + } + + /** insert two hex chars into the provided string buffer */ + static void intToHexStr(const uint8_t val, char* dst) { + dst[0] = intToHexChar((val >> 4) & 0xF); + dst[1] = intToHexChar((val >> 0) & 0xF); + } + +}; + + +/** hash-method for MAC-Addresses */ +namespace std { + template <> struct hash { + std::size_t operator() (const MACAddress& mac) const { + return std::hash()(mac.asLong()); + } + }; +} + +#endif // MACADDRESS_H diff --git a/code/frank/Position3D.h b/code/frank/Position3D.h new file mode 100755 index 0000000..1248774 --- /dev/null +++ b/code/frank/Position3D.h @@ -0,0 +1,36 @@ +#ifndef POSITION3D_H +#define POSITION3D_H + +#include + +/** + * represents a 3D position (x,y,z) + */ +struct Position3D { + + /** x-position (in centimeter) */ + double xCM; + + /** y-position (in centimeter) */ + double yCM; + + /** floor number */ + int zNr; + + /** ctor */ + Position3D() : xCM(0), yCM(0), zNr(0) {;} + + /** ctor. x,y in centimeter, z = floor-number */ + Position3D(const double xCM, const double yCM, const int zNr) : xCM(xCM), yCM(yCM), zNr(zNr) {;} + + /** get the distance to the given position (in centimeter) */ + double getDistanceCM(const Position3D& p) const { + const double dx = xCM - p.xCM; + const double dy = yCM - p.yCM; + const double dz = (zNr - p.zNr) * 300; // 300 = average floor height (centimeter) + return std::sqrt(dx*dx + dy*dy + dz*dz); + } + +}; + +#endif // POSITION3D_H diff --git a/code/frank/PositionedBeacon.h b/code/frank/PositionedBeacon.h new file mode 100755 index 0000000..6a99df1 --- /dev/null +++ b/code/frank/PositionedBeacon.h @@ -0,0 +1,23 @@ +#ifndef POSITIONEDBEACON_H +#define POSITIONEDBEACON_H + +#include "WiFiAP.h" +#include "Position3D.h" + +class PositionedBeacon : public Position3D { + +public: + + MACAddress mac; + double tx; + double pl; + + /** ctor */ + PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) : + mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) { + ; + } + +}; + +#endif // POSITIONEDBEACON_H diff --git a/code/frank/PositionedWiFiAP.h b/code/frank/PositionedWiFiAP.h new file mode 100755 index 0000000..296503b --- /dev/null +++ b/code/frank/PositionedWiFiAP.h @@ -0,0 +1,20 @@ +#ifndef POSITIONEDWIFIAP_H +#define POSITIONEDWIFIAP_H + +#include "WiFiAP.h" +#include "Position3D.h" + + +class PositionedWifiAP : public WiFiAP, public Position3D { + +public: + + /** ctor */ + PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) : + WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) { + ; + } + +}; + +#endif // POSITIONEDWIFIAP_H diff --git a/code/frank/Settings.h b/code/frank/Settings.h new file mode 100755 index 0000000..74e07f3 --- /dev/null +++ b/code/frank/Settings.h @@ -0,0 +1,136 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include "PositionedWiFiAP.h" +#include "PositionedBeacon.h" +#include "MACAddress.h" + +#include + +class Settings { + +private: + + std::unordered_map aps; + std::unordered_map beacons; + +public: + + Settings() { + + const double pl = 2.7; + const double tx = -46; + + addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, 3, tx, pl-0.5); + addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, 3, tx, pl-0.5); + addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5); + addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl); + addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, 3, tx, pl); + + // 2nd floor (vague AP position) + addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, 2, tx, pl-0.5); + addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, 2, tx, pl); + addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, 2, tx, pl); + addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2, tx, pl-0.5); + addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 2, tx, pl-0.5); + + // 1st floor (vague AP position) + addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, 1, tx, pl); + addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1, tx, pl); + addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, 1, tx, pl-0.25); + addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1, tx, pl-0.25); + addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1, tx, pl); + addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl); + + // 0th floor (exact AP position) + addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, 0, tx, pl); + addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, 0, tx, pl); + addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, 0, tx, pl); + addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, 0, tx, pl); + addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, 0, tx, pl); + addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 0, tx, pl); // vague!! + + const int ibOff = +2; + const float ibPLE = 1.9; + addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, 3); // id:16 + addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2); // id:18 + addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1); // id:11 + addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0); // id:20 + + addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, 2); // idis switchboard + addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2); // idis outside + addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2); // idis toni + + addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, 1); // i.1.47 + addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1); // fachschaft + addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 1); // neben fachschaft + +// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3); +// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3); + addBeacon("78:A5:04:1F:91:08", -88, ibPLE, 1448, 4538, 3); + addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 2028, 4528, 3); + + } + + /** get the AP behind the given MAC (if any) */ + const PositionedWifiAP* getAP(const MACAddress& mac) const { + auto it = aps.find(mac); + if (it == aps.end()) {return nullptr;} + return (it->second); + } + + /** get the Beacon behind the given MAC (if any) */ + const PositionedBeacon* getBeacon(const MACAddress& mac) const { + auto it = beacons.find(mac); + if (it == beacons.end()) {return nullptr;} + return (it->second); + } + +private: + + /** add a new known AP */ + void addAP(const std::string& mac, const std::string& room, const double x_cm, const double y_cm, const int floor, const double tx, const double pl) { + std::string mac2 = mac; + //mac2[mac2.length()-1] = '9'; + PositionedWifiAP* pap = new PositionedWifiAP(MACAddress(mac2), room, tx, pl, x_cm, y_cm, floor); + aps[mac2] = pap; + } + + /** add a new known Beacon */ + void addBeacon(const std::string& mac, const double tx, const double pl, const double x_cm, const double y_cm, const int floor) { + PositionedBeacon* pap = new PositionedBeacon(MACAddress(mac), tx, pl, x_cm, y_cm, floor); + beacons[mac] = pap; + } + +// // access points +// PositionedWifiAP aps[] = { + +//// // 3rd floor (excat AP position) +//// PositionedWifiAP(MACAddress("00:04:96:6b:64:90"), "i.3.20", 290, 1300, 3), +//// PositionedWifiAP(MACAddress("00:04:96:6b:70:c0"), "i.3.25", 290, 3930, 3), +//// PositionedWifiAP(MACAddress("00:04:96:6b:82:70"), "i.3.16", 1860, 3400, 3), +//// PositionedWifiAP(MACAddress("00:04:96:77:ed:f0"), "i.3.39", 4700, 4850, 3), +//// PositionedWifiAP(MACAddress("00:04:96:77:ed:60"), "i.3.3", 6460, 3400, 3), + +//// // 2nd floor (vague AP position) +//// PositionedWifiAP(MACAddress("00:04:96:6c:3a:a9"), "I.2.1", 6300, 3600, 2), +//// PositionedWifiAP(MACAddress("00:04:96:6b:bf:89"), "I.2.8", 3300, 3500, 2), +//// PositionedWifiAP(MACAddress("00:04:96:77:ec:a9"), "I.2.15", 300, 1300, 2), +//// PositionedWifiAP(MACAddress("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2), +//// PositionedWifiAP(MACAddress("00:04:96:6b:db:69"), "I.2.34", 4400, 4800, 2), + +//// // 1st floor (vague AP position) +//// PositionedWifiAP(MACAddress("00:04:96:6c:cf:19"), "I.1.2", 5700, 3500, 1), +//// PositionedWifiAP(MACAddress("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1), +//// PositionedWifiAP(MACAddress("00:04:96:69:48:89"), "I.1.17", 1500, 300, 1), +//// PositionedWifiAP(MACAddress("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1), +//// PositionedWifiAP(MACAddress("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1), +//// PositionedWifiAP(MACAddress("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1), + +// }; + +}; + +extern Settings settings; + +#endif // SETTINGS_H diff --git a/code/frank/WiFiAP.h b/code/frank/WiFiAP.h new file mode 100755 index 0000000..9337a4e --- /dev/null +++ b/code/frank/WiFiAP.h @@ -0,0 +1,44 @@ +#ifndef WIFIAP_H +#define WIFIAP_H + +#include "MACAddress.h" +#include + + +/** + * represents a WiFi-AccessPoint + * an AP is represented by its MAC-Address and + * may provide a readably SSID + */ +class WiFiAP { + +public: + + /** the AP's MAC-Address */ + MACAddress mac; + + /** the AP's readable SSID */ + std::string ssid; + + double tx; + + /** path loss for this ap. for testing */ + double pl; + + + +public: + + /** ctor */ + WiFiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) { + ; + } + + /** ctor */ + WiFiAP(const std::string& mac, const std::string& ssid, const double tx, const double pl) : mac(mac), ssid(ssid), tx(tx), pl(pl) { + ; + } + +}; + +#endif // WIFIAP_H diff --git a/code/frank/WiFiEvaluation.h b/code/frank/WiFiEvaluation.h new file mode 100755 index 0000000..85d715b --- /dev/null +++ b/code/frank/WiFiEvaluation.h @@ -0,0 +1,120 @@ +#ifndef WIFIEVALUATION_H +#define WIFIEVALUATION_H + +#include "../particles/MyState.h" +#include "WiFiObservation.h" +#include "PositionedWiFiAP.h" +#include "Settings.h" +#include "../particles/MyObservation.h" + +#include + + + +class WiFiEvaluation { + +private: + + Settings settings; + WiFiObservation obs; + WiFiObservationEntry strongest; + +public: + + void nextObservation(const WiFiObservation& _obs) { + + if (_obs.entries.empty()) {return;} + + obs = filter(_obs); + strongest = getStrongest(&obs); + + } + + double getProbability(const MyState& state, const MyObservation& observation) const { + + if (obs.entries.empty()) {return 1.0;} + double prob = 0;//1.0; + + //const double tx = -48; // tablet + //const double pl = 3.15; + const double waf = 7;//10.0; + const double floor_height_cm = 350; + + // get the ap the client had the strongest measurement for + const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); + const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; + const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); + const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); + + // process each detected AP + for (const WiFiObservationEntry& entry : obs.entries) { + + // get the AP data from the settings + const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap); + + // distance (in meter) between particle and AP + const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0; + + // floor difference? + const double floorDist = std::abs(ap->zNr - state.z_nr); + + // estimate the rssi depending on above distance + const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDist * waf); + + // the measured rssi + const double realRSSI = entry.rssi; + + // the measured relative rssi + const double realRelRSSI = strongest.rssi - realRSSI; + const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI; + + // probability? (sigma grows with measurement's age) + const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0; + const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI); // absolute + //const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI); // relative + + //prob *= p; + prob += std::log(p); + + } + + const double lambda = 0.25; //0.12; + return lambda * exp(- lambda * (-prob)); + //return prob; + + } + + WiFiObservation filter(const WiFiObservation& obs) const { + + WiFiObservation out; + for (const WiFiObservationEntry& entry : obs.entries) { + // alter the mac + WiFiObservationEntry ne = entry; + //ne.mac[ne.mac.length()-1] = '0'; // enabled = VAP grouping. also adjust settings to use ending "0" + if (settings.getAP(ne.mac)) {out.entries.push_back(ne);} + } + return out; + + } + + /** get the strongest AP within all measurements */ + WiFiObservationEntry getStrongest(const WiFiObservation* obs) const { + WiFiObservationEntry max = obs->entries.front(); + for (const WiFiObservationEntry& entry : obs->entries) { + if (entry.rssi > max.rssi) {max = entry;} + } + return max; + } + + static double rssiToDistance(double txPower, double rssi, double pathLoss) { + return pow(10, (txPower - rssi) / (10 * pathLoss)); + } + + static double distanceToRssi(double txPower, double distance, double pathLoss) { + if (distance <= 1) {return txPower;} + return (txPower - (10 * pathLoss * log10(distance))); + } + +}; + +#endif // WIFIEVALUATION_H diff --git a/code/frank/WiFiHelper.h b/code/frank/WiFiHelper.h new file mode 100755 index 0000000..42e7ffe --- /dev/null +++ b/code/frank/WiFiHelper.h @@ -0,0 +1,4 @@ +#ifndef WIFIHELPER_H +#define WIFIHELPER_H + +#endif // WIFIHELPER_H diff --git a/code/frank/WiFiObservation.h b/code/frank/WiFiObservation.h new file mode 100755 index 0000000..81493fd --- /dev/null +++ b/code/frank/WiFiObservation.h @@ -0,0 +1,23 @@ +#ifndef WIFIOBSERVATION_H +#define WIFIOBSERVATION_H + +#include "MACAddress.h" +#include + +/** one observed AP and its signal strength */ +struct WiFiObservationEntry { + uint64_t ts; + std::string mac; + int freq; + int rssi; + WiFiObservationEntry() {;} + WiFiObservationEntry(const uint64_t ts, const std::string& mac, const int freq, const int rssi) : ts(ts), mac(mac), freq(freq), rssi(rssi) {;} +}; + +/** all APs observed during one scan */ +struct WiFiObservation { + + std::vector entries; +}; + +#endif // WIFIOBSERVATION_H diff --git a/code/frank/WiFiSensorReader.h b/code/frank/WiFiSensorReader.h new file mode 100755 index 0000000..aa3dc41 --- /dev/null +++ b/code/frank/WiFiSensorReader.h @@ -0,0 +1,46 @@ +#ifndef WIFISENSORREADER_H +#define WIFISENSORREADER_H + +#include "../SensorReader.h" +#include "WiFiObservation.h" + +#include + +class WiFiSensorReader { + +public: + + /** get wifi observation data from one CSV entry */ + static WiFiObservation readWifi(const SensorEntry& se) { + + std::string tmp = se.data; + WiFiObservation obs; + + // process all APs + while(!tmp.empty()) { + + std::string mac = tmp.substr(0, 17); + tmp = tmp.substr(17); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + std::string freq = tmp.substr(0, 4); + tmp = tmp.substr(4); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + int pos = tmp.find(';'); + std::string rssi = tmp.substr(0, pos); + tmp = tmp.substr(pos); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + WiFiObservationEntry e(se.ts, mac, std::stoi(freq), std::stoi(rssi)); + obs.entries.push_back(e); + + } + + return obs; + + } + +}; + +#endif // WIFISENSORREADER_H diff --git a/code/lukas/ReadMe.txt b/code/lukas/ReadMe.txt new file mode 100755 index 0000000..ecf47a3 --- /dev/null +++ b/code/lukas/ReadMe.txt @@ -0,0 +1,14 @@ +Python Skripte: +StepDetector.py TurnDetector.py + +Benötigt wird Python2.7, scipy und numpy, sowie zum plotten matplotlib + +Benötigte Parameter: +1. Input-Datei +2. Output-Datei + +Beispiel: +python StepDetector.py ./FH_Sensor.csv Steps.txt +python TurnDetector.py ./FH_Sensor.csv Turns.txt + +Weitere optimale Parameter mit -h aufrufbar diff --git a/code/lukas/StepDetector.py b/code/lukas/StepDetector.py new file mode 100755 index 0000000..a7a8aa9 --- /dev/null +++ b/code/lukas/StepDetector.py @@ -0,0 +1,278 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.signal import argrelmax +import sys +import math +import argparse + + +def rotate_data_fhws(data, data_t, rotation, rotation_t): + #Invert rotationmatrix + np.linalg.inv(rotation) + + #Align rotation time according to data time + tmp = [] + for t in data_t: + # Find indices of roation matrix that are earlier + #than the current time of the sensor value + ind = np.where(rotation_t <= t)[0] + + #Use the last index + if len(ind) != 0: + tmp.append(ind[-1]) + else: + tmp.append(0) + + #Only use the values of the rotation matrix that are aligned with the sensor data + rotation = rotation[tmp] + + # Multiply data with rotationmatrix + rot_data = [] + for i, row in enumerate(data): + row = np.append(row, 1) + rot_data.append(np.dot(rotation[i], row)) + + return np.array(rot_data) + + + + +def rotate_data_lukas(data, rotation): + #Invert rotationmatrix + np.linalg.inv(rotation) + + rot_data = [] + for i, row in enumerate(data): + row = np.append(row, 1) + rot_data.append(np.dot(rotation[i], row)) + + return np.array(rot_data) + +def magnitude(x, y, z): + ret = [math.sqrt(i) for i in (x**2 + y**2 + z**2)] + mean = np.mean(ret) + ret -= mean + return ret + +def count_steps(time, signal, lt, ht, dead): + """ + Find steps in the accelerometer signal. + After a step was found, a "dead" period exists, where no step can be found again. + This is to avoid too many steps + + Parameters + ---------- + time: array_like + Timestaps of accelerometer signal + Must have same length as signal + signal: array_like + Accelerometer signal of all three axis. + Must have same length as time + lt: float + Low threshold, which must be exceeded by the accelerometer signal to be counted as step + ht: float + High treshold, which must not be exceeded by the accelerometer signal to be counted as step + dead: float + After a step was detected, during the dead time no other step will be found. + Given in milliseconds + """ + + time_signal = zip(time, signal) + dead_time = 0 + + steps = [] + for i in time_signal: + if lt < i[1] < ht and i[0] > dead_time: + steps.append(i[0]) + dead_time = i[0] + dead + + return np.array(steps) + + + + +def write_steps_to_file(fname, steps): + f = open(fname, 'w') + + print steps + for s in steps: + f.write(str(s) + "\n") + + f.close() + +def plot_steps(time, signal, steps): + plt.title("Step detection") + plt.xlabel("ms") + plt.ylabel("Accelerometer magnitude") + plt.plot(time, signal, label="Accelerometer") + + s = [] + for i,t in enumerate(time): + if t in steps: + s.append((t, signal[i])) + + s = np.array(s) + plt.plot(s[:,0], s[:,1], 'ro', label = "Steps") + + + + plt.legend(numpoints=1) + plt.show() + + + +def read_data(fname): + time = np.loadtxt(fname, + delimiter=";", + usecols=[0], + unpack=True) + + f = open(fname, 'r') + + accls = [] + accls_t = [] + rotations = [] + rotations_t = [] + start = time[0] + + for line in f: + line = line.split(";") + t = int(line[0]) - start + + #Lin Accel + if line[1] == "2": + accls_t.append(t) + accls.append((line[2], line[3], line[4])) + + #Rotation + elif line[1] == "7": + rotations_t.append(t) + rotations.append((line[2], line[3], line[4], line[5], + line[6], line[7], line[8], line[9], + line[10], line[11], line[12],line[13], + line[14], line[15], line[16], line[17])) + + + accls = np.array(accls, dtype=float) + accls_t = np.array(accls_t, dtype=int) + + rotations = np.array(rotations, dtype=float) + rotations = [row.reshape((4,4)) for row in rotations] + rotations = np.array(rotations) + rotations_t = np.array(rotations_t, dtype=int) + + return accls, accls_t, rotations, rotations_t + + +def main(): + + parser = argparse.ArgumentParser() + parser.add_argument("fname_sensor", + help = "Accelerometer file") + parser.add_argument("fname_output", + help = "Output file, where timestamps of steps will be saved") + parser.add_argument("--dead", + help = "Time span (in ms) after a detected step in which no additional step will be detected (default=600)", + type=int) + parser.add_argument("--lt", + help = "Low threshold, which must be exceeded by the accelerometer signal to be counted as step (default=1.5)", + type=float) + parser.add_argument("--ht", + help = "High treshold, which must not be exceeded by the accelerometer signal to be counted as step(default=6.5)", + type=float) + parser.add_argument("--plot", + help = "Plot step detection", + action="store_true") + parser.add_argument("--file_format", + help = "Sensor data file format [fhws|lukas] (default: fhws)", + type = str) + + args = parser.parse_args() + + file_format = "fhws" + + if args.file_format: + file_format = args.file_format + + #My own data format + if file_format == "lukas": + + delimiter = ',' + time_cols = [40] + accel_cols = [6,7,8] + + time = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=time_cols, + skiprows=2, + unpack=True) + + accelX, accelY, accelZ = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=accel_cols, + skiprows=2, + unpack=True) + + rotation = np.loadtxt(args.fname_sensor, + delimiter = delimiter, + usecols=range(18,34), + skiprows=1, + unpack=True) + + rotations = rotation.T + rotations = [row.reshape((4,4)) for row in rotations] + accl = np.array([accelX, accelY, accelZ]).T + world_accl = rotate_data_lukas(accl, rotations) + + #FHWS file format + else: + accls, time, rotation, rotation_t = read_data(args.fname_sensor) + world_accl = rotate_data_fhws(accls, time, rotation, rotation_t) + + accelX = world_accl[:,0] + accelY = world_accl[:,1] + accelZ = world_accl[:,2] + + accel_mag = magnitude(accelX, accelY, accelZ) + + lt = 1.5 + ht = 6.5 + dead = 600 + + if args.dead: + dead = args.dead + if args.lt: + lt = args.lt + if args.ht: + ht = args.ht + + steps = count_steps(time, accel_mag, lt, ht, dead) + print("#Steps detected: ", len(steps)) + write_steps_to_file(args.fname_output, steps) + + if args.plot: + plot_steps(time, accel_mag, steps) + + +if __name__ == "__main__": + main() + + + + + + + + + + + + + + + + + + + + diff --git a/code/lukas/StepEvaluation.h b/code/lukas/StepEvaluation.h new file mode 100755 index 0000000..c7f4932 --- /dev/null +++ b/code/lukas/StepEvaluation.h @@ -0,0 +1,51 @@ + +#ifndef STEPEVALUATION_H +#define STEPEVALUATION_H + + + +#include "../particles/MyState.h" +#include "StepObservation.h" +#include + +static double mu_walk = 40; +static double sigma_walk = 15; +static double mu_stop = 0; +static double sigma_stop = 5; + +class StepEvaluation { + +public: + + double getProbability(const MyState& state, const StepObservation* obs) const { + + double distance = state.distanceWalkedCM; + + double a = 1.0; + double mu_distance = 0; //cm + double sigma_distance = 10.0; //cm + + if(obs->step) { + a = 1.0; + mu_distance = mu_walk;//80.0; //cm + sigma_distance = sigma_walk;//40.0; //cm + } + + else { + a = 0.0; + mu_distance = mu_stop; //cm + sigma_distance = sigma_stop; //cm + } + + //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step + const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) + + (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance); + + return p; + } + +}; + + + +#endif // STEPEVALUATION_H diff --git a/code/lukas/StepObservation.h b/code/lukas/StepObservation.h new file mode 100755 index 0000000..7a57add --- /dev/null +++ b/code/lukas/StepObservation.h @@ -0,0 +1,14 @@ +#ifndef STEPOBSERVATION_H +#define STEPOBSERVATION_H + +struct StepObservation { + float ts; + bool step; + + StepObservation() {;} + StepObservation(const float ts) : ts(ts), step(false){;} + +}; + + +#endif // STEPOBSERVATION_H diff --git a/code/lukas/StepReader.h b/code/lukas/StepReader.h new file mode 100755 index 0000000..893a299 --- /dev/null +++ b/code/lukas/StepReader.h @@ -0,0 +1,25 @@ +#ifndef STEPREADER_H +#define STEPREADER_H + +#endif // STEPREADER_H + +#include "../SensorReaderStep.h" + +class StepReader { +public: + static StepObservation* readStep(const SensorEntryStep& se) { + + std::string tmp = se.data; + StepObservation* obs = new TurnObservation(); + + while(!tmp.empty()) { + std::string angle = tmp; + + StepObservation t(std::stof(angle)); + } + + return obs; + } + + +}; diff --git a/code/lukas/TurnDetector.py b/code/lukas/TurnDetector.py new file mode 100755 index 0000000..6e31953 --- /dev/null +++ b/code/lukas/TurnDetector.py @@ -0,0 +1,445 @@ +import numpy as np +import sys +import scipy.integrate +import math +import argparse +from sklearn.decomposition import PCA +import scipy.signal as signal + + +def project(v1, v2): + """ + Project vector v1 on v2 + Return projected vector + """ + + p = [np.dot(a, g) / np.dot(g,g) for a,g in zip(v1, v2)] + p = np.array(p) + + p = [p*g for p,g in zip(p, v2)] + p = np.array(p) + + return p + + +def motion_axis(time, lin_accel, gravity, interval = 500): + """ + Returns the motion axis, which is the axis with the biggest variance + lin_accel -- Linear acceleration + gravity -- Gravity + + Lin_accel and gravity should have equal length + """ + + p = project(lin_accel, gravity) + + #add time to vector p + p = np.array([time, p[:,0], p[:,1], p[:,2]]).T + + start = 0 + end = start + interval + end_time = p[:,0][-1] #last timestep + + pca = PCA(n_components=1) + + result = [] + while start < end_time: + indices = np.where((p[:,0] >= start) & (p[:,0] < end)) + Z = p[indices, 1:3][0] + Z[:,0] = signal.medfilt(Z[:,0],31) + Z[:,1] = signal.medfilt(Z[:,1],31) + + pca.fit(Z) + + x1 = pca.components_[0][0] + y1 = pca.components_[0][1] + + result.append((end, x1, y1)) + + start += interval + end += interval + + return np.array(result) + + + +def angle_between(v1, v2): + + l_a = np.linalg.norm(v1) + l_b = np.linalg.norm(v2) + cos_ab = np.dot(v1, v2 / (l_a * l_b)) + angle = np.arccos(cos_ab) * 180/math.pi + + return min([180 - angle, angle]) + + + +def rotate_data_fhws(data, data_t, rotation, rotation_t): + #Invert rotationmatrix + np.linalg.inv(rotation) + + #Align rotation time according to data time + tmp = [] + for t in data_t: + # Find indices of roation matrix that are earlier + #than the current time of the sensor value + ind = np.where(rotation_t <= t)[0] + + #Use the last index + if len(ind) != 0: + tmp.append(ind[-1]) + else: + tmp.append(0) + + #Only use the values of the rotation matrix that are aligned with the sensor data + rotation = rotation[tmp] + + # Multiply data with rotation matrix + rot_data = [] + for i, row in enumerate(data): + row = np.append(row, 1) + rot_data.append(np.dot(rotation[i], row)) + + return np.array(rot_data) + + + + +def rotate_data_lukas(data, rotation): + #Invert rotationmatrix + np.linalg.inv(rotation) + + rot_data = [] + for i, row in enumerate(data): + row = np.append(row, 1) + rot_data.append(np.dot(rotation[i], row)) + + return np.array(rot_data) + + + + +def read_data(fname): + """ + Read the data out of the file provided by FHWS sensor reader app + """ + + + time = np.loadtxt(fname, + delimiter=";", + usecols=[0], + unpack=True) + + + f = open(fname, 'r') + + lin_accel = [] + gyros = [] + rotations = [] + gravity = [] + start = time[0] + time = [] + + gyro_tmp = [0, 0, 0] + lin_accel_tmp = [0, 0, 0] + gravity_tmp = [0, 0, 9.81] + rotations_tmp = 16*[-1] + + s = 0 + for line in f: + line = line.split(";") + t = int(line[0]) - start + + + #Gyro-Data + if line[1] == "3": + gyro_tmp[0] = line[2] + gyro_tmp[1] = line[3] + gyro_tmp[2] = line[4] + + + #Linear Acceleration-Data + elif line[1] == "2": + lin_accel_tmp[0] = line[2] + lin_accel_tmp[1] = line[3] + lin_accel_tmp[2] = line[4] + + + #Gravity data + elif line[1] == "1": + gravity_tmp[0] = line[2] + gravity_tmp[1] = line[3] + gravity_tmp[2] = line[4] + + + #Rotation-Data + elif line[1] == "7": + for i in range(0,16): + rotations_tmp[i] = line[i+2] + + + if s != t: + gyros.append(gyro_tmp[:]) + lin_accel.append(lin_accel_tmp[:]) + gravity.append(gravity_tmp[:]) + rotations.append(rotations_tmp[:]) + time.append(t) + s = t + + + gyros = np.array(gyros, dtype=float) + lin_accel = np.array(lin_accel, dtype=float) + gravity = np.array(gravity, dtype=float) + rotations = np.array(rotations, dtype=float) + time = np.array(time, dtype = int) + + #HACK + #In the first timestamps the rotation matrix is all zero, because + #no measurements are available yet. + #Avoid this by replacing these lines with the first measured + #rotation matrix + ind = np.where(rotations[:,0] == -1)[0] + if len(ind) != 0: + index = ind[-1] + 1 + rotations[ind] = rotations[index] + + #Reshape matrix + rotations = [row.reshape((4,4)) for row in rotations] + rotations = np.array(rotations) + + + return time, gyros, lin_accel, gravity, rotations + + + +def detect_turns(time, signal, interval): + + n_intervals = int(time[-1] / interval) + 1 + result = [] + + for i in range(n_intervals): + start = i * interval + end = start + interval + + tmp = integrate(start, end, zip(time, signal)) * 180.0/math.pi + result.append((end, tmp)) + + return np.array(result) + + + +def integrate(time_from, time_to, signal): + """Integrate signal from time_from to time_to. Signal should be two dimensional. + First dimension is the timestamp, second dimension is the signal value. + dt is the interval between two recordings + """ + sum = 0 + last_time = 0 + + #go through signal + for value in signal: + #check if time is in the given timespan + if time_from <= value[0] < time_to: + #multiply value with dt and add it to the sum = integral +# sum += value[1] * dt + sum += value[1] * ((value[0] - last_time)/1000.) + last_time = value[0] + + #sum is the integral over rad/s + return sum + + + +def write_to_file(fname, turns, motion): + f = open(fname, 'w') + + for index, t in enumerate(turns): + f.write(str(t[0]) + "," + str(t[1]) + "," + str(motion[index][1]) + "\n") + + f.close() + + + +def deg_to_rad(deg): + return deg * math.pi / 180.0 + + +def rad_to_deg(rad): + return rad * 180.0 / math.pi + + + +def main(): + + parser = argparse.ArgumentParser() + + parser.add_argument("fname_sensor", + help = "Gyroscope file") + + parser.add_argument("fname_output", + help = "Output file, where timestamps and angle of heading will be saved") + + parser.add_argument("--time", + help = "Time interval, over which gyroscope will be integrated (default=500ms)", + type=int) + + parser.add_argument("--rad", + help = "Output angles in rad (default in degree)", + action = "store_true") + + parser.add_argument("--file_format", + help = "Sensor data file format [fhws|lukas] (default: fhws)", + type = str) + + parser.add_argument("--cosy", + help = "Coordinate system of the gyroscope data [world|device] (default: device). If given in device, the data will automatically be rotated in world coordinates.", + type = str) + + + args = parser.parse_args() + + + #Choose between file format of sensor data and coordinate system + file_format = "fhws" + cosy = "device" + + if args.file_format: + file_format = args.file_format + + if args.cosy: + cosy = args.cosy + + + #My own data format + if file_format == "lukas": + delimiter = "," + time_cols = [40] + + time = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=time_cols, + skiprows = 1, + unpack=True) + + if cosy == "device": + gyros_cols = [9, 10, 11] + lin_accel_cols = [6, 7, 8] + else: + gyros_cols = [34, 35,36] + lin_accel_cols = [37, 38, 39] + + grav_cols = [3, 4, 5] + + + gyroX, gyroY, gyroZ = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=gyros_cols, + skiprows = 1, + unpack=True) + + rotation = np.loadtxt(args.fname_sensor, + delimiter = delimiter, + usecols=range(18,34), + skiprows=1, + unpack=True) + + lin_accel_X, lin_accel_Y, lin_accel_Z = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=lin_accel_cols, + skiprows=1, + unpack=True) + + gravity_X, gravity_Y, gravity_Z = np.loadtxt(args.fname_sensor, + delimiter=delimiter, + usecols=grav_cols, + skiprows=1, + unpack=True) + + rotation = rotation.T + rotation = [row.reshape((4,4)) for row in rotation] +# rotation = np.array(rotation).T + print rotation + + gyro = np.array([gyroX, gyroY, gyroZ]).T + lin_accel = np.array([lin_accel_X, lin_accel_Y, lin_accel_Z]).T + gravity = np.array([gravity_X, gravity_Y, gravity_Z]).T + + if cosy == "device": + world_gyro = rotate_data_lukas(gyro, rotation) + world_lin_accel = rotate_data_lukas(lin_accel, rotation) + else: + world_gyro = gyro + world_lin_accel = lin_accel + + + #FHWS file format + else: + time, gyro, lin_accel, gravity, rotation = read_data(args.fname_sensor) + + if cosy == "device": + world_gyro = rotate_data_lukas(gyro, rotation) + world_lin_accel = rotate_data_lukas(lin_accel, rotation) + else: + print "Option 'fhws' in combination with 'world' not available" + return + + + gyroX = world_gyro[:,0] + gyroY = world_gyro[:,1] + gyroZ = world_gyro[:,2] + + lin_accel_X = world_lin_accel[:,0] + lin_accel_Y = world_lin_accel[:,1] + lin_accel_Z = world_lin_accel[:,2] + + + #Parameters + #--------- + + time_interval = 500 + + if args.time: + time_interval = args.time + + + turns = detect_turns(time, gyroZ, time_interval) + motion = motion_axis(time, lin_accel, gravity, 500) + + angles = [] + for index, axis in enumerate(motion): + if index == 0: + angle = 0 + else: + x_1 = motion[index-1][1] + y_1 = motion[index-1][2] + x_2 = axis[1] + y_2 = axis[2] + + a = np.array([x_1, y_1]) + b = np.array([x_2, y_2]) + + angle = angle_between(a,b) + angles.append((axis[0], angle)) + + + np.set_printoptions(suppress=True) + turns = np.array(turns) + angles = np.array(angles) + + + if args.rad: + turns[:,1] = deg_to_rad(turns[:,1]) + + print "Sum of all angles: ", np.sum(turns[:,1]) + write_to_file(args.fname_output, turns, angles) + + + +if __name__ == "__main__": + main() + + + + + + diff --git a/code/lukas/TurnEvaluation.h b/code/lukas/TurnEvaluation.h new file mode 100755 index 0000000..26df7ee --- /dev/null +++ b/code/lukas/TurnEvaluation.h @@ -0,0 +1,76 @@ +#ifndef TURNEVALUATION_H +#define TURNEVALUATION_H + + +#include "../particles/MyState.h" +#include "TurnObservation.h" +#include +#include + +static double sigma_heading = 35; + +class TurnEvaluation { + + //All calculations use degree not rad!!! + +public: + + double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const { + + //Particle's heading change + double delta_heading_particle = state.heading - state.heading_old; + + + //Correct offset of the heading change + if (delta_heading_particle < -180) { + delta_heading_particle += 360; + } + else if (delta_heading_particle > 180) { + delta_heading_particle -= 360; + } + + + //Switch between simple and improved evaluation + //"Simple" only evaluates the deviation between the measured heading and the particle heading change using + //normal distribution + if(simple) { + + double sigma_delta_heading = sigma_heading; + + const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle); + + + return p; + } + + //use the von Mises distribution + else { + //Here some calculations must be done in rad + + double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0; + double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0; + + //Equation for estimating kappa value of von Mises distribution + //empirically estimated + double kappa = 0.0; + + kappa = 5.0 / exp(2 * delta_motion_rad); + + double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0; + + + + //pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution) + const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa)); + + return p; + + } + + } + +}; + + + +#endif // TURNEVALUATION_H diff --git a/code/lukas/TurnObservation.h b/code/lukas/TurnObservation.h new file mode 100755 index 0000000..aa06198 --- /dev/null +++ b/code/lukas/TurnObservation.h @@ -0,0 +1,21 @@ +#ifndef TURNOBSERVATION_H +#define TURNOBSERVATION_H + +#include + + + +struct TurnObservation { + float ts; + float delta_heading; //measured change of heading direction (given by Gyroskop) + float delta_motion; //measured change of motion direction (given by PCA) + + TurnObservation() {;} + TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(delta_motion) {;} + +}; + + + + +#endif // TURNOBSERVATION_H diff --git a/code/lukas/TurnReader.h b/code/lukas/TurnReader.h new file mode 100755 index 0000000..653a339 --- /dev/null +++ b/code/lukas/TurnReader.h @@ -0,0 +1,36 @@ +#ifndef TURNREADER_H +#define TURNREADER_H + +#include "../SensorReaderTurn.h" +#include "TurnObservation.h" + +class TurnReader { +public: + static TurnObservation* readTurn(const SensorEntryTurn& se) { + + std::string tmp = se.data; + TurnObservation* obs = new TurnObservation(); + + while(!tmp.empty()) { + + int pos = tmp.find(','); + std::string heading = tmp.substr(0, pos); + tmp = tmp.substr(pos); + assert(tmp[0] == ';'); tmp = tmp.substr(1); + + std::string motion = tmp; + + TurnObservation t(std::stof(heading), std::stof(motion)); + + + + } + + return obs; + } + + +}; + + +#endif // TURNREADER_H diff --git a/code/lukas/detection.sh b/code/lukas/detection.sh new file mode 100755 index 0000000..ff77b9e --- /dev/null +++ b/code/lukas/detection.sh @@ -0,0 +1,21 @@ +#!/bin/bash +FILES=$(find ../measurements/18/{Galaxy,Nexus}/ -name "*.csv") + +for f in $FILES +do + echo $f + filename=$(basename $f) + directory=$(dirname $f) + + #echo $filename + #echo $directory + + step=$directory/Steps2.txt + turn=$directory/Turns.txt + + echo $step + echo $turn + + python StepDetector.py $f $step --lt -1.2 --ht 1.2 + python TurnDetector.py $f $turn +done diff --git a/code/main.cpp b/code/main.cpp new file mode 100644 index 0000000..76cd605 --- /dev/null +++ b/code/main.cpp @@ -0,0 +1,58 @@ +#include +#include + +#include "Vis.h" + +namespace Settings { + const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg"; + const int gridSize_cm = 200; +} + +struct MyNode : public GridNode, public GridPoint { +public: + MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;} +}; + + +int align(const int val) { + return val / Settings::gridSize_cm * Settings::gridSize_cm; +} + +int main(void) { + + Grid grid(Settings::gridSize_cm); + GridFactory gridFac(grid); + + + FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222); + + Floor f0 = fpFac.getFloor("floor_0"); + Floor f1 = fpFac.getFloor("floor_1"); + Floor f2 = fpFac.getFloor("floor_2"); + Floor f3 = fpFac.getFloor("floor_3"); + + Stairs f01 = fpFac.getStairs("staircase_0_1"); + Stairs f12 = fpFac.getStairs("staircase_1_2"); + Stairs f23 = fpFac.getStairs("staircase_2_3"); + + const LengthF h0 = LengthF::cm(align(0)); + const LengthF h1 = LengthF::cm(align(360)); + const LengthF h2 = LengthF::cm(align(360+340)); + const LengthF h3 = LengthF::cm(align(360+340+340)); + + gridFac.addFloor(f0, h0.cm()); + gridFac.addFloor(f1, h1.cm()); + gridFac.addFloor(f2, h2.cm()); + gridFac.addFloor(f3, h3.cm()); + //gridFac.removeIsolated(); + + Vis vis; + vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3); + vis.addGrid(grid); + vis.show(); + + sleep(1000); + + return 0; + +} diff --git a/code/plan.svg b/code/plan.svg new file mode 100755 index 0000000..71adfcd --- /dev/null +++ b/code/plan.svg @@ -0,0 +1,4645 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + i.2.1 + + i.2.8 + + i.2.15 + + i.2.19 + + i.2.34 + 00:04:96:6c:3a:a9 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/reader/SensorReader.h b/code/reader/SensorReader.h new file mode 100755 index 0000000..c2c0b28 --- /dev/null +++ b/code/reader/SensorReader.h @@ -0,0 +1,66 @@ +#ifndef SENSORREADER_H +#define SENSORREADER_H + +#include + +/** entry for one sensor */ +struct SensorEntry { + + /** timestamp of occurrence */ + uint64_t ts; + + /** sensor's number */ + int idx; + + /** sensor data */ + std::string data; + +}; + +/** read sensor data from CSV */ +class SensorReader { + +private: + + std::string file; + std::ifstream fp; + +public: + + SensorReader(const std::string& file) : file(file) { + rewind(); + } + + bool hasNext() { + return !fp.bad() && !fp.eof(); + } + + /** read the next sensor entry */ + SensorEntry getNext() { + + char delim; + SensorEntry entry; + + fp >> entry.ts; + fp >> delim; + fp >> entry.idx; + fp >> delim; + fp >> entry.data; + + return entry; + + + } + + /** start again */ + void rewind() { + fp.close(); + fp.open(file); + assert(fp.is_open()); + } + + +}; + + +#endif // SENSORREADER_H diff --git a/code/reader/SensorReaderStep.h b/code/reader/SensorReaderStep.h new file mode 100755 index 0000000..38a914c --- /dev/null +++ b/code/reader/SensorReaderStep.h @@ -0,0 +1,51 @@ +#ifndef SENSORREADERSTEP_H +#define SENSORREADERSTEP_H + +#include + +/** entry for one sensor */ +struct SensorEntryStep { + + /** sensor data */ + float ts; //timestamp of the step + + + +}; + +/** read sensor data from CSV */ +class SensorReaderStep { + +private: + + std::ifstream fp; + +public: + + SensorReaderStep(const std::string& file) { + fp.open(file); + assert(fp.is_open()); + } + + bool hasNext() { + return !fp.bad() && !fp.eof(); + } + + /** read the next sensor entry */ + SensorEntryStep getNext() { + + char delim; + SensorEntryStep entry; + + fp >> entry.ts; + + int i = 0; + return entry; + + + } + + +}; + +#endif // SENSORREADERSTEP_H diff --git a/code/reader/SensorReaderTurn.h b/code/reader/SensorReaderTurn.h new file mode 100755 index 0000000..3efaa77 --- /dev/null +++ b/code/reader/SensorReaderTurn.h @@ -0,0 +1,56 @@ +#ifndef SENSORREADERTURN_H +#define SENSORREADERTURN_H + +#include + +/** entry for one sensor */ +struct SensorEntryTurn { + + /** timestamp of occurrence */ + float ts; + + /** sensor data */ + float delta_heading; + float delta_motion; + +}; + +/** read sensor data from CSV */ +class SensorReaderTurn { + +private: + + std::ifstream fp; + +public: + + SensorReaderTurn(const std::string& file) { + fp.open(file); + assert(fp.is_open()); + } + + bool hasNext() { + return !fp.bad() && !fp.eof(); + } + + /** read the next sensor entry */ + SensorEntryTurn getNext() { + + char delim; + SensorEntryTurn entry; + + fp >> entry.ts; + fp >> delim; + fp >> entry.delta_heading; + fp >> delim; + fp >> entry.delta_motion; + + return entry; + + + } + + +}; + +#endif // SENSORREADERTURN_H diff --git a/code/toni/BarometerEvaluation.h b/code/toni/BarometerEvaluation.h new file mode 100755 index 0000000..c2597b0 --- /dev/null +++ b/code/toni/BarometerEvaluation.h @@ -0,0 +1,58 @@ +#pragma once + +#include "../particles/MyState.h" +#include "BarometerObservation.h" +#include "barometric.h" +#include + +double g_BarometerObservation = 0.0; + +class BarometerEvaluation { + +public: + + double getProbability(const MyState& state, const BarometerObservation* obs) const { + + //rho_z + double barometerSigma = 0.3; + + //The height of the single floor levels. + const static double floor_height[3] = {4.1, 3.4, 3.4}; + + if(USE_BAROMETRIC_FORMULAR){ + //height the particle has climbed. + double h_1 = 0.0; + for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){ + h_1 += floor_height[i]; + } + + if(h_1 != 0.0){ + // use the barometric formular to calculate the relative pressure + // the calculation is done assuming sea level height at every floor. + double mslp = BarometricFormular::s_getSeaLevelPressure(); + double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0); + barometerSigma = std::abs(mslp - pressure); + } + + } + else { + // constant value for sigma if we assume all floors are same in height + barometerSigma = 0.30 / 1.0; //hPa + } + + // evaluate the current particle with a normal distribution + const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa); + + //Just for the visualization. i'm a lazy bastard + g_BarometerObservation = obs->hpa; + + assert(barometerProbability == barometerProbability); + assert(state.hPa == state.hPa); + assert(obs->hpa == obs->hpa); + + //std::cout << barometerProbability << std::endl; + + return pow(2.0, barometerProbability); + //return barometerProbability; + } +}; diff --git a/code/toni/BarometerObservation.h b/code/toni/BarometerObservation.h new file mode 100755 index 0000000..2109ba5 --- /dev/null +++ b/code/toni/BarometerObservation.h @@ -0,0 +1,17 @@ +#pragma once + +#include + + +struct BarometerObservation { + + double hpa; + + BarometerObservation() { ; } + BarometerObservation(const float hpa) : hpa(hpa) { + + ;} + +}; + + diff --git a/code/toni/BarometerSensorReader.h b/code/toni/BarometerSensorReader.h new file mode 100755 index 0000000..f9cbe2b --- /dev/null +++ b/code/toni/BarometerSensorReader.h @@ -0,0 +1,91 @@ +#pragma once + +#include "circular.h" +#include "BarometerObservation.h" +#include "../SensorReader.h" +#include + +//circular_buffer measurementHistory(1000); + + +class BarometerSensorReader{ + +private: + circular_buffer measurementHistory; + +public: + + BarometerSensorReader(){ + if(!USE_STATIC_CIRCULAR_BUFFERING){ + //8.33min + measurementHistory.reserve(10000); + } + else{ + //30 * 500ms = 1,5s + measurementHistory.reserve(30); + } + } + + BarometerObservation* readBarometer(const SensorEntry& se) { + + std::string tmp = se.data; + BarometerObservation* obs = new BarometerObservation(); + + //Read the hPa + double hPa = stod(tmp); + + // load the measurement at current time into the history + double currentMeasurement = hPa - measurementHistory[0]; + + if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){ + + //smoothing with alpha value + if(measurementHistory.size() > 1){ + double alpha = 0.1; + double lastMeasurement = measurementHistory[measurementHistory.size() - 1]; + currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement); + + obs->hpa = currentMeasurement; + }else{ + obs->hpa = 0; + } + + measurementHistory.push_back(currentMeasurement); + } + else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){ + + currentMeasurement = hPa; + measurementHistory.push_back(currentMeasurement); + + // calculate the relative air pressure by getting the mean of the first and last three entrys of the history + // and subtract them. + if (measurementHistory.size() > 5){ + double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0; + double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0; + + obs->hpa = meanHead - meanTail; + } + else{ + obs->hpa = 0; + } + } + else //no data smoothing + { + measurementHistory.push_back(currentMeasurement); + obs->hpa = currentMeasurement; + } + + return obs; + + } + + //TODO + void readVerticalAcceleration(const SensorEntry& se){ + + //Problem: Koordinatensystem LinearAcceleraton ist relativ zum Telefon und nicht zum + //Weltkoordinatensystem. Brauchen die Beschleunigung nach Oben in Weltkoordinaten. + + } + + +}; diff --git a/code/toni/TFRingBuffer.h b/code/toni/TFRingBuffer.h new file mode 100755 index 0000000..d215f41 --- /dev/null +++ b/code/toni/TFRingBuffer.h @@ -0,0 +1,52 @@ +#ifndef TFObjectPool_TFRingBuffer_h +#define TFObjectPool_TFRingBuffer_h + +#include +#include + +template class TFRingBuffer { + T *m_buffer; + std::atomic m_head; + std::atomic m_tail; + const size_t m_size; + + size_t next(size_t current) { + return (current + 1) % m_size; + } + +public: + + TFRingBuffer(const size_t size) : m_size(size), m_head(0), m_tail(0) { + m_buffer = new T[size]; + } + + virtual ~TFRingBuffer() { + delete[] m_buffer; + } + + bool push(const T &object) { + size_t head = m_head.load(std::memory_order_relaxed); + size_t nextHead = next(head); + if (nextHead == m_tail.load(std::memory_order_acquire)) { + return false; + } + m_buffer[head] = object; + m_head.store(nextHead, std::memory_order_release); + + return true; + } + + bool pop(T &object) { + size_t tail = m_tail.load(std::memory_order_relaxed); + if (tail == m_head.load(std::memory_order_acquire)) { + return false; + } + + object = m_buffer[tail]; + m_tail.store(next(tail), std::memory_order_release); + return true; + } +}; + + +#endif \ No newline at end of file diff --git a/code/toni/barometric.h b/code/toni/barometric.h new file mode 100755 index 0000000..52603d8 --- /dev/null +++ b/code/toni/barometric.h @@ -0,0 +1,109 @@ +#ifndef BAROMETRIC +#define BAROMETRIC + +const static double mslp = 980.25; // mean sea level spressure +const static double int_lapse_rate = 0.0065; // a +const static double int_exponent = 5.255; // international barometric formular exponent calculated from (M * g) / (R * a) + +//The height of the single floor levels. +const static double floor_height[5] = {0.0, 4.1, 3.4, 3.4, 3.4}; + + +class BarometricFormular +{ + private: + const double temperature; // T in Kelvin + + const double universal_gas_constant; // R + const double molar_mass; // M + const double gravitational_acceleration; // g + const double lapse_rate; // a + double _exponent; + + public: + + /** ctor */ + BarometricFormular(const double R, const double M, const double g, const double a, const double T): + universal_gas_constant(R), molar_mass(M), gravitational_acceleration(g), lapse_rate(a), temperature(T){ + _exponent = (M * g) / (R * a); + } + + /** ctor only with Temperature*/ + BarometricFormular(const double T) : + universal_gas_constant(8.314), molar_mass(0.02896), gravitational_acceleration(9.80665), lapse_rate(0.0065), temperature(T){ + _exponent = (molar_mass * gravitational_acceleration) / (universal_gas_constant * lapse_rate); + } + + /** Atmospheric Pressure Calculation */ + double getAtmosphericPressure(double p_0, double h_1) const{ + return p_0 * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent); + } + + /** Atmospheric Pressure Calculation above sea level*/ + double getAtmosphericPressure(double h_1) const{ + return mslp * std::pow((1.0 - ((lapse_rate * h_1)/temperature)), _exponent); + } + + //TODO:: Height from pressure for the general formular + + //Static Functions + + /** International Barometric Formular*/ + static double s_getAtmosphericPressure(double p_0, double h_1, double kelvin){ + return p_0 * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent); + } + + /** International Barometric Formular above Sea Level*/ + static double s_getAtmosphericPressure(double h_1, double kelvin){ + return mslp * std::pow((1.0 - ((int_lapse_rate * h_1)/kelvin)), int_exponent); + } + + /** International Barometric Formular above Sea Level at 15 degree*/ + static double s_getAtmosphericPressure(double height_above_sea_level){ + return mslp * std::pow((1.0 - ((int_lapse_rate * height_above_sea_level)/288.15)), int_exponent); + } + + /** Get the height above sea level using a pressure measurment above sea level*/ + static double getHeightAboveSeaLevel(double p, double kelvin){ + // http://www.wolframalpha.com/input/?i=solve+for+h+++p+%3D+980.25*%281+-+0.0065+*+h%2FT%29^5.255 + return 41.4811 * ((3.70882 * kelvin) - (std::pow(p, 0.1902949571836346) * kelvin)); + } + + + /** This is a helper Class only for gnupplot visualization for ipin2015*/ + static double getHeightForVisualizationOnly(double p, double z_0, double kelvin){ + + // the height of the reference (first) pressure measurement + double h_0 = 0.0; + for(int i = 0; i <= z_0; i++){ + h_0 += floor_height[i]; + } + + // pressure value of h_0 above sea level + // we define that the bottom of floor 0 is sea level ;). + double p_0 = s_getAtmosphericPressure(h_0, kelvin); + + // pressure value of the current particle above floor 0 (sea level) + double p_height = p_0 + p; + + // height of the particle above floor 0 (sea level) + return getHeightAboveSeaLevel(p_height, kelvin); + + } + + static double s_getSeaLevelPressure(){ + return mslp; + } + + static double getPressureOfFloorForVizualization(double z, double kelvin){ + + int i = z + 0.5; + double h_z = floor_height[i+1]; + double p_z = s_getAtmosphericPressure(h_z, kelvin); + return std::abs(mslp - p_z); + } + +}; + +#endif // BAROMETRIC + diff --git a/code/toni/circular.h b/code/toni/circular.h new file mode 100755 index 0000000..f0bc8db --- /dev/null +++ b/code/toni/circular.h @@ -0,0 +1,492 @@ +/****************************************************************************** +* $Id: $ +* $Name: $ +* +* Author: Pete Goodliffe +* +* ---------------------------------------------------------------------------- +* Copyright 2002 Pete Goodliffe All rights reserved. +* +* ---------------------------------------------------------------------------- +* Purpose: STL-style circular buffer +* +* ---------------------------------------------------------------------------- +* History: See source control system log. +* +*****************************************************************************/ + +#ifndef CIRCULAR_BUFFER_H +#define CIRCULAR_BUFFER_H + +#include +#include +#include + +/****************************************************************************** +* Iterators +*****************************************************************************/ + +/** +* Iterator type for the circular_buffer class. +* +* This one template class provides all variants of forward/reverse +* const/non const iterators through plentiful template magic. +* +* You don't need to instantiate it directly, use the good public functions +* availble in circular_buffer. +*/ +template //+ const for const iter +class circular_buffer_iterator +{ +public: + + typedef circular_buffer_iterator self_type; + + typedef T cbuf_type; + typedef std::random_access_iterator_tag iterator_category; + typedef typename cbuf_type::value_type value_type; + typedef typename cbuf_type::size_type size_type; + typedef typename cbuf_type::pointer pointer; + typedef typename cbuf_type::const_pointer const_pointer; + typedef typename cbuf_type::reference reference; + typedef typename cbuf_type::const_reference const_reference; + typedef typename cbuf_type::difference_type difference_type; + + circular_buffer_iterator(cbuf_type *b, size_type p) + : buf_(b), pos_(p) {} + + // Converting a non-const iterator to a const iterator + circular_buffer_iterator + (const circular_buffer_iterator + &other) + : buf_(other.buf_), pos_(other.pos_) {} + friend class circular_buffer_iterator; + + // Use compiler generated copy ctor, copy assignment operator and dtor + + elem_type &operator*() { return (*buf_)[pos_]; } + elem_type *operator->() { return &(operator*()); } + + self_type &operator++() + { + pos_ += 1; + return *this; + } + self_type operator++(int) + { + self_type tmp(*this); + ++(*this); + return tmp; + } + + self_type &operator--() + { + pos_ -= 1; + return *this; + } + self_type operator--(int) + { + self_type tmp(*this); + --(*this); + return tmp; + } + + self_type operator+(difference_type n) const + { + self_type tmp(*this); + tmp.pos_ += n; + return tmp; + } + self_type &operator+=(difference_type n) + { + pos_ += n; + return *this; + } + + self_type operator-(difference_type n) const + { + self_type tmp(*this); + tmp.pos_ -= n; + return tmp; + } + self_type &operator-=(difference_type n) + { + pos_ -= n; + return *this; + } + + difference_type operator-(const self_type &c) const + { + return pos_ - c.pos_; + } + + bool operator==(const self_type &other) const + { + return pos_ == other.pos_ && buf_ == other.buf_; + } + bool operator!=(const self_type &other) const + { + return pos_ != other.pos_ && buf_ == other.buf_; + } + bool operator>(const self_type &other) const + { + return pos_ > other.pos_; + } + bool operator>=(const self_type &other) const + { + return pos_ >= other.pos_; + } + bool operator<(const self_type &other) const + { + return pos_ < other.pos_; + } + bool operator<=(const self_type &other) const + { + return pos_ <= other.pos_; + } + +private: + + cbuf_type *buf_; + size_type pos_; +}; + +template +circular_buffer_iterator_t operator+ +(const typename circular_buffer_iterator_t::difference_type &a, +const circular_buffer_iterator_t &b) +{ + return circular_buffer_iterator_t(a) + b; +} + +template +circular_buffer_iterator_t operator- +(const typename circular_buffer_iterator_t::difference_type &a, +const circular_buffer_iterator_t &b) +{ + return circular_buffer_iterator_t(a) - b; +} + + +/****************************************************************************** +* circular_buffer +*****************************************************************************/ + +/** +* This class provides a circular buffer in the STL style. +* +* You can add data to the end using the @ref push_back function, read data +* using @ref front() and remove data using @ref pop_front(). +* +* The class also provides random access through the @ref operator[]() +* function and its random access iterator. Subscripting the array with +* an invalid (out of range) index number leads to undefined results, both +* for reading and writing. +* +* This class template accepts three template parameters: +*
  • T The type of object contained +*
  • always_accept_data_when_full Determines the behaviour of +* @ref push_back when the buffer is full. +* Set to true new data is always added, the +* old "end" data is thrown away. +* Set to false, the new data is not added. +* No error is returned neither is an +* exception raised. +*
  • Alloc Allocator type to use (in line with other +* STL containers). +* +* @short STL style circule buffer +* @author Pete Goodliffe +* @version 1.00 +*/ +template > +class circular_buffer +{ +public: + + enum + { + version_major = 1, + version_minor = 0 + }; + + // Typedefs + typedef circular_buffer + self_type; + + typedef Alloc allocator_type; + + typedef typename Alloc::value_type value_type; + typedef typename Alloc::pointer pointer; + typedef typename Alloc::const_pointer const_pointer; + typedef typename Alloc::reference reference; + typedef typename Alloc::const_reference const_reference; + + typedef typename Alloc::size_type size_type; + typedef typename Alloc::difference_type difference_type; + + typedef circular_buffer_iterator + + iterator; + typedef circular_buffer_iterator + + const_iterator; + typedef std::reverse_iterator reverse_iterator; + typedef std::reverse_iterator const_reverse_iterator; + + // Lifetime + enum { default_capacity = 100 }; + explicit circular_buffer(size_type capacity = default_capacity) + : array_(alloc_.allocate(capacity)), array_size_(capacity), + head_(1), tail_(0), contents_size_(0) + { + } + circular_buffer(const circular_buffer &other) + : array_(alloc_.allocate(other.array_size_)), + array_size_(other.array_size_), + head_(other.head_), tail_(other.tail_), + contents_size_(other.contents_size_) + { + try + { + assign_into(other.begin(), other.end()); + } + catch (...) + { + destroy_all_elements(); + alloc_.deallocate(array_, array_size_); + throw; + } + } + template + circular_buffer(InputIterator from, InputIterator to) + : array_(alloc_.allocate(1)), array_size_(1), + head_(1), tail_(0), contents_size_(0) + { + circular_buffer tmp; + tmp.assign_into_reserving(from, to); + swap(tmp); + } + ~circular_buffer() + { + destroy_all_elements(); + alloc_.deallocate(array_, array_size_); + } + circular_buffer &operator=(const self_type &other) + { + circular_buffer tmp(other); + swap(tmp); + return *this; + } + void swap(circular_buffer &other) + { + std::swap(array_, other.array_); + std::swap(array_size_, other.array_size_); + std::swap(head_, other.head_); + std::swap(tail_, other.tail_); + std::swap(contents_size_, other.contents_size_); + } + allocator_type get_allocator() const { return alloc_; } + + // Iterators + iterator begin() { return iterator(this, 0); } + iterator end() { return iterator(this, size()); } + + const_iterator begin() const { return const_iterator(this, 0); } + const_iterator end() const { return const_iterator(this, size()); } + + reverse_iterator rbegin() { return reverse_iterator(end()); } + reverse_iterator rend() { return reverse_iterator(begin()); } + + const_reverse_iterator rbegin() const + { + return const_reverse_iterator(end()); + } + const_reverse_iterator rend() const + { + return const_reverse_iterator(begin()); + } + + // Size + size_type size() const { return contents_size_; } + size_type capacity() const { return array_size_; } + bool empty() const { return !contents_size_; } + size_type max_size() const + { + return alloc_.max_size(); + } + void reserve(size_type new_size) + { + if (capacity() < new_size) + { + circular_buffer tmp(new_size); + tmp.assign_into(begin(), end()); + swap(tmp); + } + } + + // Accessing + reference front() { return array_[head_]; } + reference back() { return array_[tail_]; } + const_reference front() const { return array_[head_]; } + const_reference back() const { return array_[tail_]; } + + void push_back(const value_type &item) + { + size_type next = next_tail(); + if (contents_size_ == array_size_) + { + if (always_accept_data_when_full) + { + array_[next] = item; + increment_head(); + } + } + else + { + alloc_.construct(array_ + next, item); + } + increment_tail(); + } + void pop_front() + { + size_type destroy_pos = head_; + increment_head(); + alloc_.destroy(array_ + destroy_pos); + } + void clear() + { + for (size_type n = 0; n < contents_size_; ++n) + { + alloc_.destroy(array_ + index_to_subscript(n)); + } + head_ = 1; + tail_ = contents_size_ = 0; + } + + reference operator[](size_type n) { return at_unchecked(n); } + const_reference operator[](size_type n) const { return at_unchecked(n); } + + reference at(size_type n) { return at_checked(n); } + const_reference at(size_type n) const { return at_checked(n); } + +private: + + reference at_unchecked(size_type index) const + { + return array_[index_to_subscript(index)]; + } + + reference at_checked(size_type index) const + { + if (size >= contents_size_) + { + throw std::out_of_range(); + } + return at_unchecked(index); + } + + // Rounds an unbounded to an index into array_ + size_type normalise(size_type n) const { return n % array_size_; } + + // Converts external index to an array subscript + size_type index_to_subscript(size_type index) const + { + return normalise(index + head_); + } + + void increment_tail() + { + ++contents_size_; + tail_ = next_tail(); + } + + size_type next_tail() + { + return (tail_ + 1 == array_size_) ? 0 : tail_ + 1; + } + + void increment_head() + { + // precondition: !empty() + ++head_; + --contents_size_; + if (head_ == array_size_) head_ = 0; + } + + template + void assign_into(f_iter from, f_iter to) + { + if (contents_size_) clear(); + while (from != to) + { + push_back(*from); + ++from; + } + } + + template + void assign_into_reserving(f_iter from, f_iter to) + { + if (contents_size_) clear(); + while (from != to) + { + if (contents_size_ == array_size_) + { + reserve(static_cast(array_size_ * 1.5)); + } + push_back(*from); + ++from; + } + } + + void destroy_all_elements() + { + for (size_type n = 0; n < contents_size_; ++n) + { + alloc_.destroy(array_ + index_to_subscript(n)); + } + } + + allocator_type alloc_; + value_type *array_; + size_type array_size_; + size_type head_; + size_type tail_; + size_type contents_size_; +}; + +template + bool operator==(const circular_buffer &a, + const circular_buffer &b) +{ + return a.size() == b.size() && std::equal(a.begin(), a.end(), b.begin()); +} + +template + bool operator!=(const circular_buffer &a, + const circular_buffer &b) +{ + return a.size() != b.size() || !std::equal(a.begin(), a.end(), b.begin()); +} + +template + bool operator<(const circular_buffer &a, + const circular_buffer &b) +{ + return std::lexicographical_compare(a.begin(), a.end(), b.begin(), b.end()); +} + +#endif