diff --git a/code/filter/KLB.h b/code/filter/KLB.h index a133998..5fef878 100644 --- a/code/filter/KLB.h +++ b/code/filter/KLB.h @@ -212,6 +212,7 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability{ p.state.position = newPosition; }else{ //no new position! - #pragma omp atomic - noNewPositionCounter++; + //#pragma omp atomic + //noNewPositionCounter++; } } - std::cout << noNewPositionCounter << std::endl; + //std::cout << noNewPositionCounter << std::endl; } }; @@ -323,8 +323,8 @@ struct PFEval : public K::ParticleFilterEvaluation { // Point3 posOld_m = p.state.positionOld.inMeter(); const double pWifi = getWIFI(observation, wifiObs, p.state.position); - const double pBaroPressure = getStairProb(p, observation.activity); - //const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure); + //const double pBaroPressure = getStairProb(p, observation.activity); + const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure); //const double pBeacon = getBEACON(observation, p.state.position); //small checks diff --git a/code/filter/Structs.h b/code/filter/Structs.h index 8477dd2..7b28a46 100644 --- a/code/filter/Structs.h +++ b/code/filter/Structs.h @@ -22,7 +22,7 @@ struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ { - static Floorplan::IndoorMap* map; + //static Floorplan::IndoorMap* map; float relativePressure = 0.0f; diff --git a/code/main.cpp b/code/main.cpp index 3953e88..5193aba 100755 --- a/code/main.cpp +++ b/code/main.cpp @@ -76,7 +76,7 @@ struct Data { DataSetup IPIN2017 = { - mapDir + "SHL38.xml", + mapDir + "SHL39.xml", { dataDir + "ipin2017/nogps/i-building/path1/1489769326868.csv", @@ -96,22 +96,22 @@ struct Data { mapDir + "wifi_fp_all.dat", 40, VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, - mapDir + "grid_SHL38.dat" + mapDir + "grid_SHL39.dat" }; } data; -Floorplan::IndoorMap* MyState::map; +//Floorplan::IndoorMap* MyState::map; -void run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { +K::Statistics run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { std::vector kld_data; std::vector quality_data; // load the floorplan Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map); - MyState::map = map; + //MyState::map = map; WiFiModelLogDistCeiling WiFiModel(map); WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); @@ -121,7 +121,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); //Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml"); - // build the grid std::ifstream inp(setup.grid, std::ifstream::binary); Grid grid(20); @@ -132,18 +131,15 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa onp.open(setup.grid); GridFactory factory(grid); factory.build(map); + + Importance::addImportance(grid); + WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); + grid.write(onp); } else { grid.read(inp); } - - // add node-importance - Importance::addImportance(grid); - - // stamp WiFi signal-strengths onto the grid - WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); - // reading file Offline::FileReader fr(setup.training[numFile]); @@ -170,9 +166,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa //std::shared_ptr> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)); // mode 1 - //std::shared_ptr> initMode1(new PFInit(grid, 1)); - std::shared_ptr> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1)); - + std::shared_ptr> initMode1(new PFInit(grid, 1)); + //std::shared_ptr> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1)); K::ParticleFilterMixing mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability); mode1.setTransition(std::shared_ptr(new PFTrans(grid, &ctrl))); mode1.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); @@ -183,8 +178,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa modes.push_back(mode1); // mode 2 - //std::shared_ptr> initMode2(new PFInit(grid, 2)); - std::shared_ptr> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2)); + std::shared_ptr> initMode2(new PFInit(grid, 2)); + //std::shared_ptr> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2)); K::ParticleFilterMixing mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability); mode2.setTransition(std::shared_ptr(new PFTransSimple(grid))); mode2.setEvaluation(std::shared_ptr(new PFEval(WiFiModel, beaconModel, grid))); @@ -201,8 +196,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa K::InteractingMultipleModelParticleFilter IMMAPF(modes, transitionProbabilityMatrix); IMMAPF.setMixingSampler(std::unique_ptr>(new K::MixingSamplerDivergency())); IMMAPF.setJointEstimation(std::unique_ptr>(new K::JointEstimationPosteriorOnly())); - IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransition(grid, Settings::Mixing::lambda))); - //IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda))); + //IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransition(grid, Settings::Mixing::lambda))); + IMMAPF.setMarkovTransitionProbability(std::unique_ptr(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda))); Timestamp lastTimestamp = Timestamp::fromMS(0); @@ -216,10 +211,9 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa K::Statistics errorStats; - //file writing for error data const long int t = static_cast(time(NULL)); - const std::string evalDir = errorDir + std::to_string(t); + const std::string evalDir = errorDir + "final_" + std::to_string(t); if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){ Assert::doThrow("Eval folder couldn't be created!"); } @@ -227,6 +221,12 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa std::ofstream errorFile; errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + std::ofstream kldFile; + kldFile.open (evalDir + "/kld_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + std::ofstream wifiFile; + wifiFile.open (evalDir + "/wifi_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + // parse each sensor-value within the offline data for (const Offline::Entry& e : fr.getEntries()) { @@ -297,30 +297,30 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa //turn angle plot static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad; - plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); + //plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); //motion angle plot static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad; - plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); + //plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); - plot.setEst(estPos); - plot.setGT(gtPos); - plot.addParticles1(IMMAPF.getModes()[0].getParticles()); - plot.addParticles2(IMMAPF.getModes()[1].getParticles()); + //plot.setEst(estPos); + //plot.setGT(gtPos); + //plot.addParticles1(IMMAPF.getModes()[0].getParticles()); + //plot.addParticles2(IMMAPF.getModes()[1].getParticles()); plot.addEstimationNode(estPos); - plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter()); + //plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter()); //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; //plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n"; - int minutes = static_cast(ts.sec()) / 60; - plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast(static_cast(ts.sec())%60) << "'\n"; - plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n"; - plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n"; + //int minutes = static_cast(ts.sec()) / 60; + //plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast(static_cast(ts.sec())%60) << "'\n"; + //plot.gp << "set label 1003 at screen 0.02, 0.92 'KLD: " << ":" << kld_data.back() << "'\n"; + //plot.gp << "set label 1004 at screen 0.90, 0.98 'act:" << obs.activity << "'\n"; - plot.gp << "set label 1011 at screen 0.90, 0.10 'Wifi Quality:" << __QUALITY << "'\n"; - plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n"; - plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n"; + //plot.gp << "set label 1011 at screen 0.90, 0.10 'Wifi Quality:" << __QUALITY << "'\n"; + //plot.gp << "set label 1005 at screen 0.90, 0.08 'Prob. Mode1:" << IMMAPF.getModes()[0].getModePosteriorProbability() << "'\n"; + //plot.gp << "set label 1006 at screen 0.90, 0.06 'Prob. Mode2:" << IMMAPF.getModes()[1].getModePosteriorProbability() << "'\n"; int ones = 0; int twos = 0; @@ -342,16 +342,20 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa } } - plot.gp << "set label 1007 at screen 0.90, 0.04 'Part. Mode1:" << ones << "'\n"; - plot.gp << "set label 1008 at screen 0.90, 0.02 'Part. Mode2:" << twos << "'\n"; + //plot.gp << "set label 1007 at screen 0.90, 0.04 'Part. Mode1:" << ones << "'\n"; + //plot.gp << "set label 1008 at screen 0.90, 0.02 'Part. Mode2:" << twos << "'\n"; // error between GT and estimation float err_m = gtPos.getDistance(estPos); errorStats.add(err_m); errorFile << err_m << "\n"; - plot.show(); - usleep(10*10); + kldFile << kld_data.back() << "\n"; + + wifiFile << __QUALITY << "\n"; + + //plot.show(); + //usleep(10*10); lastTimestamp = ts; @@ -362,13 +366,15 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa } errorFile.close(); + kldFile.close(); + wifiFile.close(); std::cout << "Statistical Analysis: " << std::endl; - std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; //Write the current plotti buffer into file std::ofstream plotFile; - plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); + plotFile.open(evalDir + "/gnuplot_" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp"); plot.saveToFile(plotFile); plotFile.close(); @@ -435,6 +441,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector gtPa std::cout << "finished" << std::endl; sleep(1); + return errorStats; + } int main(int argc, char** argv) { @@ -442,23 +450,56 @@ int main(int argc, char** argv) { //Testing files //run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor - //for(int i = 0; i < 5; ++i){ + K::Statistics statsAVG; + K::Statistics statsMedian; + K::Statistics statsSTD; + K::Statistics statsQuantil; + K::Statistics tmp; + + for(int i = 0; i < 25; ++i){ // Settings::useKLB = false; -// //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1); -// run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1); -// run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2); +// errorPair = run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1); +// run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1); +// errorPair = run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2); // //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2); // run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); // //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); - //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1); + tmp = run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); + statsMedian.add(tmp.getMedian()); + statsAVG.add(tmp.getAvg()); + statsSTD.add(tmp.getStdDev()); + statsQuantil.add(tmp.getQuantile(0.75)); - run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); - //run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1); - //run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2); - //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2); + tmp = run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3); + statsMedian.add(tmp.getMedian()); + statsAVG.add(tmp.getAvg()); + statsSTD.add(tmp.getStdDev()); + statsQuantil.add(tmp.getQuantile(0.75)); - //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3); - //} + 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 (errorDir + "/finalResults_Path3_AbsBaro.csv"); + + 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.close(); + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + + return 0; } diff --git a/tex/chapters/experiments.tex b/tex/chapters/experiments.tex index 52f97ce..8d4fec1 100644 --- a/tex/chapters/experiments.tex +++ b/tex/chapters/experiments.tex @@ -1,5 +1,7 @@ \section{Experiments} +% allgemeine infos über pfade und gebäude. wo +% bild: mit pfaden drauf und eventl. wifi qualität in jeweiligen bereichen? (kann frank das) All upcoming experiments were carried out on four floors of a \SI{77}{m} x \SI{55}{m} sized faculty building. It includes several staircases and elevators and has a ceiling height of about \SI{3}{m}. Nevertheless, the grid was generated for the complete campus and thus outdoor areas like the courtyard are also walkable. @@ -11,40 +13,49 @@ As mentioned before, we omit any time-consuming calibration processes and use th The position of the access-points (about five per floor) is known beforehand. Due to legal terms, we are not allowed to depict their positions and therefore omit this information within the figures. +% gewählte parameter (auch mal die optimieren wifi parameter testen) We arranged three distinct walks (see also fig. \ref{}). The measurements for the walks were recorded using a Motorola Nexus 6 at 2.4 GHz band only. The computation was done offline as described in algorithm \ref{}. -For each walk we deployed $xx$ MC runs using 5000 Particles. +For each walk we deployed $xx$ MC runs using 5000 Particles for each mode. Instead of an initial position and heading, all walks start with a uniform distribution (random position and heading) as prior. For the filtering we used $\sigma_\text{wifi} = 8.0$ as uncertainties, both growing with each measurement's age. While the pressure change was assumed to be \SI{0.105}{$\frac{\text{\hpa}}{\text{\meter}}$}, all other barometer-parameters are determined automatically. The step size $\mStepSize$ for the transition was configured to be \SI{70}{\centimeter} with an allowed derivation of \SI{10}{\percent}. The heading deviation was set to \SI{25}{\degree}. -KLD with normal dist and kernel density drawing from grid. - +% wie für die kld gezogen? begründen warum wir nun keine parzenschätzung machen (weil ähnliche ergebnisse) +To calculate \eqref{equ:KLD} and thus the Kullback-Leibler divergence, we need to sample densities from both modes likewise. +The grid is suitable for this purpose. +However, sampling at any vertex $\mVertexA$ of the grid, given just a set of random variables (particles), is not the easiest task. +We need to estimate the posterior distribution given by the respective particle sets. +A common way is to deploy a kernel density estimation using a Gaussian distribution as kernel. +The density of a specific point $\hat\mStateVec_{t} = \fPos{\mVertexA}$ is then given by + % + \begin{equation} + p(\hat\mStateVec_{t} \mid m_t, \mObsVec_{1:t}) = \sum_{i=1}^{N_{m_t}} \mathcal{N}(d^i_{\text{KL}} \mid 0, \sigma_{\text{KL}}) + \enspace , + \end{equation} + % +while $d^i_{\text{KL}}$ is the euclidean distance between the considered point's $\hat\mStateVec_{t}$ and all particles $\fPos{\vec{X}_t^{i,m_t}}$ of the mode. The variance $\sigma_{\text{KL}}$ is set to \SI{1}{m}. +It is well known, that the computation of the kernel density estimation is rather slow, thus we also used a much simpler estimation by assuming a multivariate Gaussian distribution for both modes. +Here, the mean is given by weighted arithmetic mean of the particles and the variance is defined by the sample covariance matrix. +% ground truth The ground truth is measured by recording a timestamp at marked spots on the walking route. When passing a marker, the pedestrian clicked a button on the smartphone application. Between two consecutive points, a constant movement speed is assumed. Thus, the ground truth might not be \SI{100}{\percent} accurate, but fair enough for error measurements. The approximation error is then calculated by comparing the interpolated ground truth position with the current estimation \cite{Fetzer2016OMC}. - - - - -% allgemeine infos über pfade und gebäude. wo -% bild: mit pfaden drauf und eventl. wifi qualität in jeweiligen bereichen? (kann frank das) - -% gewählte parameter (auch mal die optimieren wifi parameter testen) - -% wie für die kld gezogen? begründen warum wir nun keine parzenschätzung machen (weil ähnliche ergebnisse) - -% ground truth - % maß für die streuung der verteilung (diversity von partikeln) + +error at the beginning always very high. about 44 meters. therefore the median is better value oder 75 quantil. + % zeigen das es stucken verhindert (eventl. hier eine andere aufnahme die mitten drinnen stecken bleibt) -% bild: stucken im raum + nicht mehr stucken im raum +% bild: stucken im raum + nicht mehr stucken im raum + kld mit anzeigen + + + % zeigen das schlechtes wi-fi (zu hohe diversity) behoben wird. % bild: lauf auf der rechten seite des gebäudes zeige mit und ohne wifi faktor (schlechtes wifi einzeichnen)