From 24bbc56f28d83aa56dd920facba18c0510be3606 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 25 Sep 2019 09:30:16 +0200 Subject: [PATCH] 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +