From f7e817d5e4f776b113bd76c1b0edd7ffc360c394 Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 26 Apr 2016 10:12:10 +0200 Subject: [PATCH] added activity recognition to smoothing transition --- code/CMakeLists.txt.user | 2 +- code/eval/FixedLagEvalBase.h | 17 ++-- code/eval/SmoothingEval1.h | 39 ++++---- code/main.cpp | 69 ++++++++------ code/particles/MyEvaluation.h | 20 ++-- code/particles/MyObservation.h | 6 ++ code/particles/MyState.h | 4 + .../smoothing/MySmoothingTransition.h | 50 ++-------- .../MySmoothingTransitionExperimental.h | 78 ++++++++++------ .../smoothing/MySmoothingTransitionSimple.h | 87 ++++++++---------- tex/bare_conf.dvi | Bin 38596 -> 45840 bytes tex/chapters/experiments.tex | 13 +++ tex/chapters/smoothing.tex | 2 +- 13 files changed, 204 insertions(+), 183 deletions(-) diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user index 9d8028d..47f1584 100644 --- a/code/CMakeLists.txt.user +++ b/code/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h index f99d7ff..d8d67d1 100644 --- a/code/eval/FixedLagEvalBase.h +++ b/code/eval/FixedLagEvalBase.h @@ -245,12 +245,12 @@ public: break; } - case s_accel: { - float acc[3]; - SensorReaderAccel sre; sre.read(se, acc); - actDet.addAccel(acc); - break; - } + case s_accel: { + float acc[3]; + SensorReaderAccel sre; sre.read(se, acc); + actDet.addAccel(acc); + break; + } // case s_linearAcceleration:{ // baroSensorReader.readVerticalAcceleration(se); @@ -284,9 +284,10 @@ public: // currently detected activity // TODO: feed sensor values! - ctrl.currentActivitiy = actDet.getCurrentActivity(); - + ctrl.currentActivitiy = actDet.getCurrentActivity(); + // this is just for testing purposes + obs.currentActivity = actDet.getCurrentActivity(); // time for a transition? if (se.ts - lastTransitionTS > MiscSettings::timeSteps) { diff --git a/code/eval/SmoothingEval1.h b/code/eval/SmoothingEval1.h index 15e77be..3b4adae 100644 --- a/code/eval/SmoothingEval1.h +++ b/code/eval/SmoothingEval1.h @@ -51,37 +51,44 @@ public: //create the backward smoothing filter - //bf = new K::BackwardSimulation(50); - bf = new K::CondensationBackwardFilter; - //bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); + bf = new K::BackwardSimulation(500); + //bf = new K::CondensationBackwardFilter; + bf->setSampler( std::unique_ptr>(new K::CumulativeSampler())); } - void fixedIntervallSimpleTransPath1() { + void fixedIntervallSimpleTransPath1(){ runName = "fixedIntervallSimpleTransPath1"; - bool smoothing_resample = false; - smoothing_time_delay = 1; BarometerEvaluation::barometerSigma = 0.10; - sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward - srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt"); - srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt"); - gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl); + sr = new SensorReader("./measurements/bergwerk/path3/nexus/vor/1454782562231.csv"); // forward + srt = new SensorReaderTurn("./measurements/bergwerk/path3/nexus/vor/Turns.txt"); + srs = new SensorReaderStep("./measurements/bergwerk/path3/nexus/vor/Steps2.txt"); + gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl); - MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) ); + MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) ); GridWalkPathControl* walk = new GridWalkPathControl(grid, DijkstraMapper(grid), end); pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); + + //Smoothing Variables + smoothing_walk_mu = 0.7; + smoothing_walk_sigma = 0.5; + smoothing_heading_sigma = 5.0; + smoothing_baro_sigma = 0.05; + + bool smoothing_resample = false; + smoothing_time_delay = 1; + //Smoothing using Simple Trans - bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); if(smoothing_resample) bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionSimple()) ); - - } + bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); + } void fixedIntervallSimpleTransPath4(){ @@ -111,7 +118,7 @@ public: bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverageWithAngle())); if(smoothing_resample) bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransitionExperimental) ); + bf->setTransition(std::unique_ptr( new MySmoothingTransitionSimple) ); } // ============================================================ Dijkstra ============================================== // diff --git a/code/main.cpp b/code/main.cpp index 2856c88..1ebf0cd 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -72,7 +72,7 @@ void testModelWalk() { while(true) { for (GridWalkState& state : states) { - state = walk.getDestination(grid, state, std::abs(wDist.draw()), wHead.draw()); + state = walk.getDestination(grid, state, std::abs(wDist.draw()), wHead.draw(), Activity::UNKNOWN); } usleep(1000*80); vis.showStates(states); @@ -88,9 +88,18 @@ 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();} + {SmoothingEval1 eval; + eval.fixedIntervallSimpleTransPath4(); + eval.run();} //Eval1 eval; //eval.bergwerk_path4_nexus_multi(); @@ -98,41 +107,41 @@ int main(void) { //{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_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_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_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.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.bergwerk_path1_galaxy_simple(); eval.run();} - {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_multi(); eval.run();} - {SmoothingEval1 eval; eval.bergwerk_path1_galaxy_shortest(); eval.run();} +//// {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();} - {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();} +//// {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();} - {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();} +//// {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();} - {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();} +//// {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();} diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h index a327fbc..8c7109c 100755 --- a/code/particles/MyEvaluation.h +++ b/code/particles/MyEvaluation.h @@ -72,17 +72,19 @@ public: } // CONTROL! -// if (useStep) { -// weight *= stepEval.getProbability(p.state, observation.step); -// } +// if (useStep) { +// //weight *= stepEval.getProbability(p.state, observation.step); +// } - // CONTROL! -// if (useTurn) { -// weight *= turnEval.getProbability(p.state, observation.turn, true); + // CONTROL! + if (useTurn) { + //weight *= turnEval.getProbability(p.state, observation.turn, true); -// //set -// p.state.angularHeadingChange = observation.turn->delta_heading; -// } + //set + p.state.angularHeadingChange = observation.turn->delta_heading; + } + + p.state.currentActivity = observation.currentActivity; // set and accumulate p.weight = weight; diff --git a/code/particles/MyObservation.h b/code/particles/MyObservation.h index e9cca28..bc1f9d1 100755 --- a/code/particles/MyObservation.h +++ b/code/particles/MyObservation.h @@ -9,6 +9,8 @@ #include "../lukas/StepObservation.h" #include "../lukas/TurnObservation.h" +#include "Indoor/grid/walk/GridWalk.h" + /** * all available sensor readings */ @@ -31,6 +33,10 @@ struct MyObservation { /** turn observation data (if any) */ TurnObservation* turn = nullptr; + /** get the activity into the observation. just for testing in smoothing */ + Activity currentActivity = Activity::UNKNOWN; + + /** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */ uint64_t latestSensorDataTS = 0; diff --git a/code/particles/MyState.h b/code/particles/MyState.h index 2ea177d..832c4cf 100755 --- a/code/particles/MyState.h +++ b/code/particles/MyState.h @@ -5,6 +5,7 @@ #include #include +#include #include "../MyGridNode.h" @@ -34,6 +35,9 @@ struct MyState { double avgAngle; + //the current Activity + Activity currentActivity; + //int distanceWalkedCM; diff --git a/code/particles/smoothing/MySmoothingTransition.h b/code/particles/smoothing/MySmoothingTransition.h index 5738ffa..465c8cd 100644 --- a/code/particles/smoothing/MySmoothingTransition.h +++ b/code/particles/smoothing/MySmoothingTransition.h @@ -96,22 +96,8 @@ public: auto p2 = &particles_new[j]; - // find the node (square) the particle is within - // just to be safe, we round z to the nearest floor - - //TODO:: Nullptr check! sometimes src/dst can be nullptr - //const Node3* dst = graph->getNearestNode(p1->state.x_cm, p1->state.y_cm, std::round(p1->state.z_nr)); - //const Node3* src = graph->getNearestNode(p2->state.x_cm, p2->state.y_cm, std::round(p2->state.z_nr)); - const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z)); - -// Dijkstra dijkstra; -// dijkstra.build(src, dst, DijkstraMapper(*grid)); - -// double distDijkstra_m = dijkstra.getNode(*src)->cumWeight; - double distDijkstra_m = 0; - //std::vector shortestPath; // check if this shortestPath was already calculated std::map::iterator it; @@ -121,16 +107,8 @@ public: } else{ - //Dijkstra/A* for shortest path - //shortestPath = aStar.get(src, dst, dm); - distDijkstra_m = aStar.get(src, dst, dm); - //get distance walked and getProb using the walking model -// for(int i = 0; i < shortestPath.size() - 1; ++i){ -// distDijkstra_m += dm.getWeightBetween(*shortestPath[i], *shortestPath[i+1]); -// } - if(distDijkstra_m != distDijkstra_m) {throw "detected NaN";} //save distance and nodes in lookup map @@ -140,22 +118,12 @@ public: const double distProb = distWalk.getProbability(distDijkstra_m * 0.01); - //getProb using the angle(heading) between src and dst -// double angle = 0.0; -// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ -// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); -// } -// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); + //heading change prob + double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD()); + double diffDeg = Angle::radToDeg(diffRad); + double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0); - // is the heading change similiar to the measurement? - double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359; - double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359; - - double diffDeg = p2AngleDeg - p1AngleDeg; - const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg); - - //assert(headingProb != 0.0); - //assert(distProb != 0.0); + const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg); //check how near we are to the measurement double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); @@ -167,10 +135,10 @@ public: //if(distance_m != distance_m) {throw "detected NaN";} //if(distProb != distProb) {throw "detected NaN";} //if(angle != angle) {throw "detected NaN";} - if(headingProb != headingProb) {throw "detected NaN";} - if(floorProb != floorProb) {throw "detected NaN";} - if(floorProb == 0) {throw "detected NaN";} - if(prob != prob) {throw "detected NaN";} + //if(headingProb != headingProb) {throw "detected NaN";} + //if(floorProb != floorProb) {throw "detected NaN";} + //if(floorProb == 0) {throw "detected NaN";} + //if(prob != prob) {throw "detected NaN";} //assert(prob != 0.0); diff --git a/code/particles/smoothing/MySmoothingTransitionExperimental.h b/code/particles/smoothing/MySmoothingTransitionExperimental.h index 5463765..b4f39bd 100644 --- a/code/particles/smoothing/MySmoothingTransitionExperimental.h +++ b/code/particles/smoothing/MySmoothingTransitionExperimental.h @@ -52,8 +52,9 @@ public: /** * smoothing transition starting at T with t, t-1,...0 - * @param particles_new p_t (Forward Filter) - * @param particles_old p_t+1 (Smoothed Particles from Step before) + * @param particles_new p_t (Forward Filter) p2 + * @param particles_old p_t+1 (Smoothed Particles from Step before) p1 + * q(p1 | p2) is calculated */ std::vector> transition(std::vector>const& particles_new, std::vector>const& particles_old ) override { @@ -76,59 +77,76 @@ public: auto p2 = &particles_new[j]; - //!!!distance kann hier zu groß werden!!! const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0; + double muDistance = 1.0; + double sigmaDistance = 0.5; - //get distance walked and getProb using the walking model - //double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1)); - const double distProb = distWalk.getProbability(distance_m); + switch (p2->state.currentActivity) { + case Activity::ELEVATOR: + muDistance = 0.0; + sigmaDistance = 0.3; + break; + case Activity::STAIRS_DOWN: + muDistance = 0.5; + sigmaDistance = 0.3; + break; - //getProb using the angle(heading) between src and dst -// double angle = 0.0; -// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ -// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); -// } + case Activity::STAIRS_UP: + muDistance = 0.4; + sigmaDistance = 0.2; + break; -// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); + case Activity::STANDING: + muDistance = 0.0; + sigmaDistance = 0.2; + break; + case Activity::WALKING: + muDistance = 1.0; + sigmaDistance = 0.5; + break; + + default: + muDistance = 1.0; + sigmaDistance = 0.5; + break; + } + + const double distProb = K::NormalDistribution::getProbability(muDistance, sigmaDistance, distance_m); // is the heading change similiar to the measurement? - double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359; - double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359; - - double diffDeg = p2AngleDeg - p1AngleDeg; - const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg); - - //assert(headingProb != 0.0); - //assert(distProb != 0.0); + double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD()); + double diffDeg = Angle::radToDeg(diffRad); + double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0); + const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg); //check how near we are to the measurement - double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); + const double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); //combine the probabilities double prob = distProb * headingProb * floorProb; innerVector.push_back(prob); - if(distance_m != distance_m) {throw "detected NaN";} - if(distProb != distProb) {throw "detected NaN";} -// if(angle != angle) {throw "detected NaN";} - if(headingProb != headingProb) {throw "detected NaN";} - if(floorProb != floorProb) {throw "detected NaN";} - if(floorProb == 0) {throw "detected NaN";} - if(prob != prob) {throw "detected NaN";} - - //assert(prob != 0.0); + //error checks +// if(distance_m != distance_m) {throw "detected NaN";} +// if(distProb != distProb) {throw "detected NaN";} +// if(headingProb != headingProb) {throw "detected NaN";} +// if(floorProb != floorProb) {throw "detected NaN";} +// if(floorProb == 0) {throw "detected zero prob in floorprob";} +// if(prob != prob) {throw "detected NaN";} + //if(prob == 0) {++zeroCounter;} } #pragma omp critical predictionProbabilities.push_back(innerVector); } + return predictionProbabilities; } diff --git a/code/particles/smoothing/MySmoothingTransitionSimple.h b/code/particles/smoothing/MySmoothingTransitionSimple.h index 0f53077..443bbe5 100644 --- a/code/particles/smoothing/MySmoothingTransitionSimple.h +++ b/code/particles/smoothing/MySmoothingTransitionSimple.h @@ -17,37 +17,38 @@ class MySmoothingTransitionSimple : public K::BackwardFilterTransition private: - /** a simple normal distribution */ - K::NormalDistribution distWalk; + /** a simple normal distribution */ + K::NormalDistribution distWalk; public: - /** - * ctor - * @param choice the choice to use for randomly drawing nodes - * @param fp the underlying floorplan - */ + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ MySmoothingTransitionSimple() : - distWalk(smoothing_walk_mu, smoothing_walk_sigma) { + distWalk(smoothing_walk_mu, smoothing_walk_sigma) + { distWalk.setSeed(4321); - } + } public: - uint64_t ts = 0; - uint64_t deltaMS = 0; + uint64_t ts = 0; + uint64_t deltaMS = 0; - /** set the current time in millisconds */ - void setCurrentTime(const uint64_t ts) { - if (this->ts == 0) { - this->ts = ts; - deltaMS = 0; - } else { + /** set the current time in millisconds */ + void setCurrentTime(const uint64_t ts) { + if (this->ts == 0) { + this->ts = ts; + deltaMS = 0; + } else { deltaMS = this->ts - ts; - this->ts = ts; - } - } + this->ts = ts; + } + } /** * smoothing transition starting at T with t, t-1,...0 @@ -64,52 +65,44 @@ public: // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1)) std::vector> predictionProbabilities; - auto p1 = particles_old.begin(); //smoothed / backward filter p_t+1 - auto p2 = particles_new.begin(); //forward filter p_t - - #pragma omp parallel for private(p2) shared(predictionProbabilities) - for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) { + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(6); + #pragma omp parallel for shared(predictionProbabilities) + for (int i = 0; i < particles_old.size(); ++i) { std::vector innerVector; - for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){ + auto p1 = &particles_old[i]; - //!!!distance kann hier zu groß werden!!! + for(int j = 0; j < particles_new.size(); ++j){ + + auto p2 = &particles_new[j]; + + //distance can be pretty big here const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0; //get distance walked and getProb using the walking model - //double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1)); const double distProb = distWalk.getProbability(distance_m); - - //getProb using the angle(heading) between src and dst - double angle = 0.0; - if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ - angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); - } - - const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); - - //assert(headingProb != 0.0); - //assert(distProb != 0.0); - + //get proba for heading change + double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD()); + double diffDeg = Angle::radToDeg(diffRad); + double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0); + const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg); //check how near we are to the measurement double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); - //combine the probabilities double prob = distProb * headingProb * floorProb; innerVector.push_back(prob); + //error checks if(distance_m != distance_m) {throw "detected NaN";} if(distProb != distProb) {throw "detected NaN";} - if(angle != angle) {throw "detected NaN";} if(headingProb != headingProb) {throw "detected NaN";} if(floorProb != floorProb) {throw "detected NaN";} - if(floorProb == 0) {throw "detected NaN";} + if(floorProb == 0) {throw "detected zero prob in floorprob";} if(prob != prob) {throw "detected NaN";} - - //assert(prob != 0.0); - + //if(prob == 0) {throw "detected zero prob in smoothing transition";} } #pragma omp critical @@ -122,4 +115,4 @@ public: }; -#endif // MYTRANSITION_H +#endif // MYTRANSITIONSIMPLE_H diff --git a/tex/bare_conf.dvi b/tex/bare_conf.dvi index 0c34719071b7ec6232802ca40020e9440e59fbf3..b09aec58a364b5f6e967b42077bb6dc8aab012f8 100644 GIT binary patch delta 8366 zcmb7J4RjP$mhMz{keEOM0b~$yP#lvX#3YD;Ajtm+qJl&L5wN3puz%J=!^He27EyjxTfZI7KNTHT{BGi6OpX9s`suWa<{i-*h-^|t$7 z)nbw<>GbqzrmemP2Rym@8^1sYhw}4;a~|*bxA0t{AoESi8sPK2G>er+E)qU@x}!D{ z7leI3X1nFbgzl|u`I>!#zN4e4F_D)y-ilq>@uafOAIKJsezxPvcxm2@-KG1trL0-` z$B*AMpDS0di>e{J8WkTcOj(~E+tpPcl`@00p<&rDbvf8}=x~A-*n_lEHlt=_a?JWI z2iBxg*6@lgC$DQtr&HFbzgB03rf2t?zxM5`PFrOMzG^RLU9#aAVl1vm;q-$k>%_5L z&9Z10-ZleAhP7Cx@V1#PZSO@yrO{xT>WD0&i}9$_`6GMC{SUTJV`ZH;S-$j?saD>e zf1YdkSgO6es{)dKK%!crawuvSy?aPDSVU4KT{NYzRrK!54Z0R%W>jMA?WPybopOE`a2+Q#)7rxi*YS!q|??gpWa^9-`e(arwivok`vN&oD$d6FdWUA zw{ZtaY%PK|b)-Hj=bS9Y{+(6*%b@s7p-Ma9C7>(^x{4CwQ0Bpy@HC)#_fpv%=2bQRwDb zCVxj!Sc*%o-I~fo1_Og)P);hc+3Ix0;k3SiRj}s2kr!~wj&1e0KSeYNN8EgK1sS%` z^tz^Igr+wP2~KgNO;pH!BsjvxG#sQP(w2WlFcj;x=6C?WS7jFPI(6N=lh1LkTY zChBH9s;Lr!$o$!j2Hlgp6&WK#mWfMfks^RupRL=`R2>~QUadDMaG7$KLlEMxIUX61X zdW(U*5h)W0yaE`LRU}nh<4Ea*6FOZORPe;PplkZ)KIlo>EWWCpygNGvMh92gQJ;3nK|AjIEx=x{UBxCWVk`AtkidN(sSMG;$n zE90qC?&Ol&)70^Dh`T0ti`>&m)ebx~h=PrYnZQFuuu;|+prpO3v#xy#vT<-7qp#H# z)^&O3=??DIkSb^|QU&o$2T`o|23p2H@K~R#u>Av%uXypQu9cORe_%HyTtW>?dQxe{ zR>z2Hx`}(6Kwcp~1VxG^DsG-cBQAwxQL%*+b8=9f5l+lSgz4}R&qyQ(zZ!3;BkG~3 zY$BZ!I&zl#FJu~ZxnP_{!I<>T)Y~5XQ3!s_t!1J@(KyfQp5YU*A)PJWdcIfQDHb)` zf9mJR^aSOg1fZl_Vq9^GyciE)z+a0*fukhC5~`dFm!jfX)1z9FbhCw{!}^9@r6nAl zITzU62fp1PrHLdP#2^XqcQ{cER-7=iD{7$2M#jG$?4_r?Z^*@l!7J>)dQytWic1N zPI(Rz*gJ_!OLFdAf?{jf{`c6h<|~r9xaa9WV1wEqY=04D2vHRR$VuUhu>A#KIMOKCY!ljo&y-c9AOc4Q}Kyr`CIlW#=GVd}a#5y5d z|1#ydBxkejU;I#KKgv)%GTFrei(DwOqP4_S!d$BfhP+O){EPpIr=0-;3zO(ZWF9(o z>0PB){V@uf`2Duu_7^%h^?Ca=PGYoF_H^r*)P5CU;`2ShjZLSs1FN^BJT95GhW>Ez zfjnE7e5}*xOJ)Xj|E7IBJ8+$q*E+4etctzTFfzHHbob<>K`{o^{-(X0u)4D@ol4#B zd#iSTSJ{+uymG;)9>hDLs}tsc;DMKc;`%>&#Yn{CiYyr!|3|Nq&thUkl}*GyT?ZZj zp=mX|5Ji2Nkj@tQd>q>3*=ocN{B&ywm>iLaDx$JVa2R+kM;4M6Y1K6&q{SsS6F>|2 zVt%8%`m9`5lcf8!1Gk2HuM{=*)*bacZzHHilFlnD`}+p& zZ;`f(Pw#0TpP_g_^pJ#oc|8K$C9HkCb4Fm08VHP*w$CP#8aQb1x(C|Jc~tymX-QXE zb(I$uFli32OvDPvD{f)T(f$gMJ5YQEs3D4B;AV}6R_^j1^B;umgO|MDTQ+k|0OxA1 zLufI)ar-LI8)wkEOi@kAfi>(~?i*aN51rMtp1A*P%0F~`{BkeFj)9U^SN-QLeJP@I zV8q!aA!ojqWs<=+6?sbYwdW8=@fKM{Q2k#SR^l1*6)Kr zxc&XGCK(Pf-#%FY6w+9GH41T+(-kJlmDGB}GbHPwgd*zTxwi1<81;aroHIFFDyu4$-&-L8na3 z$}jnlj78oiKwnuE@a{7@LRQ);jNj!}B;tsZDQ3FJA)Gzdur3X%=;VdU zbhhxs&g|Hx4y5?7hQ_}qxmR#1mHn^eC&-THMm{<@oh|SU;NPdGclmuu!TXMg#rrGM z*`dCoevhV;6?j#LK}#z8@#*6@x^U0Wxo(s3C}myL^lX=Vr@)rw+wbxDHu!v~dTAx%2wd@t_HvF*`_}=6gS$JYV*}3hHVmno z@$Gg!w?3WTHKZZbN3oj!t76IT)a*MIJ2343qZq)L`wJWb*5|Q2fIs~qJ^+v_0`%7q z%4IM4 zjmB&Au^2Q}o>vyg4wdetx!?t#Mn1=#G&J}=?sl>xsz#(X`S342NoD`a#BsF)20Dd! z#%5elkv#n$kERaJL(yrhvjf4|&qQTO&jf|7yH4b2Pt@69v&2@^#Skc-As6L% zoI4718YB^2o`l;JcD^tV>tT12wB}s|P!~Gb{>?S+m<2s{1eFHZjTX)TXQfih42--8 zK>eF1w!mnP(ncktKFq>a{>*9dOpb~$cyyzf@sD1kNNNOKE2=JLI+*0pbPQvmB>Gpk zJ=YbH`kK*=ld7s%%y76@WYa(=hM|oovFA*hYr&2k|3XnC>_`^rNG8ufTmml#s293S z<4QV}{40h+k|LpRqlv4_kdvO^963c{9Gmr~7h)!V>`s0nwQ#-M*VVoMVV;<C#vU_CZvgo8`?*nra(gbDmjns~M! z+%(JU|H(K*%9)P{#DP5_;7O%`h^RaOMU4GG5l~>8<#8t1rN(Gpo4Z^0Ajdr>oh=#J zLNIgTr?)=e2WFJ-3bG1lp%|dyBm#LJh6lJMXI+aVdl&*xkulFw zB^PgIHk$IbLob6Z0cJ9QoplK$cMEzeiDX^Y2x!$}G4m>Ci5s*CDR_{z7$6iCS5!|~$?!eQD?OJo*PSGT*Z`^O(%C`# z_fbtLVM8_!XCJK}y-I-_Y=3A8l?yo*LsA2BEq~~Sh(Z+sjTKV!+Js`ti;&YmnMD}z z$-LpD8bbF!YUGLe7*Aq|kI>|E8(JAMF@a`B*4M^nEwKU%*3L&isk_?Pfd#*S*i2x- zFDV_V6xGrfegXzrjhM4Z3Iu?f6R$|( zM4@|r-xG!IHFToTEz?2k63(RFbDebwhNsRM$743z;2nu?q9C6Exg!D-$Ix+siPuic1ST%zC&KzF*J>x{{O@oAIRHm_4#+7&b5N-A zcb!1og&k?FrpxdbkL4y8PlK!H>N-lpB1JP%@QQE7i;TZ`dNW36sHIgb2z;%ss=_E- zMj}z&(W8pV3EKNYywuKSS~)%3+b3bs0m5oBS!KXMk$0vd`M3TRu&^9 zgeB!%r#bSPM*0!MQ{o&<>9d13%x15&UfvODeM1;PRr$;NSZsYAeT77}cm1X8wbm)a z6fr_c5NZ3YR*aKhXx+HmW?qd&hODX>=yQ)M%2ny?h;20~Yev=BHm8S|qnxg67k85#IblNp3xs0!FOD^S~A0)5K-(4`{qMluB*@hDSrg6iK z{PUR&3;E~JJ3{<(?H%s(!*@&-9_igSexti>`o>j!S$3m)z?T~*pP|;Iv#1rmvx)C| z^Ujq+pk_RdN`L5Z^^YghK$M%kded;eqGgk-#ygwF_Ls}XY<4xM-h2~Zd9U~Rr_E9! zWa2m5>ORu)ODCU2k4<+icJ;VxZeQc3-R+8AboVv<^*`Qy1ONQ&?(#lL)NUEdpDx^T zy}N453jX=^mhm}Zv+i-1EWO9oaqm5@m(IPnhOJsQfnD^I^V#xy!+~9(*BwvZ`@4cs z)<=1F-v89-srbFq=RYAl-n1d_!(+RArM`Uj*iV(A-yYYn5!+Ukp5=$qj;3u3^9uGp JF8qQ2`5#ywNoN26 delta 1984 zcmY*aeM}Q)7;mpdU_()fQxRmkIfm56qEiIT6vsB^)F|jg#>mloX-}_r$K4fFjJPfP zG_5_KQ42*1Qd|`LU=3SLmPw2YGlFAFwuKERCX3O`#AMk&+Y;S#@71{R&%^sZU%%(~ z{N8KmHgn}H6M81YwWh>XT(tU##bU97tNhu)$|I$To24J+GFRV&J=NRn@mXUx5|^sy zTCF|s^X}K|@eGS4v83h+hS{G7uG&S`Hu&~~+=N`)Z%zFvEuc@cSRgbwclEUmPCdn9 zDQ{v0KdbSQ7>v)je>b4BA6MSmsXrJRq3*KKg-=-}bZ)NXBN|bHJh}z$i4%c<{`8lx z!q}THCF=GTTCGcU-`d28aKL6iow2YZQ7&g$TVq*QvIF{^sra=rilj<^s1_@siEdU@ zd5Sldska97Kc?artGpzf(O;-3k9^UL{-vr$WZ;NQL{0@sjG&W~e9#=a?O}zzYOse^ zm&9=T*l{+RJ{I;Utn77whloUBr)z|R1_cu|FNweo<*MDyYXsyu4yaO>w^n(1HJmp1 zy-GB{46GnX9mAGpqmi&*PaFKMDZ&ao2R^B7qPq!*kBFM;FcMf5SYJJl=OhBRByw~h zHEcURJko<}f@ju4!qJ@=ZMPl7vER?K@wOS3X++Il~;(sS`24fiu+bJ`smldm} zFf)0bh^imi!z_5L5HRGSGm~Rj97WZHyy9Y@iD&Bt+_Hpjg~uT|US0O6t-{xbM#uez z_?9`sHfaRe=0&&Q=Lo3yKmgIGxHH`*eZwKiZcW&a0XSoJb9O88H!!q;<0v5mUW9|% zlA`tj-atm$hAmB~Bl9hW?E^S3VqiH3RjlL}xQKv9Oi98~S)VMBDDuGj9DW9zUA= zA|t4*rRUbp3i_lgeUgNq7LBepmU-i$iDY9^=3(37w;@tgPzZie;|1J~#v3Ff&?G+G zaoJI)lWx6(Cc{v0o6W$Hesn9fHAnN-nV*uin*ke}oy+&hxL*1wgR#tCEoNhmqvO`N z*3<(2&jn*M)_1_eV=EK;+J0c5ZkN;MoOcH*BPHp^S2mz$;|U`9;0d|l_sD$eO>N&$ zf%izg7;29-p`>8GI@KHO}<#_T12gtG$TrIBU;5wfR*61ASn;+dptrmTQ z+UwCRu;TmzDCso)+dFsA`!k)dQ~Ot^o7%b)=KU8ZN@@J`E;D{f*IU&8ewSHqs;d}& zJG%f@#Z3Q}SR?gcj@3>_tcXcxM#o((#S~RVw;9EDn_Wh_7t!j|dd&NUJv(W3bJC9Y zkW{QvwA|&Q`PC=QxW<#)Xx!H)%W3|SUh{rk?$#>@)qZ^qKv;`^@S7?kl2x?G$J2Nlc#lF+E$@VSV@5a84=y4qI$@nDdQ? ft@ozlmMn`MMlT4kWpH=mtHBCu>L)sL6aW4NQruyo diff --git a/tex/chapters/experiments.tex b/tex/chapters/experiments.tex index 01ca55d..f4869b9 100644 --- a/tex/chapters/experiments.tex +++ b/tex/chapters/experiments.tex @@ -2,6 +2,17 @@ ddd \cite{Ville09} dddd +Evaluation: +\begin{itemize} + \item Filter ist immer der gleiche mit MultiPathPrediction und Importance Factors + \item FBS Interval mit 500 und 7500 Partikeln auf 4 Pfaden mit SimpleSmoothingTrans + \item BS Interval mit 500 zu 50 und 7500 zu 500 Partikeln auf auf 4 Pfaden mit SimpleSmoothingTrans + \item FBS Lag = 5 mit 500 und 7500 Partikeln auf 4 Pfaden mit SimpleSmoothingTrans + \item BS Lag = 5 mit 500 zu 50 und 7500 zu 500 Partikeln auf auf 4 Pfaden mit SimpleSmoothingTrans + \item BS Lag zu Error Plot. Lag von 0 bis 100, wie verhält sich der Error. Am besten auf Pfad 4 mit SimpleSmoothingTrans. + \item BS Lag = 5 mit 500 Partikeln auf einem Pfad der manuell angepasst ist (mach ich) mit DijkstraTrans. +\end{itemize} + \begin{itemize} \item Vorwärtsschritt die Ergebnisse und Probleme beschreiben. Zeitlicher Verzug etc. @@ -15,3 +26,5 @@ ddd \cite{Ville09} dddd \item Fixed-lag gap \subitem einen offset (gap) im smoothing. was bringt es? sinnvoll? \end{itemize} + + diff --git a/tex/chapters/smoothing.tex b/tex/chapters/smoothing.tex index c97668d..d64df5b 100644 --- a/tex/chapters/smoothing.tex +++ b/tex/chapters/smoothing.tex @@ -1,7 +1,7 @@ \section{Smoothing} \label{sec:smoothing} -Consider a situation given all observations until a time step T... +