FHWS/IPIN2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

eval final version jopooooooooo

Browse Source
This commit is contained in:
toni
2017-05-03 16:47:21 +02:00
parent ef74eafb60
commit b4720ae288
5 changed files with 129 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
code/filter/KLB.h
View File

@@ -212,6 +212,7 @@ struct ModeProbabilityTransitionNormal : public K::MarkovTransitionProbability<M
//create the matrix
Eigen::MatrixXd m(2,2);
m << expKld, 1.0 - expKld, 1 - qualityWifi, qualityWifi;
//m << expKld, 1.0 - expKld, 0.00000000000000000001, 0.99999999999999999999;
return m;

10
code/filter/Logic.h
View File

@@ -175,13 +175,13 @@ struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{
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<MyState, MyObs> {
// 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

2
code/filter/Structs.h
View File

@@ -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;

149
code/main.cpp
View File

@@ -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<int> gtPath) {
K::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
std::vector<double> 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<int> 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<MyNode> grid(20);
@@ -132,18 +131,15 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
onp.open(setup.grid);
GridFactory<MyNode> 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<int> gtPa
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
// mode 1
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInit(grid, 1));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode1(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 1));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, initMode1, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -183,8 +178,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
modes.push_back(mode1);
// mode 2
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2));
std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInit(grid, 2));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> initMode2(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f, 2));
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, initMode2, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
@@ -201,8 +196,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
//IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(new ModeProbabilityTransitionNormal(Settings::Mixing::lambda)));
//IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransitionNormal>(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<int> gtPa
K::Statistics<float> errorStats;
//file writing for error data
const long int t = static_cast<long int>(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<int> 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<int> 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<int>(ts.sec()) / 60;
plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(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<int>(ts.sec()) / 60;
//plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(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<int> 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<int> 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<int> 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<float> statsAVG;
K::Statistics<float> statsMedian;
K::Statistics<float> statsSTD;
K::Statistics<float> statsQuantil;
K::Statistics<float> 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);
//run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
//}
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));
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;
}

45
tex/chapters/experiments.tex
View File

@@ -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)