diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index c33101e..af2a3be 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -34,10 +34,10 @@ FILE(GLOB HEADERS Settings.h FtmKalman.h main.h - mainFtm.h trilateration.h Plotta.h misc.h + Eval.h ) @@ -45,9 +45,10 @@ FILE(GLOB SOURCES ../../Indoor/lib/tinyxml/tinyxml2.cpp ../../Indoor/lib/Recast/*.cpp main.cpp - mainFtm.cpp mainTrilat.cpp mainProb.cpp + Eval.cpp + FtmKalman.cpp ) diff --git a/code/Eval.cpp b/code/Eval.cpp new file mode 100644 index 0000000..7e12d5e --- /dev/null +++ b/code/Eval.cpp @@ -0,0 +1,70 @@ +#include "Eval.h" + +#include "Settings.h" + +#include + +double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& particlePos, const std::vector& measurements, std::shared_ptr> ftmKalmanFilters) +{ + double result = 1.0; + + for (WiFiMeasurement wifi : measurements) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + { + continue; + } + + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + const Point3 apPos = Settings::CurrentPath.nucInfo(nucIndex).position; + // particlePos.z = 1.3; // smartphone höhe + const float apDist = particlePos.getDistance(apPos); + + // compute ftm distance + float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); + + if (UseSensor == SensorMode::FTM) + { + if (ftmDist > 0) + { + //double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO + double sigma = 5; + + if (ftmKalmanFilters != nullptr) + { + Kalman& kalman = ftmKalmanFilters->at(mac); + ftmDist = kalman.predict(currentTime, ftmDist); + //sigma = std::sqrt(kalman.P(0, 0)); + + Assert::isTrue(sigma > 0, "sigma"); + + double x = Distribution::Normal::getProbability(ftmDist, sigma, apDist); + + result *= x; + } + else + { + double x = Distribution::Normal::getProbability(ftmDist, sigma, apDist); + + result *= x; + } + } + } + else + { + // RSSI + double sigma = 5; + + double x = Distribution::Normal::getProbability(rssiDist, sigma, apDist); + result *= x; + } + } + + return result; +} \ No newline at end of file diff --git a/code/Eval.h b/code/Eval.h new file mode 100644 index 0000000..7bc62c7 --- /dev/null +++ b/code/Eval.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include + +#include "FtmKalman.h" +#include +#include + + +enum class SensorMode +{ + FTM, + RSSI +}; + +double ftmEval(SensorMode UseSensor, + const Timestamp& currentTime, + const Point3& particlePos, + const std::vector& measurements, + std::shared_ptr> ftmKalmanFilters); diff --git a/code/FtmKalman.cpp b/code/FtmKalman.cpp new file mode 100644 index 0000000..2e077cd --- /dev/null +++ b/code/FtmKalman.cpp @@ -0,0 +1,51 @@ +#include "FtmKalman.h" + +#include + +float Kalman::predict(const Timestamp timestamp, const float measurment) +{ + constexpr auto square = [](float x) { return x * x; }; + const auto I = Eigen::Matrix2f::Identity(); + + Eigen::Map> x(this->x); + Eigen::Map> P(this->P); + + // init kalman filter + if (isnan(lastTimestamp)) + { + P << 10, 0, + 0, 10; // Initial Uncertainty + + x << measurment, + 0; + } + + const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp; + lastTimestamp = timestamp.sec(); + + Eigen::Matrix H; // Measurement function + H << 1, 0; + + Eigen::Matrix2f A; // Transition Matrix + A << 1, dt, + 0, 1; + + Eigen::Matrix2f Q; // Process Noise Covariance + Q << square(processNoiseDistance), 0, + 0, square(processNoiseVelocity); + + // Prediction + x = A * x; // Prädizierter Zustand aus Bisherigem und System + P = A * P*A.transpose() + Q; // Prädizieren der Kovarianz + + // Correction + float Z = measurment; + auto y = Z - (H*x); // Innovation aus Messwertdifferenz + auto S = (H*P*H.transpose() + R); // Innovationskovarianz + auto K = P * H.transpose()* (1 / S); //Filter-Matrix (Kalman-Gain) + + x = x + (K*y); // aktualisieren des Systemzustands + P = (I - (K*H))*P; // aktualisieren der Kovarianz + + return x(0); +} \ No newline at end of file diff --git a/code/FtmKalman.h b/code/FtmKalman.h index 384cd38..810af5a 100644 --- a/code/FtmKalman.h +++ b/code/FtmKalman.h @@ -1,17 +1,14 @@ #pragma once -#include - #include - struct Kalman { int nucID = 0; // debug only - Eigen::Matrix x; // predicted state - Eigen::Matrix P; // Covariance - + float x[2]; // predicted state + float P[4]; // Covariance + float R = 30; // measurement noise covariance float processNoiseDistance; // stdDev float processNoiseVelocity; // stdDev @@ -28,50 +25,7 @@ struct Kalman : nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity) {} - float predict(const Timestamp timestamp, const float measurment) - { - constexpr auto square = [](float x) { return x * x; }; - const auto I = Eigen::Matrix2f::Identity(); - - // init kalman filter - if (isnan(lastTimestamp)) - { - P << 10, 0, - 0, 10; // Initial Uncertainty - - x << measurment, - 0; - } - - const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp; - lastTimestamp = timestamp.sec(); - - Eigen::Matrix H; // Measurement function - H << 1, 0; - - Eigen::Matrix2f A; // Transition Matrix - A << 1, dt, - 0, 1; - - Eigen::Matrix2f Q; // Process Noise Covariance - Q << square(processNoiseDistance), 0, - 0, square(processNoiseVelocity); - - // Prediction - x = A * x; // Prädizierter Zustand aus Bisherigem und System - P = A * P*A.transpose()+Q; // Prädizieren der Kovarianz - - // Correction - float Z = measurment; - auto y = Z - (H*x); // Innovation aus Messwertdifferenz - auto S = (H*P*H.transpose()+R); // Innovationskovarianz - auto K = P * H.transpose()* (1/S); //Filter-Matrix (Kalman-Gain) - - x = x + (K*y); // aktualisieren des Systemzustands - P = (I - (K*H))*P; // aktualisieren der Kovarianz - - return x(0); - } + float predict(const Timestamp timestamp, const float measurment); }; diff --git a/code/Plotta.h b/code/Plotta.h index 395de2c..2eb22e6 100644 --- a/code/Plotta.h +++ b/code/Plotta.h @@ -6,6 +6,7 @@ #include #include #include +#include namespace Plotta { @@ -39,6 +40,8 @@ namespace Plotta { // send data std::ofstream stream; + stream.exceptions(stream.exceptions() | std::ios::failbit); + stream.open(dataFile); std::time_t t = std::time(nullptr); @@ -126,6 +129,12 @@ namespace Plotta return writeNumpyArray(stream, list.begin(), list.end()); } + template + plottastream& operator<<(plottastream& stream, const std::array& list) + { + return writeNumpyArray(stream, list.begin(), list.end()); + } + template static plottastream& operator<<(plottastream& stream, const T& value) diff --git a/code/Plotty.h b/code/Plotty.h index a301171..d8f7344 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -108,6 +108,7 @@ public: K::GnuplotSplotElementEmpty emptyElem; + std::vector distanceCircles; K::GnuplotSplotElementPM3D pm3doutline; @@ -186,6 +187,31 @@ public: } + void addDistanceCircle(const Point2& center, float radius, const K::GnuplotColor& strokeColor) + { + auto c = K::GnuplotCoordinate2(center.x, center.y, K::GnuplotCoordinateSystem::FIRST); + auto r = K::GnuplotCoordinate1(radius, K::GnuplotCoordinateSystem::FIRST); + + K::GnuplotFill fill(K::GnuplotFillStyle::EMPTY, K::GnuplotColor::fromRGB(0, 0, 0)); + K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, strokeColor); + + K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke); + + splot.getObjects().add(obj); + + distanceCircles.push_back(obj->getID()); + } + + void clearDistanceCircles() + { + for (int oldID : distanceCircles) + { + splot.getObjects().remove(oldID); + } + + distanceCircles.clear(); + } + void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { for (BBox3 bb : boxes.get()) { //&&addBBoxPoly(bb, c); @@ -554,9 +580,14 @@ public: void saveToFile(std::ofstream& stream){ gp.draw(splot); + +#if defined(_WINDOWS) + stream << "set terminal windows size 2000,1500\n"; +#elif stream << "set terminal x11 size 2000,1500\n"; +#endif stream << gp.getBuffer(); - stream << "pause -1\n"; +// stream << "pause -1\n"; gp.flush(); } @@ -588,8 +619,8 @@ public: // K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z) // ); - splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x)); - splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y)); + splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x-5, bbox.getMax().x+5)); + splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y-5, bbox.getMax().y+5)); splot.getAxisZ().setRange(K::GnuplotAxis::Range(0 /*bbox.getMin().z*/, bbox.getMax().z)); // process each selected floor diff --git a/code/Settings.h b/code/Settings.h index f2a6363..10f3efc 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -2,100 +2,17 @@ #include -#include -#include -#include - namespace Settings { - const bool useKLB = false; - const int numParticles = 5000; - const int numBSParticles = 50; - namespace IMU { - const float turnSigma = 2.5; // 3.5 - const float stepLength = 1.00; - const float stepSigma = 0.15; //toni changed - } - - const float smartphoneAboveGround = 1.3; - - const float offlineSensorSpeedup = 2; - - namespace Grid { - constexpr int gridSize_cm = 20; - } - - namespace Smoothing { - const bool activated = true; - const double stepLength = 0.7; - const double stepSigma = 0.2; - const double headingSigma = 25.0; - const double zChange = 0.0; // mu change in height between two time steps - const double zSigma = 0.1; - const int lag = 5; - - } - - namespace KDE { - const Point2 bandwidth(1,1); - const float gridSize = 0.2; - } - - namespace KDE3D { - const Point3 bandwidth(1, 1, 1); - const Point3 gridSize(0.2, 0.2, 1); // in meter - } - - //const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination - const GridPoint destination = GridPoint(0,0,0); // do not use destination - - namespace SensorDebug { - const Timestamp updateEvery = Timestamp::fromMS(200); - } - - namespace WiFiModel { - constexpr float sigma = 8.0; - /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ - constexpr float TXP = -45; - constexpr float EXP = 2.3; - constexpr float WAF = -11.0; - - const bool optimize = false; - const bool useRegionalOpt = false; - - // how to perform VAP grouping. see - // - calibration in Controller.cpp - // - eval in Filter.h - // NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without! - const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM - const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::AVERAGE, 1); // Frank: WAS MAXIMUM - } - - namespace BeaconModel { - constexpr float sigma = 8.0; - constexpr float TXP = -71; - constexpr float EXP = 1.5; - constexpr float WAF = -20.0; //-5 //20?? - } - - namespace MapView3D { - const int maxColorPoints = 1000; - constexpr int fps = 15; - const Timestamp msPerFrame = Timestamp::fromMS(1000/fps); - } - - namespace Filter { - const Timestamp updateEvery = Timestamp::fromMS(500); - constexpr bool useMainThread = false; // perform filtering in the main thread - } - const std::string mapDir = "../map/"; const std::string dataDir = "../measurements/data/"; const std::string errorDir = "../measurements/error/"; + const std::string plotDataDir = "../plots/data/"; + const std::string outputDir = "../output/"; - const bool UseKalman = false; + const bool UseKalman = true; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -110,7 +27,9 @@ namespace Settings { if (addr == Settings::NUC2) return 1; if (addr == Settings::NUC3) return 2; if (addr == Settings::NUC4) return 3; - else assert(false); + else { + assert(false); return 0; + } } static MACAddress nucFromIndex(int idx) @@ -121,7 +40,7 @@ namespace Settings { case 1: return NUC2; case 2: return NUC3; case 3: return NUC4; - default: assert(false); + default: assert(false); return NUC1; } } @@ -138,21 +57,29 @@ namespace Settings { }; struct DataSetup { + std::string name; std::string map; std::vector training; std::unordered_map NUCs; std::vector gtPath; + bool HasNanoSecondTimestamps; NUCSettings nucInfo(int idx) const { return NUCs.at(nucFromIndex(idx)); } + + NUCSettings nuc(const MACAddress& mac) const + { + return NUCs.at(mac); + } }; /** all configured datasets */ const struct Data { const DataSetup Path0 = { + "path0", mapDir + "map0_ap_path0.xml", { dataDir + "Pixel2/Path0_0605.csv", @@ -164,11 +91,13 @@ namespace Settings { { NUC3, {3, {21.6, 19.1, 0.8}, 1.75, 3.375, 6.107} }, // NUC 3 { NUC4, {4, {20.8, 27.1, 0.8}, 2.75, 2.750, 3.985} }, // NUC 4 }, - { 0, 1, 2, 3 } + { 0, 1, 2, 3 }, + false }; // 1 Path: U von TR nach TD und zurück; const DataSetup Path1 = { + "path1", mapDir + "map2_ap_path1.xml", { dataDir + "Pixel2/path1/1560153927208_2_1.csv", @@ -184,11 +113,13 @@ namespace Settings { { NUC3, {3, {21.3, 19.3, 0.8}, 2.50, 0, 10.0f} }, // NUC 3 { NUC4, {4, {20.6, 26.8, 0.8}, 3.00, 0, 10.0f} }, // NUC 4 }, - { 1, 2, 6, 7, 6, 2, 1 } + { 1, 2, 6, 7, 6, 2, 1 }, + false }; // 2 Path: Wie 2 nur von TD zu TR const DataSetup Path2 = { + "path2", mapDir + "map2_ap_path1.xml", { dataDir + "Pixel2/path2/1560154622883_3_1.csv", @@ -204,12 +135,14 @@ namespace Settings { { NUC3, {3, {21.3, 19.3, 0.8}, 2.25, 0, 3.0f} }, // NUC 3 { NUC4, {4, {20.6, 26.8, 0.8}, 2.00, 0, 3.0f} }, // NUC 4 }, - { 7, 6, 2, 1, 2, 6, 7 } + { 7, 6, 2, 1, 2, 6, 7 }, + false }; // 3 Path: U von TR nach TD; 4 mal das U const DataSetup Path3 = { + "path3", mapDir + "map2_ap_path2.xml", { dataDir + "Pixel2/path3/1560155227376_4_1.csv", @@ -225,11 +158,13 @@ namespace Settings { { NUC3, {3, {21.3, 19.3, 0.8}, 1.75, 0, 3.0f} }, // NUC 3 { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 0, 3.0f} }, // NUC 4 }, - { 1, 2, 6, 7, 6, 2, 1, 2, 6, 7, 6, 2, 1 } + { 1, 2, 6, 7, 6, 2, 1, 2, 6, 7, 6, 2, 1 }, + false }; // 4 Path: In Räumen const DataSetup Path4 = { + "path4", mapDir + "map2_ap_path2.xml", { dataDir + "Pixel2/path4/1560156876457_5_1.csv", @@ -245,12 +180,14 @@ namespace Settings { { NUC3, {3, {21.3, 19.3, 0.8}, 2.25, 0, 3.0f} }, // NUC 3 { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 0, 3.0f} }, // NUC 4 }, - { 0, 1, 2, 3, 2, 6, 5, 6, 7, 8 } + { 0, 1, 2, 3, 2, 6, 5, 6, 7, 8 }, + false }; // 5 Path: In Räumen extendend const DataSetup Path5 = { + "path5", mapDir + "map2_ap_path2.xml", { dataDir + "Pixel2/path5/1560158444772_6_1.csv", @@ -266,12 +203,90 @@ namespace Settings { { NUC3, {3, {21.3, 19.3, 0.8}, 2.75, 3.250, 3.0f} }, // NUC 3 { NUC4, {4, {20.6, 26.8, 0.8}, 2.25, 3.375, 3.0f} }, // NUC 4 }, - { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 } + { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 }, + false }; - const DataSetup CurrentPath = Path5; + // 6 Path: SHL Path 1 + const DataSetup Path6 = { + "path6", + mapDir + "shl.xml", + { + dataDir + "Pixel2/path6/14681054221905_6_1.csv" + }, + { + // NUC, ID Pos X Y Z offset loss kalman stddev + { NUC1, {1, { 54, 46, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 1 + { NUC2, {2, { 45, 37, 0.8}, 5.00, 3.375, 3.0f} }, // NUC 2 + { NUC3, {3, { 27, 45, 0.8}, 5.00, 3.250, 3.0f} }, // NUC 3 + { NUC4, {4, { 16, 36, 0.8}, 5.75, 3.375, 3.0f} }, // NUC 4 + }, + { 100, 101, 102, 103, 104, 103, 102, 101, 100 }, + true + }; + + // 7 Path: SHL Path 2; Versuche mit NUCs in den Räumen war nicht vielversprechend ... + const DataSetup Path7 = { + "path7", + mapDir + "shl.xml", + { + dataDir + "Pixel2/path7/23388354821394.csv", + dataDir + "Pixel2/path7/23569363647863.csv", + dataDir + "Pixel2/path7/23776390928852.csv", + dataDir + "Pixel2/path7/23938602403553.csv" + }, + { + // NUC, ID Pos X Y Z offset loss kalman stddev + { NUC1, {1, { 54, 46, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 1 + { NUC2, {2, { 45, 37, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 2 + { NUC3, {3, { 27, 45, 0.8}, 0.0, 3.250, 3.0f} }, // NUC 3 + { NUC4, {4, { 16, 36, 0.8}, 0.0, 3.375, 3.0f} }, // NUC 4 + }, + { 100, 102, 103, 104, 105, 104, 103, 102, 100 }, + true + }; + + // 8 Path: Wie SHL Path 2 nur, dass die NUCs im Gang stehen + const DataSetup Path8 = { + "path8", + mapDir + "shl.xml", + { + dataDir + "Pixel2/path8/25967118530318.csv", // gang + dataDir + "Pixel2/path8/25439520303384.csv", // tür + }, + { + // NUC, ID Pos X Y Z offset loss kalman stddev + { NUC1, {1, { 55, 44, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 + { NUC2, {2, { 46, 40, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 2 + { NUC3, {3, { 27, 44, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 3 + { NUC4, {4, { 15, 40, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 4 + }, + { 100, 102, 103, 104, 105, 104, 103, 102, 100 }, + true + }; + + // 9 Path: SHL Path 3, NUCs stehen im Gang + const DataSetup Path9 = { + "path9", + mapDir + "shl_nuc_gang.xml", + { + dataDir + "Pixel2/path9/27911186920065.csv", + dataDir + "Pixel2/path9/28255150484121.csv", + dataDir + "Pixel2/path9/28404719230167.csv", + }, + { + // NUC, ID Pos X Y Z offset loss kalman stddev + { NUC1, {1, { 55, 44, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 1 + { NUC2, {2, { 46, 40, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 2 + { NUC3, {3, { 27, 44, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 3 + { NUC4, {4, { 15, 40, 0.8}, 0.00, 2.500, 3.0f} }, // NUC 4 + }, + { 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 }, + true + }; } data; + static DataSetup CurrentPath = data.Path7; } diff --git a/code/filter.h b/code/filter.h index 006d473..d2ebd66 100644 --- a/code/filter.h +++ b/code/filter.h @@ -3,7 +3,9 @@ #include "mesh.h" #include "Settings.h" #include +#include +#include #include #include #include @@ -32,6 +34,7 @@ #include #include "FtmKalman.h" +#include "Eval.h" struct MyState { @@ -111,19 +114,13 @@ struct MyControl { struct MyObservation { - // pressure - float sigmaPressure = 0.10f; - float relativePressure = 0; - //wifi - std::unordered_map wifi; + std::unordered_map wifi; // deprecated + + std::vector ftm; //time Timestamp currentTime; - - //activity - Activity activity; - }; class MyPFInitUniform : public SMC::ParticleFilterInitializer { @@ -183,8 +180,10 @@ class MyPFTransStatic : public SMC::ParticleFilterTransition struct MyPFTransRandom : public SMC::ParticleFilterTransition{ Distribution::Normal dStepSize; + Distribution::Uniform dHeading; + MyPFTransRandom() - : dStepSize(0.0f, 0.6f) + : dStepSize(2.0f, 0.2f), dHeading(0, 2*M_PI) {} void transition(std::vector>& particles, const MyControl* control) override { @@ -192,8 +191,12 @@ struct MyPFTransRandom : public SMC::ParticleFilterTransition& p = particles[i]; - p.state.pos.pos.x += dStepSize.draw(); - p.state.pos.pos.y += dStepSize.draw(); + + const float angle = dHeading.draw(); + const float stepSize = dStepSize.draw(); + + p.state.pos.pos.x += std::cos(angle) * stepSize; + p.state.pos.pos.y += std::sin(angle) * stepSize; } } }; @@ -260,95 +263,35 @@ public: //control->afterEval(); } - - }; -class MyPFEval : public SMC::ParticleFilterEvaluation { - - //TODO: add this to transition probability - double getStairProb(const SMC::Particle& p, const Activity act) { - - const float kappa = 0.75; - - switch (act) { - - case Activity::WALKING: - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} - {return 1-kappa;} - - case Activity::WALKING_UP: - case Activity::WALKING_DOWN: - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;} - if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;} - {return 1-kappa;} - } - return 1.0; - } - -public: +struct MyPFEval : public SMC::ParticleFilterEvaluation { // FRANK MyPFEval() { }; bool assignProps = false; - - std::shared_ptr> kalmanMap; + std::shared_ptr> ftmKalmanFilters; virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { double sum = 0; - - //#pragma omp parallel for num_threads(3) + + #pragma omp parallel for num_threads(3) for (int i = 0; i < particles.size(); ++i) { SMC::Particle& p = particles[i]; - double pFtm = 1.0; + auto kalmanFilters = ftmKalmanFilters; - if (observation.wifi.size() == 0) + if (!Settings::UseKalman) { - printf(""); + kalmanFilters = nullptr; } - for (auto& wifi : observation.wifi) { - - if ( (true && wifi.second.getAP().getMAC() == Settings::NUC1) - || (true && wifi.second.getAP().getMAC() == Settings::NUC2) - || (true && wifi.second.getAP().getMAC() == Settings::NUC3) - || (true && wifi.second.getAP().getMAC() == Settings::NUC4) - ) - { - float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.second.getAP().getMAC()).rssi_pathloss; - - float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.second.getRSSI()); - float ftmDist = wifi.second.getFtmDist(); - - Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.first)->second.position; - Point3 particlePos = p.state.pos.pos; - particlePos.z = 1.3; // smartphone höhe - float apDist = particlePos.getDistance(apPos); - - if (Settings::UseKalman) - { - auto kalman = kalmanMap->at(wifi.second.getAP().getMAC()); - pFtm *= Distribution::Normal::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist); - } - else - { - pFtm *= Distribution::Normal::getProbability(apDist, 3.5, ftmDist); - //pFtm *= Distribution::Region::getProbability(apDist, 3.5/2, ftmDist); - } - } - } - - - double prob = pFtm; + double prob = ftmEval(SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters); if (assignProps) - p.weight = prob; // p.weight *= prob + p.weight = prob; else p.weight *= prob; @@ -357,9 +300,7 @@ public: } return sum; - } - }; diff --git a/code/main.cpp b/code/main.cpp index 882d4a9..c8d8b4a 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -5,6 +5,7 @@ #include "Settings.h" #include "meshPlotter.h" #include "Plotty.h" +#include "Plotta.h" #include #include @@ -27,7 +28,6 @@ #include #include "FtmKalman.h" -#include "mainFtm.h" #include "misc.h" #include @@ -50,7 +50,7 @@ std::vector> getFtmValues(Offline::FileReader& f if (wifi.getAP().getMAC() == nuc) { - Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position; + Point3 apPos = Settings::CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position; float apDist = gtPos.getDistance(apPos); float ftmDist = wifi.getFtmDist(); float rssi = wifi.getRSSI(); @@ -155,9 +155,9 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator& gt auto wifi = fr.getWifiFtm()[e.idx].data; - int nucid = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ID; - float ftm_offset = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).ftm_offset; - float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.getAP().getMAC()).rssi_pathloss; + int nucid = Settings::CurrentPath.NUCs.at(wifi.getAP().getMAC()).ID; + float ftm_offset = Settings::CurrentPath.NUCs.at(wifi.getAP().getMAC()).ftm_offset; + float rssi_pathloss = Settings::CurrentPath.NUCs.at(wifi.getAP().getMAC()).rssi_pathloss; float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); float ftmDist = wifi.getFtmDist() + ftm_offset; //in m; plus static offset @@ -165,7 +165,7 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator& gt int numMeas = wifi.getNumAttemptedMeasurements(); int numSuccessMeas = wifi.getNumSuccessfulMeasurements(); - Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position; + Point3 apPos = Settings::CurrentPath.NUCs.find(wifi.getAP().getMAC())->second.position; float apDist = gtPos.getDistance(apPos); fs << ts.ms() << ";" << nucid << ";" << apDist << ";" << rssiDist << ";" << ftmDist << ";" << ftmStdDev << ";" << numMeas << ";" << numSuccessMeas << "\n"; @@ -180,18 +180,18 @@ void exportFtmValues(Offline::FileReader& fr, Interpolator& gt static float kalman_procNoiseDistStdDev = 1.2f; // standard deviation of distance for process noise static float kalman_procNoiseVelStdDev = 0.1f; // standard deviation of velocity for process noise -static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std::string folder) { +static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::string folder) { // reading file std::string currDir = std::filesystem::current_path().string(); Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[walkIdx]); - + Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps); + // ground truth std::vector gtPath = setup.gtPath; Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); - Stats::Statistics errorStats; + CombinedStats errorStats; //calculate distance of path std::vector::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); @@ -214,6 +214,13 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: std::ofstream errorFile; errorFile.open (evalDir.string() + "/" + std::to_string(walkIdx) + "_" + std::to_string(t) + ".csv"); + // Output dir + auto outputDir = std::filesystem::path(Settings::outputDir); + outputDir.append(Settings::CurrentPath.name + "_" + std::to_string(walkIdx)); + + if (!std::filesystem::exists(outputDir)) { + std::filesystem::create_directories(outputDir); + } // wifi auto kalmanMap = std::make_shared>(); @@ -232,13 +239,6 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: MyNavMeshFactory fac(&mesh, set); fac.build(map); - const Point3 srcPath0(9.8, 24.9, 0); // fixed start pos - - // add shortest-path to destination - //const Point3 dst(51, 45, 1.7); - //const Point3 dst(25, 45, 0); - //NM::NavMeshDijkstra::stamp(mesh, dst); - // debug show //MeshPlotter dbg; //dbg.addFloors(map); @@ -252,10 +252,10 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: plot.setGroundTruth(gtPath); plot.setView(30, 0); // APs Positions - plot.addCircle(100000 + 0, Settings::data.CurrentPath.nucInfo(0).position.xy(), 0.05); - plot.addCircle(100000 + 1, Settings::data.CurrentPath.nucInfo(1).position.xy(), 0.05); - plot.addCircle(100000 + 2, Settings::data.CurrentPath.nucInfo(2).position.xy(), 0.05); - plot.addCircle(100000 + 3, Settings::data.CurrentPath.nucInfo(3).position.xy(), 0.05); + plot.addCircle(100000 + 0, Settings::CurrentPath.nucInfo(0).position.xy(), 0.1); + plot.addCircle(100000 + 1, Settings::CurrentPath.nucInfo(1).position.xy(), 0.1); + plot.addCircle(100000 + 2, Settings::CurrentPath.nucInfo(2).position.xy(), 0.1); + plot.addCircle(100000 + 3, Settings::CurrentPath.nucInfo(3).position.xy(), 0.1); plot.plot(); // particle-filter @@ -263,13 +263,14 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: //auto init = std::make_unique(&mesh, srcPath0); // known position auto init = std::make_unique(&mesh); // uniform distribution auto eval = std::make_unique(); - eval->kalmanMap = kalmanMap; + eval->ftmKalmanFilters = kalmanMap; auto trans = std::make_unique(); //auto trans = std::make_unique(); auto resample = std::make_unique>(); auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); @@ -283,172 +284,132 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: MyControl ctrl; MyObservation obs; - StepDetection sd; - PoseDetection pd; - TurnDetection td(&pd); - RelativePressure relBaro; - ActivityDetector act; - relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); Timestamp lastTimestamp = Timestamp::fromMS(0); + + std::vector errorValuesFtm, errorValuesRssi; + std::vector timestamps; + std::vector> gtDistances, ftmDistances, rssiDistances; // distance per AP - int i = 0; - // parse each sensor-value within the offline data - for (const Offline::Entry& e : fr.getEntries()) { + Plotta::Plotta errorPlot("errorPlot", outputDir.string() + "/errorData.py"); + Plotta::Plotta distsPlot("distsPlot", outputDir.string() + "/distances.py"); - const Timestamp ts = Timestamp::fromMS(e.ts); - - if (e.type == Offline::Sensor::WIFI_FTM) { - auto ftm = fr.getWifiFtm()[e.idx].data; - - float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset; - float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset - - - if (Settings::UseKalman) - { - auto& kalman = kalmanMap->at(ftm.getAP().getMAC()); - float predictDist = kalman.predict(ts, ftmDist); - - ftm.setFtmDist(predictDist); - - obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm); - } - else - { - // MOV AVG - if (obs.wifi.count(ftm.getAP().getMAC()) == 0) - { - obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm); - } - else - { - auto currFtm = obs.wifi.find(ftm.getAP().getMAC()); - - float currDist = currFtm->second.getFtmDist(); - - const float alpha = 0.6; - float newDist = alpha * currDist + (1 - alpha) * ftmDist; - - currFtm->second.setFtmDist(newDist); - } - } - } else if (e.type == Offline::Sensor::WIFI) { - //obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; - //ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data; - } else if (e.type == Offline::Sensor::ACC) { - if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { - ++ctrl.numStepsSinceLastEval; - } - const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - pd.addAccelerometer(ts, _acc.data); - - //simpleActivity walking / standing - act.add(ts, fr.getAccelerometer()[e.idx].data); - - } else if (e.type == Offline::Sensor::GYRO) { - const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; - const float delta_gyro = td.addGyroscope(ts, _gyr.data); - - ctrl.headingChangeSinceLastEval += delta_gyro; - - } else if (e.type == Offline::Sensor::BARO) { - relBaro.add(ts, fr.getBarometer()[e.idx].data); - obs.relativePressure = relBaro.getPressureRealtiveToStart(); - obs.sigmaPressure = relBaro.getSigma(); - - //simpleActivity stairs up / down - act.add(ts, fr.getBarometer()[e.idx].data); - obs.activity = act.get(); + for (const Offline::Entry& e : fr.getEntries()) + { + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; } - if (ctrl.numStepsSinceLastEval > 0) - //if (ts - lastTimestamp >= Timestamp::fromMS(500)) - //if (obs.wifi.size() == 4) - { + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.ftm.push_back(wifiFtm); + + + if (ts - lastTimestamp >= Timestamp::fromMS(500)) + { + // Do update step + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + gtDistances.push_back({ gtPos.getDistance(Settings::CurrentPath.nucInfo(0).position.xy()), + gtPos.getDistance(Settings::CurrentPath.nucInfo(1).position.xy()), + gtPos.getDistance(Settings::CurrentPath.nucInfo(2).position.xy()), + gtPos.getDistance(Settings::CurrentPath.nucInfo(3).position.xy()) }); + + Point3 estPos; + float distErrorFtm = 0; + float distErrorRssi = 0; + + // Run PF obs.currentTime = ts; ctrl.currentTime = ts; -// if(ctrl.numStepsSinceLastEval > 0){ -// pf.updateTransitionOnly(&ctrl); -// } - MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs); + MyState est = pf.update(&ctrl, obs); ctrl.afterEval(); - Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1); lastTimestamp = ts; - ctrl.lastEstimate = est.pos.pos; + estPos = est.pos.pos; + ctrl.lastEstimate = estPos; + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); + + // Error + distErrorFtm = gtPos.getDistance(estPos.xy()); + errorStats.ftm.add(distErrorFtm); // draw wifi ranges - for (auto& ftm : obs.wifi) - { - int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID; + plot.clearDistanceCircles(); - if (nucid == 1) + for (size_t i = 0; i < obs.ftm.size(); i++) + { + WiFiMeasurement wifi2 = obs.ftm[i]; + + Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; + + K::GnuplotColor color; + switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID) { - Point3 apPos = Settings::data.CurrentPath.NUCs.find(ftm.first)->second.position; - //plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist()); + case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; + case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; + case 3: color = K::GnuplotColor::fromRGB(255, 255, 0); break; + default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; } + + plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color); } obs.wifi.clear(); + obs.ftm.clear(); - //plot - //dbg.showParticles(pf.getParticles()); - //dbg.setCurPos(est.pos.pos); - //dbg.setGT(gtPos); - //dbg.addEstimationNode(est.pos.pos); - //dbg.addGroundTruthNode(gtPos); - //dbg.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); - //dbg.draw(); - //plot.printOverview("test"); + errorValuesFtm.push_back(distErrorFtm); + errorValuesRssi.push_back(distErrorRssi); + timestamps.push_back(ts.ms()); + // Error plot + errorPlot.add("t", timestamps); + errorPlot.add("errorFtm", errorValuesFtm); + errorPlot.add("errorRssi", errorValuesRssi); + errorPlot.frame(); + + // Distances plot + //distsPlot.add("t", timestamps); + //distsPlot.add("gtDists", gtDistances); + //distsPlot.add("ftmDists", ftmDistances); + //distsPlot.frame(); + + // Plotting plot.showParticles(pf.getParticles()); - plot.setCurEst(est.pos.pos); - plot.setGroundTruth(gtPos); - - plot.addEstimationNode(est.pos.pos); - plot.setActivity((int) act.get()); + plot.setCurEst(estPos); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + plot.addEstimationNode(estPos); + //plot.setActivity((int)act.get()); //plot.splot.getView().setEnabled(false); //plot.splot.getView().setCamera(0, 0); //plot.splot.getView().setEqualXY(true); -// plot.plot(); - plot.plot(); - //plot.closeStream(); - std::this_thread::sleep_for(100ms); - - // error calc -// float err_m = gtPos.getDistance(est.pos.pos); -// errorStats.add(err_m); -// errorFile << ts.ms() << " " << err_m << "\n"; - - //error calc with penalty for wrong floor - double errorFactor = 3.0; - Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z); - Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z); - float err_m = gtPosError.getDistance(estError); - errorStats.add(err_m); - errorFile << ts.ms() << " " << err_m << "\n"; } } + + printErrorStats(errorStats); + std::ofstream plot_out; + plot_out.open(outputDir.string() + "/plot.gp"); - // get someting on console - std::cout << "Statistical Analysis Filtering: " << std::endl; - std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; + plot.clearDistanceCircles(); + plot.saveToFile(plot_out); - // save the statistical data in file - errorFile << "========================================================== \n"; - errorFile << "Average of all statistical data: \n"; - errorFile << "Median: " << errorStats.getMedian() << "\n"; - errorFile << "Average: " << errorStats.getAvg() << "\n"; - errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n"; - errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n"; - errorFile.close(); + std::ofstream errorStats_out; + errorStats_out.open(outputDir.string() + "/error_stats.txt"); + printErrorStats(errorStats_out, errorStats); + + errorPlot.frame(); + + //system("pause"); return errorStats; } @@ -468,20 +429,71 @@ int main(int argc, char** argv) return mainTrilat(argc, argv); } - //mainFtm(argc, argv); - //return 0; - - Stats::Statistics statsAVG; - Stats::Statistics statsMedian; - Stats::Statistics statsSTD; - Stats::Statistics statsQuantil; - Stats::Statistics tmp; + CombinedStats statsAVG; + CombinedStats statsMedian; + CombinedStats statsSTD; + CombinedStats statsQuantil; + CombinedStats tmp; std::string evaluationName = "prologic/tmp"; + + std::vector setupsToRun = { Settings::data.Path7, + Settings::data.Path8, + Settings::data.Path9 }; - std::vector> error; - std::ofstream error_out; - error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); + for (Settings::DataSetup setupToRun : setupsToRun) + { + Settings::CurrentPath = setupToRun; + + + for (size_t walkIdx = 0; walkIdx < 1 /*Settings::CurrentPath.training.size()*/; walkIdx++) + { + std::cout << "Executing walk " << walkIdx << "\n"; + for (int i = 0; i < 1; ++i) + { + std::cout << "Start of iteration " << i << "\n"; + + tmp = run(Settings::CurrentPath, walkIdx, evaluationName); + + statsAVG.ftm.add(tmp.ftm.getAvg()); + statsMedian.ftm.add(tmp.ftm.getMedian()); + statsSTD.ftm.add(tmp.ftm.getStdDev()); + statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); + + statsAVG.rssi.add(tmp.rssi.getAvg()); + statsMedian.rssi.add(tmp.rssi.getMedian()); + statsSTD.rssi.add(tmp.rssi.getStdDev()); + statsQuantil.rssi.add(tmp.rssi.getQuantile(0.75)); + + std::cout << "Iteration " << i << " completed" << std::endl; + } + } + + std::cout << "Results for path " << Settings::CurrentPath.name << std::endl; + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data FTM: " << std::endl; + std::cout << "Median: " << statsMedian.ftm.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.ftm.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.ftm.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.ftm.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data RSSI: " << std::endl; + std::cout << "Median: " << statsMedian.rssi.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.rssi.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.rssi.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.rssi.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + } + + + + + + //std::vector> error; + //std::ofstream error_out; + //error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); //for (kalman_procNoiseDistStdDev = 0.8f; kalman_procNoiseDistStdDev < 1.5f; kalman_procNoiseDistStdDev += 0.1f) @@ -489,24 +501,24 @@ int main(int argc, char** argv) // for (kalman_procNoiseVelStdDev = 0.1f; kalman_procNoiseVelStdDev < 0.5f; kalman_procNoiseVelStdDev += 0.1f) // { - for (size_t walkIdx = 0; walkIdx < 6; walkIdx++) - { - std::cout << "Executing walk " << walkIdx << "\n"; - for (int i = 0; i < 1; ++i) - { - std::cout << "Start of iteration " << i << "\n"; + //for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); walkIdx++) + //{ + // std::cout << "Executing walk " << walkIdx << "\n"; + // for (int i = 0; i < 1; ++i) + // { + // std::cout << "Start of iteration " << i << "\n"; - tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName); - statsMedian.add(tmp.getMedian()); - statsAVG.add(tmp.getAvg()); - statsSTD.add(tmp.getStdDev()); - statsQuantil.add(tmp.getQuantile(0.75)); + // tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName); + // statsMedian.add(tmp.getMedian()); + // statsAVG.add(tmp.getAvg()); + // statsSTD.add(tmp.getStdDev()); + // statsQuantil.add(tmp.getQuantile(0.75)); - std::cout << kalman_procNoiseDistStdDev << " " << kalman_procNoiseVelStdDev << std::endl; - std::cout << "Iteration " << i << " completed" << std::endl; + // std::cout << kalman_procNoiseDistStdDev << " " << kalman_procNoiseVelStdDev << std::endl; + // std::cout << "Iteration " << i << " completed" << std::endl; - } - } + // } + //} // error.push_back({{ kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev, statsAVG.getAvg() }}); @@ -536,38 +548,6 @@ int main(int argc, char** argv) //error_out.close(); - //for(int i = 0; i < 2; ++i){ - // - // tmp = run(Settings::data.CurrentPath, 0, evaluationName); - // statsMedian.add(tmp.getMedian()); - // statsAVG.add(tmp.getAvg()); - // statsSTD.add(tmp.getStdDev()); - // statsQuantil.add(tmp.getQuantile(0.75)); - - // std::cout << "Iteration " << i << " completed" << std::endl; - //} - - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data: " << std::endl; - std::cout << "Median: " << statsMedian.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - - //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS - std::ofstream finalStatisticFile; - finalStatisticFile.open (Settings::errorDir + evaluationName + ".csv", std::ios_base::app); - - finalStatisticFile << "========================================================== \n"; - finalStatisticFile << "Average of all statistical data: \n"; - finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n"; - finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n"; - finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n"; - finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n"; - finalStatisticFile << "========================================================== \n"; - - finalStatisticFile.close(); //std::this_thread::sleep_for(std::chrono::seconds(60)); diff --git a/code/mainFtm.cpp b/code/mainFtm.cpp deleted file mode 100644 index 3d93aad..0000000 --- a/code/mainFtm.cpp +++ /dev/null @@ -1,279 +0,0 @@ -#include "mainFtm.h" - -#include "mesh.h" -#include "filter.h" -#include "Settings.h" -//#include "meshPlotter.h" -#include "Plotty.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include - -#include "FtmKalman.h" - -#include - -static Stats::Statistics run(Settings::DataSetup setup, int numFile, std::string folder) { - - // reading file - std::string currDir = std::filesystem::current_path().string(); - - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[numFile]); - - // ground truth - std::vector gtPath = setup.gtPath; - Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); - Stats::Statistics errorStats; - - //calculate distance of path - std::vector::InterpolatorEntry> gtEntries = gtInterpolator.getEntries(); - double distance = 0; - for(int i = 1; i < gtEntries.size(); ++i){ - distance += gtEntries[i].value.getDistance(gtEntries[i-1].value); - } - - std::cout << "Distance of Path: " << distance << std::endl; - - // error file - const long int t = static_cast(time(NULL)); - auto evalDir = std::filesystem::path(Settings::errorDir); - evalDir.append(folder); - - if (!std::filesystem::exists(evalDir)) { - std::filesystem::create_directory(evalDir); - } - - std::ofstream errorFile; - errorFile.open (evalDir.string() + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); - - - // wifi - auto kalmanMap = std::make_shared>(); - kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev) }); - kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev) }); - - // mesh - NM::NavMeshSettings set; - MyNavMesh mesh; - MyNavMeshFactory fac(&mesh, set); - fac.build(map); - - const Point3 srcPath0(9.8, 24.9, 0); // fixed start pos - - // add shortest-path to destination - //const Point3 dst(51, 45, 1.7); - //const Point3 dst(25, 45, 0); - //NM::NavMeshDijkstra::stamp(mesh, dst); - - // debug show - //MeshPlotter dbg; - //dbg.addFloors(map); - //dbg.addOutline(map); - //dbg.addMesh(mesh); - ////dbg.addDijkstra(mesh); - //dbg.draw(); - - Plotty plot(map); - plot.buildFloorplan(); - plot.setGroundTruth(gtPath); - plot.plot(); - - // particle-filter - const int numParticles = 5000; - //auto init = std::make_unique(&mesh, srcPath0); // known position - auto init = std::make_unique(&mesh); // uniform distribution - auto eval = std::make_unique(); - eval->assignProps = true; - eval->kalmanMap = kalmanMap; - - //auto trans = std::make_unique(mesh); - auto trans = std::make_unique(); - - auto resample = std::make_unique>(); - auto estimate = std::make_unique>(); - - // setup - MyFilter pf(numParticles, std::move(init)); - pf.setEvaluation(std::move(eval)); - pf.setTransition(std::move(trans)); - pf.setResampling(std::move(resample)); - pf.setEstimation(std::move(estimate)); - //pf.setNEffThreshold(0.85); - pf.setNEffThreshold(0.0); - - // sensors - MyControl ctrl; - MyObservation obs; - - StepDetection sd; - PoseDetection pd; - TurnDetection td(&pd); - RelativePressure relBaro; - ActivityDetector act; - relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); - Timestamp lastTimestamp = Timestamp::fromMS(0); - - - // parse each sensor-value within the offline data - for (const Offline::Entry& e : fr.getEntries()) { - - const Timestamp ts = Timestamp::fromMS(e.ts); - - if (e.type == Offline::Sensor::WIFI_FTM) { - auto ftm = fr.getWifiFtm()[e.idx].data; - - float ftm_offset = Settings::data.CurrentPath.NUCs.at(ftm.getAP().getMAC()).ftm_offset; - float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset - - auto& kalman = kalmanMap->at(ftm.getAP().getMAC()); - float predictDist = kalman.predict(ts, ftmDist); - - //ftm.setFtmDist(predictDist); - - obs.wifi.insert_or_assign(ftm.getAP().getMAC(), ftm); - } - - //if (ctrl.numStepsSinceLastEval > 0) - //if (ts - lastTimestamp >= Timestamp::fromMS(500)) - if (obs.wifi.size() == 4) - { - - obs.currentTime = ts; - ctrl.currentTime = ts; - -// if(ctrl.numStepsSinceLastEval > 0){ -// pf.updateTransitionOnly(&ctrl); -// } - MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs); - ctrl.afterEval(); - Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1); - lastTimestamp = ts; - - ctrl.lastEstimate = est.pos.pos; - - - // draw wifi ranges - for (auto& ftm : obs.wifi) - { - int nucid = Settings::data.CurrentPath.NUCs.at(ftm.second.getAP().getMAC()).ID; - - if (nucid == 1) - { - Point3 apPos = Settings::data.CurrentPath.NUCs.find(ftm.first)->second.position; - // plot.addCircle(nucid, apPos.xy(), ftm.second.getFtmDist()); - } - } - - obs.wifi.clear(); - - //plot - //dbg.showParticles(pf.getParticles()); - //dbg.setCurPos(est.pos.pos); - //dbg.setGT(gtPos); - //dbg.addEstimationNode(est.pos.pos); - //dbg.addGroundTruthNode(gtPos); - //dbg.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); - //dbg.draw(); - - plot.showParticles(pf.getParticles()); - plot.setCurEst(est.pos.pos); - plot.setGroundTruth(gtPos); - - plot.addEstimationNode(est.pos.pos); - plot.setActivity((int) act.get()); - plot.splot.getView().setCamera(0, 0); - //plot.splot.getView().setEqualXY(true); - - plot.plot(); - - - //std::this_thread::sleep_for(500ms); - - // error calc -// float err_m = gtPos.getDistance(est.pos.pos); -// errorStats.add(err_m); -// errorFile << ts.ms() << " " << err_m << "\n"; - - //error calc with penalty for wrong floor - double errorFactor = 3.0; - Point3 gtPosError = Point3(gtPos.x, gtPos.y, errorFactor * gtPos.z); - Point3 estError = Point3(est.pos.pos.x, est.pos.pos.y, errorFactor * est.pos.pos.z); - float err_m = gtPosError.getDistance(estError); - errorStats.add(err_m); - errorFile << ts.ms() << " " << err_m << "\n"; - } - } - - - - // get someting on console - std::cout << "Statistical Analysis Filtering: " << std::endl; - std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; - - // save the statistical data in file - errorFile << "========================================================== \n"; - errorFile << "Average of all statistical data: \n"; - errorFile << "Median: " << errorStats.getMedian() << "\n"; - errorFile << "Average: " << errorStats.getAvg() << "\n"; - errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n"; - errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n"; - errorFile.close(); - - return errorStats; -} - -int mainFtm(int argc, char** argv) { - - Stats::Statistics statsAVG; - Stats::Statistics statsMedian; - Stats::Statistics statsSTD; - Stats::Statistics statsQuantil; - Stats::Statistics tmp; - - std::string evaluationName = "prologic/tmp"; - - for(int i = 0; i < 1; ++i){ - for(int j = 0; j < 1; ++j){ - tmp = run(Settings::data.CurrentPath, j, evaluationName); - statsMedian.add(tmp.getMedian()); - statsAVG.add(tmp.getAvg()); - statsSTD.add(tmp.getStdDev()); - statsQuantil.add(tmp.getQuantile(0.75)); - } - - std::cout << "Iteration " << i << " completed" << std::endl; - } - - std::cout << "==========================================================" << std::endl; - std::cout << "Average of all statistical data: " << std::endl; - std::cout << "Median: " << statsMedian.getAvg() << std::endl; - std::cout << "Average: " << statsAVG.getAvg() << std::endl; - std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; - std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; - std::cout << "==========================================================" << std::endl; - - - //std::this_thread::sleep_for(std::chrono::seconds(60)); - - - return 0; -} diff --git a/code/mainFtm.h b/code/mainFtm.h deleted file mode 100644 index c59939c..0000000 --- a/code/mainFtm.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once - -int mainFtm(int argc, char** argv); - diff --git a/code/mainProb.cpp b/code/mainProb.cpp index c48cb40..e410f1c 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -30,7 +30,7 @@ static float kalman_procNoiseDistStdDev = 1.2f; // standard deviation of distance for process noise static float kalman_procNoiseVelStdDev = 0.1f; // standard deviation of velocity for process noise -static void poorMansKDE(const BBox3& bbox, float sigma, std::array dist, std::array apPos, std::vector>& density, std::pair& maxElem) +static void poorMansKDE(const BBox3& bbox, std::vector>& density, std::pair& maxElem, const std::function& evalProc) { density.clear(); @@ -48,35 +48,99 @@ static void poorMansKDE(const BBox3& bbox, float sigma, std::array dis { const Point2 pos(x, y); - float P = 1.0f; - - for (size_t i = 0; i < 4; i++) - { - // TODO: Was mit nan machen? - if (!isnan(dist[i])) - { - float d = pos.getDistance(apPos[i]) - dist[i]; - float p = Distribution::Normal::getProbability(0, sigma, d); - P *= p; - } - } + double P = evalProc(pos); density.push_back({ pos, P }); } } - auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair a, std::pair b) { + auto maxElement = std::max_element(density.begin(), density.end(), [](std::pair a, std::pair b) { return a.second < b.second; }); maxElem = *maxElement; } +static void computeDensity(const BBox3& bbox, std::vector>& density, std::pair& maxElem, const std::vector& obs, bool useFtm, double sigma) +{ + poorMansKDE(bbox, density, maxElem, [&obs, useFtm, sigma](Point2 pt) { + double p = 1.0; + + for (const WiFiMeasurement& wifi : obs) + { + if (wifi.getNumSuccessfulMeasurements() < 3) + continue; + + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + // compute AP distance + const Point3 apPos = Settings::CurrentPath.nucInfo(nucIndex).position; + Point3 pos = Point3(pt.x, pt.y, 1.3); // smartphone höhe + const float apDist = pos.getDistance(apPos); + + double dist = 0; + if (useFtm) + { + // compute ftm distance + float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + dist = ftmDist; + } + else + { + // compute rssi distance + float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, 2.25 /*rssi_pathloss*/, wifi.getRSSI()); + + dist = rssiDist; + } + + if (dist > 0) + { + double d = apDist - dist; + double x = Distribution::Normal::getProbability(0, 3.5, d); + + p *= x; + } + } + + return p; + }); +} + +static void plotDensity(Plotty& plot, std::vector>& density) +{ + plot.particles.clear(); + + double min = std::numeric_limits::max(); + double max = std::numeric_limits::min(); + + for (auto it = density.begin(); ; std::advance(it, 2)) + { + if (it > density.end()) + break; + + auto p = *it; + + const K::GnuplotPoint3 p3(p.first.x, p.first.y, 0.1); + const double prob = std::pow(p.second, 0.25); + + plot.particles.add(p3, prob); + + if (prob > max) { max = prob; } + if (prob < min) { min = prob; } + } + + plot.splot.getAxisCB().setRange(min, max + 0.000001); +} + static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::string folder) { // reading file Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[walkIdx]); + Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps); // ground truth std::vector gtPath = setup.gtPath; @@ -113,7 +177,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plot.setGroundTruth(gtPath); plot.setView(30, 0); plot.plot(); - + // wifi std::array ftmKalmanFilters{ Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), @@ -123,13 +187,15 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str }; std::array apPositions{ - Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(), }; - std::vector data = filterOfflineData(fr); + std::vector obs; + + Timestamp lastTimestamp = Timestamp::fromMS(0); const float sigma = 3.5; const int movAvgWnd = 15; @@ -139,132 +205,88 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; - for (const WifiMeas& wifi : data) + for (const Offline::Entry& e : fr.getEntries()) { - Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py"); + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } - Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); - float distErrorFtm = 0; - float distErrorRssi = 0; + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.push_back(wifiFtm); - //if (wifi.numSucessMeas() == 4) + if (ts - lastTimestamp >= Timestamp::fromMS(500)) { + // Do update + Plotta::Plotta test("test", "C:\\Temp\\Plotta\\probData.py"); + + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + + float distErrorFtm = 0; + float distErrorRssi = 0; + // FTM { - std::array dists = wifi.ftmDists; - - if (Settings::UseKalman) - { - std::cout << "Using Kalman" << "\n"; - - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - } - } - } - BBox3 bbox = FloorplanHelper::getBBox(map); - std::vector> density; - std::pair maxElement; + std::vector> density; + std::pair maxElement; - poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + computeDensity(bbox, density, maxElement, obs, true, 3.5); Point2 estPos = maxElement.first; - - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); plot.addEstimationNode(Point3(estPos.x, estPos.y, 0.1)); + plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); + + // Plot density + plotDensity(plot, density); // Error distErrorFtm = gtPos.getDistance(estPos); errorStats.ftm.add(distErrorFtm); - - - //std::vector densityX, densityY, densityZ; - //for (const auto& item : density) - //{ - // densityX.push_back(item.first.x); - // densityY.push_back(item.first.y); - // densityZ.push_back(item.second); - //} - - //test.add("densityX", densityX); - //test.add("densityY", densityY); - //test.add("densityZ", densityZ); } // RSSI { - std::array dists = wifi.rssiDists; - - if (Settings::UseKalman) - { - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - } - } - } - - BBox3 bbox = FloorplanHelper::getBBox(map); - std::vector> density; - std::pair maxElement; + std::vector> density; + std::pair maxElement; - poorMansKDE(bbox, sigma, dists, apPositions, density, maxElement); + computeDensity(bbox, density, maxElement, obs, false, 8); Point2 estPos = maxElement.first; - - plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); plot.addEstimationNode2(Point3(estPos.x, estPos.y, 0.1)); + // Plot density + //plotDensity(plot, density); + // Error distErrorRssi = gtPos.getDistance(estPos); errorStats.rssi.add(distErrorRssi); - - - //std::vector densityX, densityY, densityZ; - //for (const auto& item : density) - //{ - // densityX.push_back(item.first.x); - // densityY.push_back(item.first.y); - // densityZ.push_back(item.second); - //} - - //test.add("densityX", densityX); - //test.add("densityY", densityY); - //test.add("densityZ", densityZ); } - //std::cout << wifi.ts.ms() << " " << distError << "\n"; errorValuesFtm.push_back(distErrorFtm); errorValuesRssi.push_back(distErrorRssi); - timestamps.push_back(wifi.ts.ms()); + timestamps.push_back(ts.ms()); test.add("t", timestamps); test.add("errorFtm", errorValuesFtm); test.add("errorRssi", errorValuesRssi); test.frame(); - } - plot.plot(); - //Sleep(250); - printf(""); + plot.plot(); + //Sleep(250); + printf(""); + + lastTimestamp = ts; + obs.clear(); + } } - std::cout << "Walk error:" << "\n"; - std::cout << "[m] " << std::setw(10) << "mean" << std::setw(10) << "stdDev" << std::setw(10) << "median" << "\n"; - - std::cout << "FTM " << std::setw(10) << errorStats.ftm.getAvg() << std::setw(10) << errorStats.ftm.getStdDev() << std::setw(10) << errorStats.ftm.getMedian() << "\n"; - std::cout << "RSSI " << std::setw(10) << errorStats.rssi.getAvg() << std::setw(10) << errorStats.rssi.getStdDev() << std::setw(10) << errorStats.rssi.getMedian() << "\n"; - std::cout << std::endl; + printErrorStats(errorStats); return errorStats; } @@ -282,14 +304,14 @@ int mainProp(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < 6; walkIdx++) + for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; for (int i = 0; i < 1; ++i) { std::cout << "Start of iteration " << i << "\n"; - tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName); + tmp = run(Settings::CurrentPath, walkIdx, evaluationName); statsAVG.ftm.add(tmp.ftm.getAvg()); statsMedian.ftm.add(tmp.ftm.getMedian()); diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp index 103d2e4..8a5ac6b 100644 --- a/code/mainTrilat.cpp +++ b/code/mainTrilat.cpp @@ -67,10 +67,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Plotta::Plotta plotta("test", "C:\\Temp\\Plotta\\dataTrilat.py"); std::vector apPositions{ - Settings::data.CurrentPath.NUCs.at(Settings::NUC1).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC2).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC3).position.xy(), - Settings::data.CurrentPath.NUCs.at(Settings::NUC4).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC1).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC2).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC3).position.xy(), + Settings::CurrentPath.NUCs.at(Settings::NUC4).position.xy(), }; plotta.add("apPos", apPositions); @@ -196,12 +196,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plotta.add("estPathRssi", estPathRssi); plotta.frame(); - std::cout << "Walk error:" << "\n"; - std::cout << "[m] " << " mean \t stdDev median" << "\n"; - - std::cout << "FTM " << errorStats.ftm.getAvg() << "\t" << errorStats.ftm.getStdDev() << "\t" << errorStats.ftm.getMedian() << "\n"; - std::cout << "RSSI " << errorStats.rssi.getAvg() << "\t" << errorStats.rssi.getStdDev() << "\t" << errorStats.rssi.getMedian() << "\n"; - std::cout << std::endl; + printErrorStats(errorStats); return errorStats; } @@ -218,14 +213,14 @@ int mainTrilat(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < 6; walkIdx++) + for (size_t walkIdx = 0; walkIdx < Settings::CurrentPath.training.size(); walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; for (int i = 0; i < 1; ++i) { std::cout << "Start of iteration " << i << "\n"; - tmp = run(Settings::data.CurrentPath, walkIdx, evaluationName); + tmp = run(Settings::CurrentPath, walkIdx, evaluationName); statsAVG.ftm.add(tmp.ftm.getAvg()); statsMedian.ftm.add(tmp.ftm.getMedian()); diff --git a/code/misc.h b/code/misc.h index e4309a8..535d0c0 100644 --- a/code/misc.h +++ b/code/misc.h @@ -48,6 +48,22 @@ struct CombinedStats { Stats::Statistics rssi; }; +template +void printErrorStats(std::ostream& stream, const CombinedStats& errorStats) +{ + stream << "Walk error:" << "\n"; + stream << "[m] " << std::setw(10) << "mean" << std::setw(10) << "stdDev" << std::setw(10) << "median" << "\n"; + + stream << "FTM " << std::setw(10) << errorStats.ftm.getAvg() << std::setw(10) << errorStats.ftm.getStdDev() << std::setw(10) << errorStats.ftm.getMedian() << "\n"; + stream << "RSSI " << std::setw(10) << errorStats.rssi.getAvg() << std::setw(10) << errorStats.rssi.getStdDev() << std::setw(10) << errorStats.rssi.getMedian() << "\n"; + stream << std::endl; +} + +template +void printErrorStats(const CombinedStats& errorStats) +{ + return printErrorStats(std::cout, errorStats); +} static std::vector filterOfflineData(const Offline::FileReader& fr) @@ -84,10 +100,10 @@ static std::vector filterOfflineData(const Offline::FileReader& fr) auto ftm = fr.getWifiFtm()[e.idx].data; const MACAddress& mac = ftm.getAP().getMAC(); - float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftm_offset = Settings::CurrentPath.NUCs.at(mac).ftm_offset; float ftmDist = ftm.getFtmDist() + ftm_offset; // in m; plus static offset - float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss; float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, ftm.getRSSI()); int nucIndex = Settings::nucIndex(mac); diff --git a/map/shl.xml b/map/shl.xml new file mode 100644 index 0000000..6955de8 --- /dev/null +++ b/map/shl.xml @@ -0,0 +1,318 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/shl_nuc_gang.xml b/map/shl_nuc_gang.xml new file mode 100644 index 0000000..8e55722 --- /dev/null +++ b/map/shl_nuc_gang.xml @@ -0,0 +1,318 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +