toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

activated interval smoothing for evaluation

Browse Source
This commit is contained in:
toni
2016-05-03 11:17:08 +02:00
parent 8b7b38dd5a
commit 1e8f4259a2
10 changed files with 272 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt
View File

@@ -67,7 +67,7 @@ ADD_DEFINITIONS(
-O2
-DWITH_TESTS
-DWITH_ASSERTIONS
#-DUSE_FIXED_SEED
)
endif()

2
code/CMakeLists.txt.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.6.0, 2016-04-26T17:01:57. -->
<!-- Written by QtCreator 3.6.0, 2016-05-03T09:50:19. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

6
code/Settings.h
View File

@@ -17,11 +17,11 @@ namespace MiscSettings {
const int timeSteps = 500;
const int numParticles = 500;
const int numParticles = 2500;
const int numBSParticles = 50;
const int numBSParticles = 500;
const int lag = 15;
const int lag = 5;
const int fixedLagGap = 1;

2
code/eval/FixedLagEvalBase.h
View File

@@ -457,7 +457,7 @@ public:
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setTerminal("png", K::GnuplotSize(2560 * 2.54, 1440 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position

101
code/eval/SmoothingEval1.h
View File

@@ -21,7 +21,7 @@
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h>
class SmoothingEval1 : public FixedLagEvalBase {
class SmoothingEval1 : public SmoothingEvalBase {
public:
@@ -81,7 +81,7 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
@@ -112,13 +112,13 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
// ============================================================ Dijkstra ============================================== //
@@ -146,13 +146,13 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Dijkstra
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
bf->setTransition(std::unique_ptr<MySmoothingTransition>( new MySmoothingTransition(&grid)) );
}
@@ -175,7 +175,7 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -185,7 +185,7 @@ public:
}
void bergwerk_path1_nexus_simple() {
runName = "bergwerk_path1_nexus_simple_fixedlag";
runName = "bergwerk_path1_nexus_simple_interval";
bergwerk_path1_nexus();
for (auto& n : grid) {n.imp = 1;}
@@ -198,7 +198,7 @@ public:
void bergwerk_path1_nexus_imp() {
runName = "bergwerk_path1_nexus_importance_fixedlag";
runName = "bergwerk_path1_nexus_importance_interval";
bergwerk_path1_nexus();
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
@@ -206,9 +206,9 @@ public:
}
void bergwerk_path1_nexus_multi() {
void bergwerk_path1_nexus_multi(std::string name) {
runName = "bergwerk_path1_nexus_multi_fixedlag";
runName = name;
bergwerk_path1_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
@@ -219,7 +219,7 @@ public:
void bergwerk_path1_nexus_shortest() {
runName = "bergwerk_path1_nexus_shortest_fixedlag";
runName = "bergwerk_path1_nexus_shortest_interval";
bergwerk_path1_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
@@ -243,7 +243,7 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -254,7 +254,7 @@ public:
void bergwerk_path2_nexus_simple() {
runName = "bergwerk_path2_nexus_simple_fixedlag";
runName = "bergwerk_path2_nexus_simple_interval";
bergwerk_path2_nexus();
for (auto& n : grid) {n.imp = 1;} // remove importance
@@ -266,7 +266,7 @@ public:
void bergwerk_path2_nexus_imp() {
runName = "bergwerk_path2_nexus_importance_fixedlag";
runName = "bergwerk_path2_nexus_importance_interval";
bergwerk_path2_nexus();
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
@@ -274,9 +274,9 @@ public:
}
void bergwerk_path2_nexus_multi() {
void bergwerk_path2_nexus_multi(std::string name) {
runName = "bergwerk_path2_nexus_multi_fixedlag";
runName = name;
bergwerk_path2_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
@@ -287,7 +287,7 @@ public:
void bergwerk_path2_nexus_shortest() {
runName = "bergwerk_path2_nexus_shortest_fixedlag";
runName = "bergwerk_path2_nexus_shortest_interval";
bergwerk_path2_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
@@ -312,7 +312,7 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -323,7 +323,7 @@ public:
void bergwerk_path3_nexus_simple() {
runName = "bergwerk_path3_nexus_simple_fixedlag";
runName = "bergwerk_path3_nexus_simple_interval";
bergwerk_path3_nexus();
for (auto& n : grid) {n.imp = 1;} // remove importance
@@ -335,7 +335,7 @@ public:
void bergwerk_path3_nexus_imp() {
runName = "bergwerk_path3_nexus_importance_fixedlag";
runName = "bergwerk_path3_nexus_importance_interval";
bergwerk_path3_nexus();
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
@@ -343,9 +343,9 @@ public:
}
void bergwerk_path3_nexus_multi() {
void bergwerk_path3_nexus_multi(std::string name) {
runName = "bergwerk_path3_nexus_multi_fixedlag";
runName = name;
bergwerk_path3_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
@@ -356,7 +356,7 @@ public:
void bergwerk_path3_nexus_shortest() {
runName = "bergwerk_path3_nexus_shortest_fixedlag";
runName = "bergwerk_path3_nexus_shortest_interval";
bergwerk_path3_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
@@ -380,7 +380,7 @@ public:
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -391,7 +391,7 @@ public:
void bergwerk_path4_nexus_simple() {
runName = "bergwerk_path4_nexus_simple_fixedlag";
runName = "bergwerk_path4_nexus_simple_interval";
bergwerk_path4_nexus();
for (auto& n : grid) {n.imp = 1;} // remove importance
@@ -403,7 +403,7 @@ public:
void bergwerk_path4_nexus_imp() {
runName = "bergwerk_path4_nexus_importance_fixedlag";
runName = "bergwerk_path4_nexus_importance_interval";
bergwerk_path4_nexus();
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
@@ -411,9 +411,9 @@ public:
}
void bergwerk_path4_nexus_multi() {
void bergwerk_path4_nexus_multi(std::string name) {
runName = "bergwerk_path4_nexus_multi_fixedlag";
runName = name;
bergwerk_path4_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
@@ -424,7 +424,7 @@ public:
void bergwerk_path4_nexus_shortest() {
runName = "bergwerk_path4_nexus_shortest_fixedlag";
runName = "bergwerk_path4_nexus_shortest_interval";
bergwerk_path4_nexus();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
@@ -449,7 +449,7 @@ public:
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -459,15 +459,15 @@ public:
}
void bergwerk_path1_galaxy_simple() {
runName = "bergwerk_path1_galaxy_simple_fixedlag";
runName = "bergwerk_path1_galaxy_simple_interval";
bergwerk_path1_galaxy();
for (auto& n : grid) {n.imp = 1;} // remove importance
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path1_galaxy_multi() {
runName = "bergwerk_path1_galaxy_multi_fixedlag";
void bergwerk_path1_galaxy_multi(std::string name) {
runName = name;
bergwerk_path1_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
@@ -476,7 +476,7 @@ public:
}
void bergwerk_path1_galaxy_shortest() {
runName = "bergwerk_path1_galaxy_shortest_fixedlag";
runName = "bergwerk_path1_galaxy_shortest_interval";
bergwerk_path1_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
@@ -500,7 +500,7 @@ public:
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -510,15 +510,16 @@ public:
}
void bergwerk_path2_galaxy_simple() {
runName = "bergwerk_path2_galaxy_simple_fixedlag";
runName = "bergwerk_path2_galaxy_simple_interval";
bergwerk_path2_galaxy();
for (auto& n : grid) {n.imp = 1;} // remove importance
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path2_galaxy_multi() {
runName = "bergwerk_path2_galaxy_multi_fixedlag";
void bergwerk_path2_galaxy_multi(std::string name) {
//runName = "bergwerk_path2_galaxy_multi_interval";
runName = name;
bergwerk_path2_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
@@ -527,7 +528,7 @@ public:
}
void bergwerk_path2_galaxy_shortest() {
runName = "bergwerk_path2_galaxy_shortest_fixedlag";
runName = "bergwerk_path2_galaxy_shortest_interval";
bergwerk_path2_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
@@ -551,7 +552,7 @@ public:
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -561,15 +562,15 @@ public:
}
void bergwerk_path3_galaxy_simple() {
runName = "bergwerk_path3_galaxy_simple_fixedlag";
runName = "bergwerk_path3_galaxy_simple_interval";
bergwerk_path3_galaxy();
for (auto& n : grid) {n.imp = 1;} // remove importance
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path3_galaxy_multi() {
runName = "bergwerk_path3_galaxy_multi_fixedlag";
void bergwerk_path3_galaxy_multi(std::string name) {
runName = name;
bergwerk_path3_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
@@ -578,7 +579,7 @@ public:
}
void bergwerk_path3_galaxy_shortest() {
runName = "bergwerk_path3_galaxy_shortest_fixedlag";
runName = "bergwerk_path3_galaxy_shortest_interval";
bergwerk_path3_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
@@ -604,7 +605,7 @@ public:
smoothing_baro_sigma = 0.15;
bool smoothing_resample = false;
smoothing_time_delay = 1;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -614,15 +615,15 @@ public:
}
void bergwerk_path4_galaxy_simple() {
runName = "bergwerk_path4_galaxy_simple_fixedlag";
runName = "bergwerk_path4_galaxy_simple_interval";
bergwerk_path4_galaxy();
for (auto& n : grid) {n.imp = 1;} // remove importance
GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
}
void bergwerk_path4_galaxy_multi() {
runName = "bergwerk_path4_galaxy_multi_fixedlag";
void bergwerk_path4_galaxy_multi(std::string name) {
runName = name;
bergwerk_path4_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
@@ -632,7 +633,7 @@ public:
void bergwerk_path4_galaxy_shortest() {
runName = "bergwerk_path4_galaxy_shortest_fixedlag";
runName = "bergwerk_path4_galaxy_shortest_interval";
bergwerk_path4_galaxy();
MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);

205
code/eval/SmoothingEvalBase.h
View File

@@ -8,6 +8,9 @@
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
#include <KLib/math/statistics/Statistics.h>
#include "GroundTruthWay.h"
@@ -19,13 +22,16 @@
#include "../particles/MyInitializer.h"
#include "../particles/smoothing/MySmoothingTransition.h"
#include "../particles/smoothing/MySmoothingTransitionSimple.h"
#include "../particles/smoothing/MySmoothingTransitionExperimental.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"
#include "../reader/SensorReaderAccel.h"
#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"
#include "../lukas/ActivityDetection.h"
#include "../toni/BarometerSensorReader.h"
@@ -33,8 +39,6 @@
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
static int smoothing_time_delay = 0;
float stepSize = 0.71;
class SmoothingEvalBase {
@@ -71,6 +75,8 @@ protected:
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
float stepSize = 0.71;
public:
SmoothingEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
@@ -130,7 +136,7 @@ public:
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
//const int s_linearAcceleration = 2;
const int s_accel = 0;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
@@ -138,6 +144,8 @@ public:
//Create an BarometerSensorReader
BarometerSensorReader baroSensorReader;
// activity detection
ActivityDetection actDet;
//Read all turn Observations
while(srt->hasNext()) {
@@ -186,8 +194,16 @@ public:
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
K::Statistics<float> statsDistFiltering;
K::Statistics<float> statsDistSmoothing;
int cnt = 0;
// error per update-step
std::vector<float> errorsNorm;
std::vector<float> errorsSmooth;
std::vector<float> errorsDistNorm;
std::vector<float> errorsDistSmooth;
//stats file
std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
@@ -220,13 +236,21 @@ public:
case s_barometer: {
obs.barometer = baroSensorReader.readBarometer(se);
actDet.addBaro(baroSensorReader.getHPA(se));
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_accel: {
float acc[3];
SensorReaderAccel sre; sre.read(se, acc);
actDet.addAccel(acc);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
@@ -251,6 +275,13 @@ public:
}
// currently detected activity
// TODO: feed sensor values!
ctrl.currentActivitiy = actDet.getCurrentActivity();
// this is just for testing purposes
obs.currentActivity = actDet.getCurrentActivity();
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
@@ -266,16 +297,24 @@ public:
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 750;
const int offset = 0;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
pathEst.push_back(curEst);
const float err = diff.length();
const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth
++cnt;
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
pathEst.push_back(curEst);
const float err = diff.length();
if (cnt > 35) {
statsFiltering.add(err);
std::cout << statsFiltering.asString() << std::endl;
statsDistFiltering.add(errDist);
errorsNorm.push_back(err);
errorsDistNorm.push_back(errDist);
std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
@@ -326,28 +365,37 @@ public:
bf->reset();
MyState estBF;
int currentParticleSetToSmoothAtTimet = 0;
while(currentParticleSetToSmoothAtTimet != pfHistory.size() - 1){
//iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT)
for(int i = pfHistory.size() - 1; i >= 0 ; i -= MiscSettings::fixedLagGap){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[i]);
//iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT)
for(int i = pfHistory.size() - 1; i >= currentParticleSetToSmoothAtTimet; i -= smoothing_time_delay){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[i]);
}
smoothedEst.push_back(estBF.pCur);
}
const Point3 curSmoothedEst = estBF.pCur;
std::reverse(smoothedEst.begin(), smoothedEst.end());
smoothedEst.push_back(curSmoothedEst);
for(int t = 0; t < smoothedEst.size(); ++t){
const Point3 curSmoothedEst = smoothedEst[t];
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[currentParticleSetToSmoothAtTimet]);
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[t]);
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
const float errDistSmoothed = gtw.getMinDist(curSmoothedEst); // minimum distance between smoothed-estimation and ground-truth
statsSmoothing.add(errSmoothed);
std::cout << statsSmoothing.asString() << std::endl;
statsDistSmoothing.add(errDistSmoothed);
errorsSmooth.push_back(errSmoothed);
errorsDistSmooth.push_back(errDistSmoothed);
std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl;
// plot
@@ -356,7 +404,7 @@ public:
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(tsHistory[currentParticleSetToSmoothAtTimet]);
vis.setTimestamp(tsHistory[t]);
vis.addGroundTruth(gtw);
vis.addEstPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
@@ -365,6 +413,7 @@ public:
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
vis.gp << "set label 112 '" << "Act: " << actDet.toString() << "' at screen 0.1,0.15\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
@@ -377,12 +426,71 @@ public:
// prevent gnuplot errors
usleep(1000*33);
++currentParticleSetToSmoothAtTimet;
}
//statsout2.close();
{ // detailled error-description (normally filtered)
std::ofstream oError("/tmp/err_norm_" + runName + ".dat");
for (float f : errorsNorm) {oError << f << "\n";}
oError.close();
}
{ // detailled error-description (smothed)
std::ofstream oError("/tmp/err_smooth_" + runName + ".dat");
for (float f : errorsSmooth) {oError << f << "\n";}
oError.close();
}
{ // detailled distance-error-description (normally filtered)
std::ofstream oError("/tmp/err_dist_norm_" + runName + ".dat");
for (float f : errorsDistNorm) {oError << f << "\n";}
oError.close();
}
{ // detailled distance-error-description (smothed)
std::ofstream oError("/tmp/err_dist_smooth_" + runName + ".dat");
for (float f : errorsDistSmooth) {oError << f << "\n";}
oError.close();
}
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
//vis.setEstAndShould(curEst, curGT);
//vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
// append error for each run to a file
std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app);
oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n";
@@ -393,11 +501,22 @@ public:
oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n";
oSError.close();
// append error for each run to a file
std::ofstream oTDError("/tmp/errorsDistFiltering.txt", std::ios_base::app);
oTDError << runName << "\n\t"; statsDistFiltering.appendTo(oTDError); oTDError << "\n\n";
oTDError.close();
// append error for each run to a file
std::ofstream oSDError("/tmp/errorsDistSmoothing.txt", std::ios_base::app);
oSDError << runName << "\n\t"; statsDistSmoothing.appendTo(oSDError); oSDError << "\n\n";
oSDError.close();
// plot-data
std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); // ground truth
std::ofstream oEstN("/tmp/est_norm" + runName + ".dat"); vis.estPath.addDataTo(oEstN); oEstN.close(); // estimation via filter itself
std::ofstream oEstS("/tmp/est_smooth" + runName + ".dat"); vis.smoothPath.addDataTo(oEstS); oEstS.close(); // estimation via smoothing
std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close();
std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); // ground truth
std::ofstream oEstN("/tmp/est_norm_" + runName + ".dat"); vis.estPath.addDataTo(oEstN); oEstN.close(); // estimation via filter itself
std::ofstream oEstS("/tmp/est_smooth_" + runName + ".dat"); vis.smoothPath.addDataTo(oEstS); oEstS.close(); // estimation via smoothing
std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close();
std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
@@ -417,12 +536,28 @@ public:
oPlot << "unset ztics\n";
oPlot << "splot \\\n";
oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n";
oPlot << "'path_fixed_interval.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_norm_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
oPlot << "'est_smooth_fixed_interval.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";
oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n";
oPlot << "'path_" << runName << ".dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_norm_" << runName << ".dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
oPlot << "'est_smooth_" << runName << ".dat' skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";
oPlot.close();
{
std::ofstream oErrPlot("/tmp/plot_err_" + runName + ".gp");
oErrPlot << "plot \\\n";
oErrPlot << "'err_norm_" << runName << ".dat' with lines, \\\n";
oErrPlot << "'err_smooth_" << runName << ".dat' with lines";
oErrPlot.close();
}
{
std::ofstream oErrPlot("/tmp/plot_dist_err_" + runName + ".gp");
oErrPlot << "plot \\\n";
oErrPlot << "'err_dist_norm_" << runName << ".dat' with lines, \\\n";
oErrPlot << "'err_dist_smooth_" << runName << ".dat' with lines";
oErrPlot.close();
}
}
};

81
code/main.cpp
View File

@@ -87,77 +87,42 @@ void testModelWalk() {
int main(void) {
// testModelWalk();
{SmoothingEval1 eval;
eval.fixedIntervallSimpleTransPath4();
eval.run();}
{SmoothingEval1 eval;
eval.fixedIntervallSimpleTransPath4();
eval.run();}
{SmoothingEval1 eval;
eval.fixedIntervallSimpleTransPath4();
eval.run();}
{SmoothingEval1 eval;
eval.fixedIntervallSimpleTransPath4();
eval.run();}
//Eval1 eval;
//eval.bergwerk_path4_nexus_multi();
//eval.run();
//{SmoothingEval1 eval; eval.bergwerk_path1_nexus_simple(); eval.run();}
//{SmoothingEval1 eval; eval.bergwerk_path1_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path2_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_simple(); eval.run();}
// //{SmoothingEval1 eval; eval.bergwerk_path3_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_imp(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_multi(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_nexus_shortest(); eval.run();}
// {SmoothingEval1 eval;
// eval.fixedIntervallSimpleTransPath4();
// eval.run();}
for(int i = 0; i < 10; ++i){
//// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_multi(); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_shortest(); eval.run();}
//nexus
std::string name = "bergwerk_path1_nexus_simple_interval_" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path1_nexus_multi(name); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_multi(); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path2_galaxy_shortest(); eval.run();}
name = "bergwerk_path2_nexus_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path2_nexus_multi(name); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_multi(); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path3_galaxy_shortest(); eval.run();}
name = "bergwerk_path3_nexus_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path3_nexus_multi(name); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_simple(); eval.run();}
// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_multi(); eval.run();}
//// {SmoothingEval1 eval; eval.bergwerk_path4_galaxy_shortest(); eval.run();}
name = "bergwerk_path4_nexus_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path4_nexus_multi(name); eval.run();}
//galaxy
name = "bergwerk_path1_galaxy_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path1_galaxy_multi(name); eval.run();}
name = "bergwerk_path2_galaxy_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path2_galaxy_multi(name); eval.run();}
name = "bergwerk_path3_galaxy_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path3_galaxy_multi(name); eval.run();}
name = "bergwerk_path4_galaxy_simple_interval" + std::to_string(i);
{SmoothingEval1 eval; eval.bergwerk_path4_galaxy_multi(name); eval.run();}
}
//eval.run();
//sleep(100);
// PaperVisGrid::show();
// PaperVisGrid::showStairs();
// PaperVisImportance::createImportance();
// PaperVisImportance::createPath();
// PaperVisImportance::showDijkstraDistance();
return 0;
}

26
code/particles/smoothing/MySmoothingTransitionExperimental.h
View File

@@ -67,7 +67,7 @@ public:
std::vector<std::vector<double>> predictionProbabilities;
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(4);
omp_set_num_threads(7);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_old.size(); ++i) {
std::vector<double> innerVector;
@@ -79,38 +79,53 @@ public:
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
double muDistance = 1.0;
double muDistance = 0.8;
double sigmaDistance = 0.5;
double muFloor = 0.0;
double sigmaFloor = 0.1;
switch (p2->state.currentActivity) {
case Activity::ELEVATOR:
muDistance = 0.0;
sigmaDistance = 0.3;
muFloor = 0.6;
sigmaFloor = 0.25;
break;
case Activity::STAIRS_DOWN:
muDistance = 0.5;
sigmaDistance = 0.3;
muFloor = 0.3;
break;
case Activity::STAIRS_UP:
muDistance = 0.4;
sigmaDistance = 0.2;
muFloor = -0.3;
break;
case Activity::STANDING:
muDistance = 0.0;
sigmaDistance = 0.2;
muFloor = 0.0;
break;
case Activity::WALKING:
muDistance = 1.0;
muDistance = 0.8;
sigmaDistance = 0.5;
muFloor = 0.0;
break;
default:
muDistance = 1.0;
muDistance = 0.8;
sigmaDistance = 0.5;
muFloor = 0.0;
break;
}
@@ -125,7 +140,8 @@ public:
//check how near we are to the measurement
const double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
double diffZ = (p2->state.pCur.z - p1->state.pCur.z) / 100.0;
const double floorProb = K::NormalDistribution::getProbability(muFloor, sigmaFloor, diffZ);
//combine the probabilities

BIN
tex/bare_conf.dvi
View File

Binary file not shown.

2
tex/chapters/experiments.tex
View File

@@ -34,7 +34,7 @@ Due to legal terms, we are not allowed to depict their positions and therefore o
Additionally, we used five \docIBeacon{}s for slight enhancements in some areas.
The empirically chosen values for \docWIFI{} were $P_{0_{\text{wifi}}} = \SI{-46}{\dBm}, \mPLE_{\text{wifi}} = \SI{2.7}{}, \mWAF_{\text{wifi}} = \SI{8}{\dB}$, and $\mPLE_{\text{ib}} = \SI{1.5}{}$ for the \docIBeacon{}s, respectively.
Due to omitting a time-consuming calibration process for those values we expect the localisation process to perform generally worse compared to standard fingerprinting methods \cite{Ville09}.
However, incorporating prior knowledge will often compensate for those poorly chosen system parameters.
%However, incorporating prior knowledge will often compensate for those poorly chosen system parameters.
\commentByToni{Hier eure noetigen Werte eintragen.}
As uncertainties we used $\sigma_\text{wifi} = \sigma_\text{ib} = 8.0$, both growing with each measurement's age.