From 36c64b503286dd05866ff0fe755d748f89011312 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 25 Sep 2019 09:25:23 +0200 Subject: [PATCH 01/16] Added printErrorStats function --- code/mainProb.cpp | 9 ++------- code/mainTrilat.cpp | 9 ++------- code/misc.h | 10 ++++++++++ 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/code/mainProb.cpp b/code/mainProb.cpp index c48cb40..b45b21b 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -259,12 +259,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str printf(""); } - 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,7 +277,7 @@ 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::data.CurrentPath.training.size(); walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; for (int i = 0; i < 1; ++i) diff --git a/code/mainTrilat.cpp b/code/mainTrilat.cpp index 103d2e4..008836a 100644 --- a/code/mainTrilat.cpp +++ b/code/mainTrilat.cpp @@ -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,7 +213,7 @@ 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::data.CurrentPath.training.size(); walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; for (int i = 0; i < 1; ++i) diff --git a/code/misc.h b/code/misc.h index e4309a8..e565f34 100644 --- a/code/misc.h +++ b/code/misc.h @@ -48,6 +48,16 @@ struct CombinedStats { Stats::Statistics rssi; }; +template +void printErrorStats(const CombinedStats& errorStats) +{ + 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; +} static std::vector filterOfflineData(const Offline::FileReader& fr) From 24bbc56f28d83aa56dd920facba18c0510be3606 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 25 Sep 2019 09:30:16 +0200 Subject: [PATCH 02/16] Particle reduced to ftm eval only --- code/Settings.h | 76 +++++++++- code/filter.h | 84 ++--------- code/main.cpp | 352 +++++++++++++++++-------------------------- code/mainFtm.cpp | 1 - map/shl.xml | 340 +++++++++++++++++++++++++++++++++++++++++ map/shl_nuc_gang.xml | 340 +++++++++++++++++++++++++++++++++++++++++ 6 files changed, 908 insertions(+), 285 deletions(-) create mode 100644 map/shl.xml create mode 100644 map/shl_nuc_gang.xml diff --git a/code/Settings.h b/code/Settings.h index f2a6363..cc9cf8c 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -95,7 +95,7 @@ namespace Settings { const std::string dataDir = "../measurements/data/"; const std::string errorDir = "../measurements/error/"; - const bool UseKalman = false; + const bool UseKalman = true; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -269,7 +269,79 @@ namespace Settings { { 0, 1, 2, 11, 10, 9, 10, 11, 2, 6, 5, 12, 13, 12, 5, 6, 7, 8 } }; - const DataSetup CurrentPath = Path5; + + // 6 Path: SHL Path 1 + const DataSetup 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 } + }; + + // 7 Path: SHL Path 2; Versuche mit NUCs in den Räumen war nicht vielversprechend ... + const DataSetup 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}, 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, 102, 103, 104, 105, 104, 103, 102, 100 } + }; + + // 8 Path: Wie SHL Path 2 nur, dass die NUCs im Gang stehen + const DataSetup 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 } + }; + + // 9 Path: SHL Path 3, NUCs stehen im Gang + const DataSetup 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 } + }; + + + const DataSetup CurrentPath = Path8; } data; diff --git a/code/filter.h b/code/filter.h index 006d473..faf1553 100644 --- a/code/filter.h +++ b/code/filter.h @@ -3,6 +3,7 @@ #include "mesh.h" #include "Settings.h" #include +#include #include #include @@ -111,19 +112,13 @@ struct MyControl { struct MyObservation { - // pressure - float sigmaPressure = 0.10f; - float relativePressure = 0; - //wifi - std::unordered_map wifi; + std::unordered_map wifi; // deprecated + std::array dists; + std::array sigmas; // from kalman //time Timestamp currentTime; - - //activity - Activity activity; - }; class MyPFInitUniform : public SMC::ParticleFilterInitializer { @@ -260,44 +255,15 @@ 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; - virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { double sum = 0; @@ -308,47 +274,27 @@ public: double pFtm = 1.0; - if (observation.wifi.size() == 0) + for (size_t i = 0; i < 4; i++) { - printf(""); - } + float dist = observation.dists[i]; + const float sigma = isnan(observation.sigmas[i]) ? 3.5 : observation.sigmas[i]; - 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) - ) + if (!isnan(dist)) { - 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 apPos = Settings::data.CurrentPath.nucInfo(i).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 x = Distribution::Normal::getProbability(dist, std::sqrt(sigma), apDist); + pFtm *= x; + } } - double prob = pFtm; if (assignProps) - p.weight = prob; // p.weight *= prob + p.weight = prob; else p.weight *= prob; @@ -357,9 +303,7 @@ public: } return sum; - } - }; diff --git a/code/main.cpp b/code/main.cpp index 882d4a9..95a6957 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -180,7 +180,7 @@ 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(); @@ -191,7 +191,7 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: // 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(); @@ -216,11 +216,12 @@ static Stats::Statistics run(Settings::DataSetup setup, int walkIdx, std: // wifi - auto kalmanMap = std::make_shared>(); - kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); - kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); - kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); - kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); + std::array ftmKalmanFilters{ + Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), + Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) + }; std::cout << "Optimal wifi parameters for " << setup.training[walkIdx] << "\n"; optimizeWifiParameters(fr, gtInterpolator); @@ -232,13 +233,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); @@ -263,7 +257,6 @@ 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; auto trans = std::make_unique(); //auto trans = std::make_unique(); @@ -283,172 +276,99 @@ 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); - int i = 0; - // parse each sensor-value within the offline data - for (const Offline::Entry& e : fr.getEntries()) { + std::vector data = filterOfflineData(fr); - const Timestamp ts = Timestamp::fromMS(e.ts); + std::vector errorValuesFtm, errorValuesRssi; + std::vector timestamps; - 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 + for (const WifiMeas& wifi : data) + { + Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + Point3 estPos; + float distErrorFtm = 0; + float distErrorRssi = 0; + + // FTM + { + std::array dists = wifi.ftmDists; + std::array sigmas = {NAN, NAN, NAN, NAN }; + + for (size_t i = 0; i < 4; i++) + { + if (dists[i] <= 0) + { + dists[i] = NAN; + } + } 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) + for (size_t i = 0; i < 4; i++) { - 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); + if (!isnan(dists[i])) + { + dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); + sigmas[i] = ftmKalmanFilters[i].P(0, 0); + } } } - } 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); + + obs.dists = dists; + obs.sigmas = sigmas; - //simpleActivity walking / standing - act.add(ts, fr.getAccelerometer()[e.idx].data); + // Run PF + obs.currentTime = wifi.ts; + ctrl.currentTime = wifi.ts; - } 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(); - } - - 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); + MyState est = pf.update(&ctrl, obs); ctrl.afterEval(); - Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())) + Point3(0,0,0.1); - lastTimestamp = ts; + lastTimestamp = wifi.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) + for (size_t i = 0; i < 4; i++) { - 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()); - } + Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position; + plot.addCircle(1000+i, apPos.xy(), dists[i]); } - - 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.printOverview("test"); - - 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().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"; } + + errorValuesFtm.push_back(distErrorFtm); + errorValuesRssi.push_back(distErrorRssi); + timestamps.push_back(wifi.ts.ms()); + + // Plotting + //plot.showParticles(pf.getParticles()); + 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(); + //std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - // 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(); + printErrorStats(errorStats); return errorStats; } @@ -470,18 +390,58 @@ int main(int argc, char** 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> error; - std::ofstream error_out; - error_out.open(Settings::errorDir + evaluationName + "_error_path1" + ".csv", std::ios_base::app); + 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); + + 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 << "==========================================================" << 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 +449,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 +496,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 index 3d93aad..d77e6f7 100644 --- a/code/mainFtm.cpp +++ b/code/mainFtm.cpp @@ -103,7 +103,6 @@ static Stats::Statistics run(Settings::DataSetup setup, int numFile, std: 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(); diff --git a/map/shl.xml b/map/shl.xml new file mode 100644 index 0000000..459ea9d --- /dev/null +++ b/map/shl.xml @@ -0,0 +1,340 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/shl_nuc_gang.xml b/map/shl_nuc_gang.xml new file mode 100644 index 0000000..415db5f --- /dev/null +++ b/map/shl_nuc_gang.xml @@ -0,0 +1,340 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From deb318e115a5db2f4734f21dacbc9e67b199c565 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 25 Sep 2019 16:49:57 +0200 Subject: [PATCH 03/16] Particle filter is now using a fixed time interval as step --- code/Plotta.h | 9 +++ code/Plotty.h | 14 +++++ code/Settings.h | 6 ++ code/filter.h | 80 +++++++++++++++++++----- code/main.cpp | 159 +++++++++++++++++++++++++++--------------------- 5 files changed, 185 insertions(+), 83 deletions(-) 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..e2ef07c 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -184,6 +184,20 @@ public: splot.getObjects().set(id, obj); + } + + void addCircle(int id, 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().set(id, obj); + } void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) { diff --git a/code/Settings.h b/code/Settings.h index cc9cf8c..4887be8 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -94,6 +94,7 @@ namespace Settings { const std::string mapDir = "../map/"; const std::string dataDir = "../measurements/data/"; const std::string errorDir = "../measurements/error/"; + const std::string plotDataDir = "../plots/data/"; const bool UseKalman = true; @@ -147,6 +148,11 @@ namespace Settings { { return NUCs.at(nucFromIndex(idx)); } + + NUCSettings nuc(const MACAddress& mac) const + { + return NUCs.at(mac); + } }; /** all configured datasets */ diff --git a/code/filter.h b/code/filter.h index faf1553..552c209 100644 --- a/code/filter.h +++ b/code/filter.h @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -114,8 +115,8 @@ struct MyObservation { //wifi std::unordered_map wifi; // deprecated - std::array dists; - std::array sigmas; // from kalman + + std::vector ftm; //time Timestamp currentTime; @@ -263,34 +264,85 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { MyPFEval() { }; bool assignProps = false; + 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; - for (size_t i = 0; i < 4; i++) + for (WiFiMeasurement wifi : observation.ftm) { - float dist = observation.dists[i]; - const float sigma = isnan(observation.sigmas[i]) ? 3.5 : observation.sigmas[i]; - - if (!isnan(dist)) + if (wifi.getNumSuccessfulMeasurements() < 3) { - Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position; - Point3 particlePos = p.state.pos.pos; - particlePos.z = 1.3; // smartphone höhe - float apDist = particlePos.getDistance(apPos); + continue; + } - double x = Distribution::Normal::getProbability(dist, std::sqrt(sigma), apDist); - pFtm *= x; + const MACAddress& mac = wifi.getAP().getMAC(); + int nucIndex = Settings::nucIndex(mac); + + const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; + Point3 particlePos = p.state.pos.pos; + particlePos.z = 1.3; // smartphone höhe + const float apDist = particlePos.getDistance(apPos); + + // compute ftm distance + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); + + + if (ftmDist > 0) + { + double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO + + if (sigma == 0) + { + sigma = 38*38; + } + + if (Settings::UseKalman) + { + Kalman& kalman = ftmKalmanFilters->at(mac); + ftmDist = kalman.predict(observation.currentTime, ftmDist); + sigma = kalman.P(0, 0); + + //Assert::isTrue(sigma > 0, "sigma"); + if (sigma <= 0) + { + sigma = 38 * 38; + } + + double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); + + if (x > 1e-90) + { + pFtm *= x; + Assert::isTrue(pFtm != 0, "zero prob"); + } + } + else + { + double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); + + if (x > 1e-90) + { + pFtm *= x; + + Assert::isTrue(pFtm != 0, "zero prob"); + } + } } } + double prob = pFtm; if (assignProps) diff --git a/code/main.cpp b/code/main.cpp index 95a6957..75c4f8b 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 @@ -216,12 +217,11 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str // wifi - std::array ftmKalmanFilters{ - Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev), - Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) - }; + auto kalmanMap = std::make_shared>(); + kalmanMap->insert({ Settings::NUC1, Kalman(1, setup.NUCs.at(Settings::NUC1).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); + kalmanMap->insert({ Settings::NUC2, Kalman(2, setup.NUCs.at(Settings::NUC2).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); + kalmanMap->insert({ Settings::NUC3, Kalman(3, setup.NUCs.at(Settings::NUC3).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); + kalmanMap->insert({ Settings::NUC4, Kalman(4, setup.NUCs.at(Settings::NUC4).kalman_measStdDev, kalman_procNoiseDistStdDev, kalman_procNoiseVelStdDev) }); std::cout << "Optimal wifi parameters for " << setup.training[walkIdx] << "\n"; optimizeWifiParameters(fr, gtInterpolator); @@ -257,12 +257,14 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //auto init = std::make_unique(&mesh, srcPath0); // known position auto init = std::make_unique(&mesh); // uniform distribution auto eval = std::make_unique(); + 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>(); + auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); @@ -277,60 +279,51 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str MyObservation obs; Timestamp lastTimestamp = Timestamp::fromMS(0); - - std::vector data = filterOfflineData(fr); - + std::vector errorValuesFtm, errorValuesRssi; std::vector timestamps; + std::vector> gtDistances, ftmDistances, rssiDistances; // distance per AP + Plotta::Plotta errorPlot("errorPlot", Settings::plotDataDir + "errorData.py"); + Plotta::Plotta distsPlot("distsPlot", Settings::plotDataDir + "distances.py"); - for (const WifiMeas& wifi : data) + for (const Offline::Entry& e : fr.getEntries()) { - Point2 gtPos = gtInterpolator.get(static_cast(wifi.ts.ms())).xy(); - plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); + if (e.type != Offline::Sensor::WIFI_FTM) { + continue; + } - Point3 estPos; - float distErrorFtm = 0; - float distErrorRssi = 0; + // TIME + const Timestamp ts = Timestamp::fromMS(e.ts); - // FTM + auto wifiFtm = fr.getWifiFtm()[e.idx].data; + obs.ftm.push_back(wifiFtm); + + + if (ts - lastTimestamp >= Timestamp::fromMS(500)) { - std::array dists = wifi.ftmDists; - std::array sigmas = {NAN, NAN, NAN, NAN }; + // Do update step + Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); + plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); - for (size_t i = 0; i < 4; i++) - { - if (dists[i] <= 0) - { - dists[i] = NAN; - } - } + gtDistances.push_back({ gtPos.getDistance(Settings::data.CurrentPath.nucInfo(0).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(1).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(2).position.xy()), + gtPos.getDistance(Settings::data.CurrentPath.nucInfo(3).position.xy()) }); - if (Settings::UseKalman) - { - for (size_t i = 0; i < 4; i++) - { - if (!isnan(dists[i])) - { - dists[i] = ftmKalmanFilters[i].predict(wifi.ts, dists[i]); - sigmas[i] = ftmKalmanFilters[i].P(0, 0); - } - } - } - - obs.dists = dists; - obs.sigmas = sigmas; + Point3 estPos; + float distErrorFtm = 0; + float distErrorRssi = 0; // Run PF - obs.currentTime = wifi.ts; - ctrl.currentTime = wifi.ts; + obs.currentTime = ts; + ctrl.currentTime = ts; MyState est = pf.update(&ctrl, obs); ctrl.afterEval(); - lastTimestamp = wifi.ts; + lastTimestamp = ts; estPos = est.pos.pos; - ctrl.lastEstimate = estPos; plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); @@ -341,35 +334,63 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorStats.ftm.add(distErrorFtm); // draw wifi ranges - for (size_t i = 0; i < 4; i++) + for (size_t i = 0; i < obs.ftm.size(); i++) { - Point3 apPos = Settings::data.CurrentPath.nucInfo(i).position; - plot.addCircle(1000+i, apPos.xy(), dists[i]); + WiFiMeasurement wifi2 = obs.ftm[i]; + + Point3 apPos = Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).position; + + K::GnuplotColor color; + switch (Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).ID) + { + 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.addCircle(1000 + i, apPos.xy(), wifi2.getFtmDist(), color); } + + obs.wifi.clear(); + obs.ftm.clear(); + + + 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(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(); } - - errorValuesFtm.push_back(distErrorFtm); - errorValuesRssi.push_back(distErrorRssi); - timestamps.push_back(wifi.ts.ms()); - - // Plotting - //plot.showParticles(pf.getParticles()); - 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(); - //std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - - + printErrorStats(errorStats); + //system("pause"); + return errorStats; } @@ -398,10 +419,10 @@ int main(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); walkIdx++) + for (size_t walkIdx = 0; walkIdx < 1 /*Settings::data.CurrentPath.training.size()*/; walkIdx++) { std::cout << "Executing walk " << walkIdx << "\n"; - for (int i = 0; i < 1; ++i) + for (int i = 0; i < 5; ++i) { std::cout << "Start of iteration " << i << "\n"; From 5c80f6b61a628540a7fb058cbf3f7b652355992c Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 10:08:37 +0200 Subject: [PATCH 04/16] Only convert nanosecond timestamps for new data files --- code/Settings.h | 31 +++++++++++++++++++++---------- code/main.cpp | 4 ++-- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/code/Settings.h b/code/Settings.h index 4887be8..b95f125 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -143,6 +143,7 @@ namespace Settings { std::vector training; std::unordered_map NUCs; std::vector gtPath; + bool HasNanoSecondTimestamps; NUCSettings nucInfo(int idx) const { @@ -170,7 +171,8 @@ 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; @@ -190,7 +192,8 @@ 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 @@ -210,7 +213,8 @@ 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 }; @@ -231,7 +235,8 @@ 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 @@ -251,7 +256,8 @@ 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 }; @@ -272,7 +278,8 @@ 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 }; @@ -289,7 +296,8 @@ namespace Settings { { 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 } + { 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 ... @@ -308,7 +316,8 @@ namespace Settings { { 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, 102, 103, 104, 105, 104, 103, 102, 100 } + { 100, 102, 103, 104, 105, 104, 103, 102, 100 }, + true }; // 8 Path: Wie SHL Path 2 nur, dass die NUCs im Gang stehen @@ -325,7 +334,8 @@ namespace Settings { { 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 } + { 100, 102, 103, 104, 105, 104, 103, 102, 100 }, + true }; // 9 Path: SHL Path 3, NUCs stehen im Gang @@ -343,7 +353,8 @@ namespace Settings { { 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 } + { 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 }, + true }; diff --git a/code/main.cpp b/code/main.cpp index 75c4f8b..d52f944 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -187,8 +187,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str 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.NanoSecondTimestamps); + // ground truth std::vector gtPath = setup.gtPath; Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); From 09f46ac37234e15cd5e896330c6dbdeb3de34236 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 10:09:13 +0200 Subject: [PATCH 05/16] Removed small probability clamp --- code/filter.h | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/code/filter.h b/code/filter.h index 552c209..9105d76 100644 --- a/code/filter.h +++ b/code/filter.h @@ -322,22 +322,13 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - if (x > 1e-90) - { - pFtm *= x; - Assert::isTrue(pFtm != 0, "zero prob"); - } + pFtm *= x; } else { double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - if (x > 1e-90) - { - pFtm *= x; - - Assert::isTrue(pFtm != 0, "zero prob"); - } + pFtm *= x; } } } From 253b96b2f9859993f5a03d3ac22107f4a066f675 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 10:12:01 +0200 Subject: [PATCH 06/16] Removed stairs and elevators from shl map --- map/shl.xml | 22 ---------------------- map/shl_nuc_gang.xml | 22 ---------------------- 2 files changed, 44 deletions(-) diff --git a/map/shl.xml b/map/shl.xml index 459ea9d..6955de8 100644 --- a/map/shl.xml +++ b/map/shl.xml @@ -310,30 +310,8 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/map/shl_nuc_gang.xml b/map/shl_nuc_gang.xml index 415db5f..8e55722 100644 --- a/map/shl_nuc_gang.xml +++ b/map/shl_nuc_gang.xml @@ -310,30 +310,8 @@ - - - - - - - - - - - - - - - - - - - - - - From f8a9739dafa2da4372e88351e2c8072fe71c0a3e Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 14:45:52 +0200 Subject: [PATCH 07/16] Fixed drawing of distance circles --- code/Plotty.h | 16 ++++++++++++++-- code/main.cpp | 6 ++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/code/Plotty.h b/code/Plotty.h index e2ef07c..0df0f3c 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -108,6 +108,7 @@ public: K::GnuplotSplotElementEmpty emptyElem; + std::vector distanceCircles; K::GnuplotSplotElementPM3D pm3doutline; @@ -186,7 +187,7 @@ public: } - void addCircle(int id, const Point2& center, float radius, const K::GnuplotColor& strokeColor) + 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); @@ -196,8 +197,19 @@ public: K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke); - splot.getObjects().set(id, obj); + 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) { diff --git a/code/main.cpp b/code/main.cpp index d52f944..2b93956 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -187,7 +187,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::string currDir = std::filesystem::current_path().string(); Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - Offline::FileReader fr(setup.training[walkIdx], setup.NanoSecondTimestamps); + Offline::FileReader fr(setup.training[walkIdx], setup.HasNanoSecondTimestamps); // ground truth std::vector gtPath = setup.gtPath; @@ -334,6 +334,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str errorStats.ftm.add(distErrorFtm); // draw wifi ranges + plot.clearDistanceCircles(); + for (size_t i = 0; i < obs.ftm.size(); i++) { WiFiMeasurement wifi2 = obs.ftm[i]; @@ -349,7 +351,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str default: color = K::GnuplotColor::fromRGB(255, 0, 0); break; } - plot.addCircle(1000 + i, apPos.xy(), wifi2.getFtmDist(), color); + plot.addDistanceCircle(apPos.xy(), wifi2.getFtmDist(), color); } obs.wifi.clear(); From 17140f2dbe9e04a3ae21205c592ecf7d7a23b443 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 1 Oct 2019 17:23:59 +0200 Subject: [PATCH 08/16] Refactored probabilistic code --- code/mainProb.cpp | 217 ++++++++++++++++++++++++++-------------------- 1 file changed, 122 insertions(+), 95 deletions(-) diff --git a/code/mainProb.cpp b/code/mainProb.cpp index b45b21b..4702451 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::data.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::data.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::data.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), @@ -129,7 +193,9 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Settings::data.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,124 +205,85 @@ 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(); + } } printErrorStats(errorStats); From 704b64bfcfd55dd3ad5765a84298298e5c61763d Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 13:31:59 +0200 Subject: [PATCH 09/16] Moved eval into own cpp to allow shorter compile times --- code/CMakeLists.txt | 2 ++ code/Eval.cpp | 70 +++++++++++++++++++++++++++++++++++++++++++++ code/Eval.h | 23 +++++++++++++++ code/filter.h | 64 ++++------------------------------------- 4 files changed, 101 insertions(+), 58 deletions(-) create mode 100644 code/Eval.cpp create mode 100644 code/Eval.h diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index c33101e..b3d15b9 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -38,6 +38,7 @@ FILE(GLOB HEADERS trilateration.h Plotta.h misc.h + Eval.h ) @@ -48,6 +49,7 @@ FILE(GLOB SOURCES mainFtm.cpp mainTrilat.cpp mainProb.cpp + Eval.cpp ) diff --git a/code/Eval.cpp b/code/Eval.cpp new file mode 100644 index 0000000..11b0172 --- /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::data.CurrentPath.nucInfo(nucIndex).position; + // particlePos.z = 1.3; // smartphone höhe + const float apDist = particlePos.getDistance(apPos); + + // compute ftm distance + float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; + float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset + + float rssi_pathloss = Settings::data.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/filter.h b/code/filter.h index 9105d76..fdff44d 100644 --- a/code/filter.h +++ b/code/filter.h @@ -34,6 +34,7 @@ #include #include "FtmKalman.h" +#include "Eval.h" struct MyState { @@ -269,72 +270,19 @@ struct MyPFEval : public SMC::ParticleFilterEvaluation { virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { double sum = 0; - + #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; - for (WiFiMeasurement wifi : observation.ftm) + if (!Settings::UseKalman) { - if (wifi.getNumSuccessfulMeasurements() < 3) - { - continue; - } - - const MACAddress& mac = wifi.getAP().getMAC(); - int nucIndex = Settings::nucIndex(mac); - - const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; - Point3 particlePos = p.state.pos.pos; - particlePos.z = 1.3; // smartphone höhe - const float apDist = particlePos.getDistance(apPos); - - // compute ftm distance - float ftm_offset = Settings::data.CurrentPath.NUCs.at(mac).ftm_offset; - float ftmDist = wifi.getFtmDist() + ftm_offset; // in m; plus static offset - - float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(mac).rssi_pathloss; - float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); - - - if (ftmDist > 0) - { - double sigma = wifi.getFtmDistStd()*wifi.getFtmDistStd(); // 3.5; // TODO - - if (sigma == 0) - { - sigma = 38*38; - } - - if (Settings::UseKalman) - { - Kalman& kalman = ftmKalmanFilters->at(mac); - ftmDist = kalman.predict(observation.currentTime, ftmDist); - sigma = kalman.P(0, 0); - - //Assert::isTrue(sigma > 0, "sigma"); - if (sigma <= 0) - { - sigma = 38 * 38; - } - - double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - - pFtm *= x; - } - else - { - double x = Distribution::Normal::getProbability(ftmDist, std::sqrt(sigma), apDist); - - pFtm *= x; - } - } + kalmanFilters = nullptr; } - - double prob = pFtm; + double prob = ftmEval(SensorMode::FTM, observation.currentTime, p.state.pos.pos, observation.ftm, kalmanFilters); if (assignProps) p.weight = prob; From 9b9feaa2c2a53075192085da82ad3864fbab4a10 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 13:37:06 +0200 Subject: [PATCH 10/16] Removed unused stuff from settings.h --- code/Settings.h | 91 +++---------------------------------------------- 1 file changed, 4 insertions(+), 87 deletions(-) diff --git a/code/Settings.h b/code/Settings.h index b95f125..1163c53 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -2,95 +2,10 @@ #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/"; @@ -111,7 +26,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) @@ -122,7 +39,7 @@ namespace Settings { case 1: return NUC2; case 2: return NUC3; case 3: return NUC4; - default: assert(false); + default: assert(false); return NUC1; } } From f9d2b10839e3a569d6b261ed9df3040e71ae68d6 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 13:37:29 +0200 Subject: [PATCH 11/16] Changed random transition, using weightes avg as estimation, removed ftm offsets from path7 --- code/Plotty.h | 4 ++-- code/Settings.h | 12 ++++++------ code/filter.h | 12 +++++++++--- code/main.cpp | 4 ++-- 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/code/Plotty.h b/code/Plotty.h index 0df0f3c..b76fef5 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -614,8 +614,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 1163c53..245602e 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -11,7 +11,7 @@ namespace Settings { const std::string errorDir = "../measurements/error/"; const std::string plotDataDir = "../plots/data/"; - const bool UseKalman = true; + const bool UseKalman = false; /** describes one dataset (map, training, parameter-estimation, ...) */ @@ -228,10 +228,10 @@ namespace Settings { }, { // 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 + { 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 @@ -275,7 +275,7 @@ namespace Settings { }; - const DataSetup CurrentPath = Path8; + const DataSetup CurrentPath = Path7; } data; diff --git a/code/filter.h b/code/filter.h index fdff44d..d2ebd66 100644 --- a/code/filter.h +++ b/code/filter.h @@ -180,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 { @@ -189,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; } } }; diff --git a/code/main.cpp b/code/main.cpp index 2b93956..e2db731 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -263,8 +263,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str //auto trans = std::make_unique(); auto resample = std::make_unique>(); - //auto estimate = std::make_unique>(); - auto estimate = std::make_unique>(); + auto estimate = std::make_unique>(); + //auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); From 5b20b67e5f496dc75cd67b2c4e19d79aad3d58f2 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 13:59:58 +0200 Subject: [PATCH 12/16] Moved kalman computation into cpp to improve compile speed --- code/CMakeLists.txt | 1 + code/FtmKalman.cpp | 51 ++++++++++++++++++++++++++++++++++++++++++ code/FtmKalman.h | 54 ++++----------------------------------------- 3 files changed, 56 insertions(+), 50 deletions(-) create mode 100644 code/FtmKalman.cpp diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index b3d15b9..3afaa25 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -50,6 +50,7 @@ FILE(GLOB SOURCES mainTrilat.cpp mainProb.cpp Eval.cpp + FtmKalman.cpp ) 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); }; From 066b7791d12a61e9ea72656267c96a4e7391d462 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 14:00:05 +0200 Subject: [PATCH 13/16] Fixed encoding --- code/Eval.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/Eval.cpp b/code/Eval.cpp index 11b0172..f8edbad 100644 --- a/code/Eval.cpp +++ b/code/Eval.cpp @@ -19,7 +19,7 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& int nucIndex = Settings::nucIndex(mac); const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; - // particlePos.z = 1.3; // smartphone höhe + // particlePos.z = 1.3; // smartphone höhe const float apDist = particlePos.getDistance(apPos); // compute ftm distance From a4669a8b415d5a130032dbaf0479ff0c8cc26f96 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 14:02:09 +0200 Subject: [PATCH 14/16] Removed obsolete mainFtm.cpp --- code/CMakeLists.txt | 2 - code/main.cpp | 2 - code/mainFtm.cpp | 278 -------------------------------------------- code/mainFtm.h | 4 - 4 files changed, 286 deletions(-) delete mode 100644 code/mainFtm.cpp delete mode 100644 code/mainFtm.h diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt index 3afaa25..af2a3be 100644 --- a/code/CMakeLists.txt +++ b/code/CMakeLists.txt @@ -34,7 +34,6 @@ FILE(GLOB HEADERS Settings.h FtmKalman.h main.h - mainFtm.h trilateration.h Plotta.h misc.h @@ -46,7 +45,6 @@ FILE(GLOB SOURCES ../../Indoor/lib/tinyxml/tinyxml2.cpp ../../Indoor/lib/Recast/*.cpp main.cpp - mainFtm.cpp mainTrilat.cpp mainProb.cpp Eval.cpp diff --git a/code/main.cpp b/code/main.cpp index e2db731..7ba79d3 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -411,8 +411,6 @@ int main(int argc, char** argv) return mainTrilat(argc, argv); } - //mainFtm(argc, argv); - //return 0; CombinedStats statsAVG; CombinedStats statsMedian; CombinedStats statsSTD; diff --git a/code/mainFtm.cpp b/code/mainFtm.cpp deleted file mode 100644 index d77e6f7..0000000 --- a/code/mainFtm.cpp +++ /dev/null @@ -1,278 +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; - - //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); - From 08be3e9af5059b693b0f46b52c56b061df317ab1 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 14:07:49 +0200 Subject: [PATCH 15/16] no message --- code/main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/code/main.cpp b/code/main.cpp index 7ba79d3..5d29db9 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -28,7 +28,6 @@ #include #include "FtmKalman.h" -#include "mainFtm.h" #include "misc.h" #include From 8236069094d5b8d3ded06655ffeea579aafcfc5d Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 8 Oct 2019 16:46:22 +0200 Subject: [PATCH 16/16] Refactored code to walk multiple runs --- code/Eval.cpp | 6 +-- code/Plotty.h | 7 ++- code/Settings.h | 19 +++++-- code/main.cpp | 122 ++++++++++++++++++++++++++++---------------- code/mainProb.cpp | 20 ++++---- code/mainTrilat.cpp | 12 ++--- code/misc.h | 22 +++++--- 7 files changed, 130 insertions(+), 78 deletions(-) diff --git a/code/Eval.cpp b/code/Eval.cpp index f8edbad..7e12d5e 100644 --- a/code/Eval.cpp +++ b/code/Eval.cpp @@ -18,15 +18,15 @@ double ftmEval(SensorMode UseSensor, const Timestamp& currentTime, const Point3& const MACAddress& mac = wifi.getAP().getMAC(); int nucIndex = Settings::nucIndex(mac); - const Point3 apPos = Settings::data.CurrentPath.nucInfo(nucIndex).position; + 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::data.CurrentPath.NUCs.at(mac).ftm_offset; + 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::data.CurrentPath.NUCs.at(mac).rssi_pathloss; + float rssi_pathloss = Settings::CurrentPath.NUCs.at(mac).rssi_pathloss; float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.getRSSI()); if (UseSensor == SensorMode::FTM) diff --git a/code/Plotty.h b/code/Plotty.h index b76fef5..d8f7344 100644 --- a/code/Plotty.h +++ b/code/Plotty.h @@ -580,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(); } diff --git a/code/Settings.h b/code/Settings.h index 245602e..10f3efc 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -10,8 +10,9 @@ namespace Settings { 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, ...) */ @@ -56,6 +57,7 @@ namespace Settings { }; struct DataSetup { + std::string name; std::string map; std::vector training; std::unordered_map NUCs; @@ -77,6 +79,7 @@ namespace Settings { const struct Data { const DataSetup Path0 = { + "path0", mapDir + "map0_ap_path0.xml", { dataDir + "Pixel2/Path0_0605.csv", @@ -94,6 +97,7 @@ namespace Settings { // 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", @@ -115,6 +119,7 @@ namespace Settings { // 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", @@ -137,6 +142,7 @@ namespace Settings { // 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", @@ -158,6 +164,7 @@ namespace Settings { // 4 Path: In Räumen const DataSetup Path4 = { + "path4", mapDir + "map2_ap_path2.xml", { dataDir + "Pixel2/path4/1560156876457_5_1.csv", @@ -180,6 +187,7 @@ namespace Settings { // 5 Path: In Räumen extendend const DataSetup Path5 = { + "path5", mapDir + "map2_ap_path2.xml", { dataDir + "Pixel2/path5/1560158444772_6_1.csv", @@ -202,6 +210,7 @@ namespace Settings { // 6 Path: SHL Path 1 const DataSetup Path6 = { + "path6", mapDir + "shl.xml", { dataDir + "Pixel2/path6/14681054221905_6_1.csv" @@ -219,6 +228,7 @@ namespace Settings { // 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", @@ -239,6 +249,7 @@ namespace Settings { // 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 @@ -257,6 +268,7 @@ namespace Settings { // 9 Path: SHL Path 3, NUCs stehen im Gang const DataSetup Path9 = { + "path9", mapDir + "shl_nuc_gang.xml", { dataDir + "Pixel2/path9/27911186920065.csv", @@ -273,11 +285,8 @@ namespace Settings { { 200, 201, 203, 104, 204, 205, 206, 207, 206, 208, 209, 210, 211, 212 }, true }; - - - const DataSetup CurrentPath = Path7; - } data; + static DataSetup CurrentPath = data.Path7; } diff --git a/code/main.cpp b/code/main.cpp index 5d29db9..c8d8b4a 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -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"; @@ -214,6 +214,13 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str 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>(); @@ -245,10 +252,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str 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 @@ -283,8 +290,8 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str std::vector timestamps; std::vector> gtDistances, ftmDistances, rssiDistances; // distance per AP - Plotta::Plotta errorPlot("errorPlot", Settings::plotDataDir + "errorData.py"); - Plotta::Plotta distsPlot("distsPlot", Settings::plotDataDir + "distances.py"); + Plotta::Plotta errorPlot("errorPlot", outputDir.string() + "/errorData.py"); + Plotta::Plotta distsPlot("distsPlot", outputDir.string() + "/distances.py"); for (const Offline::Entry& e : fr.getEntries()) { @@ -305,10 +312,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str Point2 gtPos = gtInterpolator.get(static_cast(ts.ms())).xy(); plot.setGroundTruth(Point3(gtPos.x, gtPos.y, 0.1)); - gtDistances.push_back({ gtPos.getDistance(Settings::data.CurrentPath.nucInfo(0).position.xy()), - gtPos.getDistance(Settings::data.CurrentPath.nucInfo(1).position.xy()), - gtPos.getDistance(Settings::data.CurrentPath.nucInfo(2).position.xy()), - gtPos.getDistance(Settings::data.CurrentPath.nucInfo(3).position.xy()) }); + 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; @@ -339,10 +346,10 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str { WiFiMeasurement wifi2 = obs.ftm[i]; - Point3 apPos = Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).position; + Point3 apPos = Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).position; K::GnuplotColor color; - switch (Settings::data.CurrentPath.nuc(wifi2.getAP().getMAC()).ID) + switch (Settings::CurrentPath.nuc(wifi2.getAP().getMAC()).ID) { case 1: color = K::GnuplotColor::fromRGB(0, 255, 0); break; case 2: color = K::GnuplotColor::fromRGB(0, 0, 255); break; @@ -390,6 +397,18 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str printErrorStats(errorStats); + std::ofstream plot_out; + plot_out.open(outputDir.string() + "/plot.gp"); + + plot.clearDistanceCircles(); + plot.saveToFile(plot_out); + + std::ofstream errorStats_out; + errorStats_out.open(outputDir.string() + "/error_stats.txt"); + printErrorStats(errorStats_out, errorStats); + + errorPlot.frame(); + //system("pause"); return errorStats; @@ -417,45 +436,58 @@ int main(int argc, char** argv) CombinedStats tmp; std::string evaluationName = "prologic/tmp"; + + std::vector setupsToRun = { Settings::data.Path7, + Settings::data.Path8, + Settings::data.Path9 }; - for (size_t walkIdx = 0; walkIdx < 1 /*Settings::data.CurrentPath.training.size()*/; walkIdx++) + for (Settings::DataSetup setupToRun : setupsToRun) { - std::cout << "Executing walk " << walkIdx << "\n"; - for (int i = 0; i < 5; ++i) + Settings::CurrentPath = setupToRun; + + + for (size_t walkIdx = 0; walkIdx < 1 /*Settings::CurrentPath.training.size()*/; walkIdx++) { - std::cout << "Start of iteration " << i << "\n"; + 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()); - statsSTD.ftm.add(tmp.ftm.getStdDev()); - statsQuantil.ftm.add(tmp.ftm.getQuantile(0.75)); + 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)); + 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 << "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::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; diff --git a/code/mainProb.cpp b/code/mainProb.cpp index 4702451..e410f1c 100644 --- a/code/mainProb.cpp +++ b/code/mainProb.cpp @@ -75,7 +75,7 @@ static void computeDensity(const BBox3& bbox, std::vector 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 obs; @@ -241,7 +241,7 @@ static CombinedStats run(Settings::DataSetup setup, int walkIdx, std::str plot.setCurEst(Point3(estPos.x, estPos.y, 0.1)); // Plot density - //plotDensity(plot, density); + plotDensity(plot, density); // Error distErrorFtm = gtPos.getDistance(estPos); @@ -304,14 +304,14 @@ int mainProp(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); 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 008836a..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); @@ -213,14 +213,14 @@ int mainTrilat(int argc, char** argv) std::string evaluationName = "prologic/tmp"; - for (size_t walkIdx = 0; walkIdx < Settings::data.CurrentPath.training.size(); 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 e565f34..535d0c0 100644 --- a/code/misc.h +++ b/code/misc.h @@ -48,15 +48,21 @@ 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) { - 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; + return printErrorStats(std::cout, errorStats); } @@ -94,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);