Merge branch 'master' of https://git.frank-ebner.de/FHWS/Fusion2016
This commit is contained in:
toni
@@ -65,7 +65,6 @@ ADD_DEFINITIONS(
|
|||||||
|
|
||||||
-g
|
-g
|
||||||
-O0
|
-O0
|
||||||
|
|
||||||
-DWITH_TESTS
|
-DWITH_TESTS
|
||||||
-DWITH_ASSERTIONS
|
-DWITH_ASSERTIONS
|
||||||
|
|
||||||
|
|||||||
29
code/DijkstraMapper.h
Normal file
29
code/DijkstraMapper.h
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#ifndef DIJKSTRAMAPPER_H
|
||||||
|
#define DIJKSTRAMAPPER_H
|
||||||
|
|
||||||
|
#include "MyGridNode.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* allows dijkstra calculation on top of our data-structure
|
||||||
|
*/
|
||||||
|
class DijkstraMapper {
|
||||||
|
|
||||||
|
Grid<MyGridNode>& grid;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
DijkstraMapper(Grid<MyGridNode>& grid) : grid(grid) {;}
|
||||||
|
|
||||||
|
int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
|
||||||
|
|
||||||
|
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
|
||||||
|
|
||||||
|
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
|
||||||
|
float d = ((Point3)n1 - (Point3)n2).length(2.0);
|
||||||
|
//if (d > 20) {d*= 1.30;}
|
||||||
|
return d / std::pow(n2.imp, 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // DIJKSTRAMAPPER_H
|
||||||
147
code/Helper.h
Normal file
147
code/Helper.h
Normal file
@@ -0,0 +1,147 @@
|
|||||||
|
#ifndef HELPER_H
|
||||||
|
#define HELPER_H
|
||||||
|
|
||||||
|
#include <Indoor/grid/Grid.h>
|
||||||
|
#include <Indoor/grid/factory/GridFactory.h>
|
||||||
|
#include <Indoor/grid/factory/GridImportance.h>
|
||||||
|
|
||||||
|
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
||||||
|
|
||||||
|
#include "Settings.h"
|
||||||
|
#include "MyGridNode.h"
|
||||||
|
|
||||||
|
class Helper {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** convert height (in cm) to floor-numbers */
|
||||||
|
static int getFloorNr(float z_cm) {
|
||||||
|
// if (z_cm < 360) {return 0;}
|
||||||
|
// if (z_cm < 360+340) {return 1;}
|
||||||
|
// if (z_cm < 360+340+340) {return 2;}
|
||||||
|
// return 3;
|
||||||
|
if (z_cm < 180) {return 0;}
|
||||||
|
if (z_cm < 360+180) {return 1;}
|
||||||
|
if (z_cm < 360+340+180) {return 2;}
|
||||||
|
return 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** convert height (in cm) to floor-numbers */
|
||||||
|
static int getFloorNrFloat(float z_cm) {
|
||||||
|
return z_cm / 340.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int getHeight(const int floorNr) {
|
||||||
|
switch(floorNr) {
|
||||||
|
case 0: return 0;
|
||||||
|
case 1: return 360;
|
||||||
|
case 2: return 360+340;
|
||||||
|
case 3: return 360+340+340;
|
||||||
|
default: throw "error";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/** align the given value onto the grid */
|
||||||
|
static int align(const int val) {
|
||||||
|
return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** all floors within the building */
|
||||||
|
struct FHWSFloors {
|
||||||
|
|
||||||
|
Floor f0, f1, f2, f3;
|
||||||
|
|
||||||
|
Stairs s01, s12, s23;
|
||||||
|
|
||||||
|
const LengthF h0 = LengthF::cm(align(getHeight(0)));
|
||||||
|
const LengthF h1 = LengthF::cm(align(getHeight(1)));
|
||||||
|
const LengthF h2 = LengthF::cm(align(getHeight(2)));
|
||||||
|
const LengthF h3 = LengthF::cm(align(getHeight(3)));
|
||||||
|
|
||||||
|
FHWSFloors() {;}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/** load the entire floorplan */
|
||||||
|
static FHWSFloors getFloors() {
|
||||||
|
|
||||||
|
FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
|
||||||
|
FHWSFloors f;
|
||||||
|
|
||||||
|
f.f0 = fpFac.getFloor("floor_0");
|
||||||
|
f.f1 = fpFac.getFloor("floor_1");
|
||||||
|
f.f2 = fpFac.getFloor("floor_2");
|
||||||
|
f.f3 = fpFac.getFloor("floor_3");
|
||||||
|
|
||||||
|
f.s01 = fpFac.getStairs("staircase_0_1");
|
||||||
|
f.s12 = fpFac.getStairs("staircase_1_2");
|
||||||
|
f.s23 = fpFac.getStairs("staircase_2_3");
|
||||||
|
|
||||||
|
return f;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {
|
||||||
|
|
||||||
|
GridFactory<MyGridNode> gridFac(grid);
|
||||||
|
|
||||||
|
gridFac.addFloor(floors.f0, floors.h0.cm());
|
||||||
|
gridFac.addFloor(floors.f1, floors.h1.cm());
|
||||||
|
gridFac.addFloor(floors.f2, floors.h2.cm());
|
||||||
|
gridFac.addFloor(floors.f3, floors.h3.cm());
|
||||||
|
|
||||||
|
gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
|
||||||
|
gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
|
||||||
|
gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
|
||||||
|
|
||||||
|
// maybe the two sides are wrong?
|
||||||
|
PlatformStair psUpperLeft;
|
||||||
|
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
|
||||||
|
psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
|
||||||
|
psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
|
||||||
|
gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
|
||||||
|
gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
|
||||||
|
gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
|
||||||
|
// vis.gp << "set xrange [1100:1800]\n";
|
||||||
|
// vis.gp << "set yrange [4500:5200]\n";
|
||||||
|
|
||||||
|
PlatformStair psUpperRight;
|
||||||
|
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
|
||||||
|
psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
|
||||||
|
psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
|
||||||
|
gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
|
||||||
|
gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
|
||||||
|
gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
|
||||||
|
// vis.gp << "set xrange [6100:6900]\n";
|
||||||
|
// vis.gp << "set yrange [4500:5200]\n";
|
||||||
|
|
||||||
|
PlatformStair psLowerLeft;
|
||||||
|
psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
|
||||||
|
psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
|
||||||
|
psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
|
||||||
|
gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
|
||||||
|
gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
|
||||||
|
gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
|
||||||
|
// vis.gp << "set xrange [1300:2100]\n";
|
||||||
|
// vis.gp << "set yrange [400:1400]\n";
|
||||||
|
|
||||||
|
// remove all isolated nodes not attached to 300,300,floor0
|
||||||
|
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );
|
||||||
|
|
||||||
|
// stamp importance information onto the grid-nodes
|
||||||
|
GridImportance gridImp;
|
||||||
|
gridImp.addImportance(grid, floors.h0.cm());
|
||||||
|
gridImp.addImportance(grid, floors.h1.cm());
|
||||||
|
gridImp.addImportance(grid, floors.h2.cm());
|
||||||
|
gridImp.addImportance(grid, floors.h3.cm());
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // HELPER_H
|
||||||
26
code/MyGridNode.h
Normal file
26
code/MyGridNode.h
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
#ifndef MYGRIDNODE_H
|
||||||
|
#define MYGRIDNODE_H
|
||||||
|
|
||||||
|
#include <Indoor/grid/GridNode.h>
|
||||||
|
#include <Indoor/grid/GridPoint.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* the nodes we add to our grid
|
||||||
|
*/
|
||||||
|
struct MyGridNode : public GridNode, public GridPoint {
|
||||||
|
|
||||||
|
/** distance to the desired target */
|
||||||
|
float distToTarget = 1.0;
|
||||||
|
|
||||||
|
/** node importance based on surroundings */
|
||||||
|
float imp = 1.0;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** needed ctor */
|
||||||
|
MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif // MYGRIDNODE_H
|
||||||
26
code/Settings.h
Normal file
26
code/Settings.h
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
#ifndef OTHER_SETTINGS_H
|
||||||
|
#define OTHER_SETTINGS_H
|
||||||
|
|
||||||
|
#define USE_STATIC_CIRCULAR_BUFFERING false
|
||||||
|
#define USE_BAROMETER_SMOOTHING_RC_LOWPASS false
|
||||||
|
#define USE_BAROMETER_SMOOTHING_HEAD_TAIL false
|
||||||
|
#define USE_BAROMETRIC_FORMULAR false
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
namespace MiscSettings {
|
||||||
|
|
||||||
|
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
|
||||||
|
|
||||||
|
const int gridSize_cm = 40;
|
||||||
|
|
||||||
|
const int timeSteps = 500;
|
||||||
|
|
||||||
|
const int numParticles = 1000;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif // OTHER_SETTINGS_H
|
||||||
|
|
||||||
|
|
||||||
52
code/Vis.h
52
code/Vis.h
@@ -5,6 +5,7 @@
|
|||||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
||||||
|
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
||||||
|
|
||||||
#include <Indoor/geo/Length.h>
|
#include <Indoor/geo/Length.h>
|
||||||
#include <Indoor/floorplan/Floor.h>
|
#include <Indoor/floorplan/Floor.h>
|
||||||
@@ -16,21 +17,29 @@ public:
|
|||||||
K::Gnuplot gp;
|
K::Gnuplot gp;
|
||||||
K::GnuplotSplot splot;
|
K::GnuplotSplot splot;
|
||||||
K::GnuplotSplotElementLines floors;
|
K::GnuplotSplotElementLines floors;
|
||||||
K::GnuplotSplotElementPoints gridNodes;
|
K::GnuplotSplotElementColorPoints gridNodes;
|
||||||
K::GnuplotSplotElementLines gridEdges;
|
K::GnuplotSplotElementLines gridEdges;
|
||||||
|
K::GnuplotSplotElementPoints particles;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
Vis() {
|
Vis() {
|
||||||
|
|
||||||
gp << "set hidden3d front\n";
|
gp << "set hidden3d front\n";
|
||||||
gp << "set view equal xy\n";
|
//gp << "set view equal xy\n";
|
||||||
gp << "set ticslevel 0\n";
|
gp << "set ticslevel 0\n";
|
||||||
|
gp << "set cbrange[0.8:2.0]\n";
|
||||||
|
|
||||||
|
gp << "unset xtics\n";
|
||||||
|
gp << "unset ytics\n";
|
||||||
|
gp << "unset ztics\n";
|
||||||
|
gp << "unset border\n";
|
||||||
|
|
||||||
// attach all layers
|
// attach all layers
|
||||||
splot.add(&floors);
|
splot.add(&floors);
|
||||||
splot.add(&gridNodes);
|
splot.add(&gridNodes);
|
||||||
splot.add(&gridEdges);
|
splot.add(&gridEdges);
|
||||||
|
splot.add(&particles);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -50,13 +59,44 @@ public:
|
|||||||
|
|
||||||
/** add the grid to the plot */
|
/** add the grid to the plot */
|
||||||
template <typename T> Vis& addGrid(Grid<T>& grid) {
|
template <typename T> Vis& addGrid(Grid<T>& grid) {
|
||||||
|
|
||||||
|
float max = 0;
|
||||||
|
for (const T& n1 : grid) {
|
||||||
|
if (n1.distToTarget > max) {max = n1.distToTarget;}
|
||||||
|
}
|
||||||
|
gp << "set cbrange[0.0:1.0]\n";
|
||||||
|
//gp << "set cbrange[0.8:1.3]\n";
|
||||||
|
|
||||||
for (const T& n1 : grid) {
|
for (const T& n1 : grid) {
|
||||||
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
||||||
gridNodes.add(p1);
|
//const float color = n1.imp;
|
||||||
for (const T& n2 : grid.neighbors(n1)) {
|
//const float color = n1.distToTarget/max;
|
||||||
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
const float color = 0;
|
||||||
gridEdges.addSegment(p1, p2);
|
gridNodes.add(p1, color);
|
||||||
|
// for (const T& n2 : grid.neighbors(n1)) {
|
||||||
|
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
||||||
|
// gridEdges.addSegment(p1, p2);
|
||||||
|
// }
|
||||||
}
|
}
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
void removeGrid() {
|
||||||
|
gridNodes.clear();;
|
||||||
|
}
|
||||||
|
|
||||||
|
void clearStates() {
|
||||||
|
particles.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T> void addState(const GridWalkState<T>& n) {
|
||||||
|
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
|
||||||
|
particles.clear();;
|
||||||
|
for (const GridWalkState<T>& n : states) {
|
||||||
|
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
||||||
}
|
}
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|||||||
66
code/eval/Eval.h
Normal file
66
code/eval/Eval.h
Normal file
@@ -0,0 +1,66 @@
|
|||||||
|
#ifndef EVAL_H
|
||||||
|
#define EVAL_H
|
||||||
|
|
||||||
|
#include "EvalBase.h"
|
||||||
|
#include "../DijkstraMapper.h"
|
||||||
|
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkPushForward.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
|
||||||
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
||||||
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
||||||
|
|
||||||
|
class Eval : public EvalBase {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Eval() {
|
||||||
|
|
||||||
|
|
||||||
|
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
|
||||||
|
|
||||||
|
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
|
||||||
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
|
||||||
|
|
||||||
|
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
|
||||||
|
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
|
||||||
|
//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
|
||||||
|
GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
|
||||||
|
|
||||||
|
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
|
||||||
|
|
||||||
|
sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
|
||||||
|
srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
|
||||||
|
srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
|
||||||
|
//gtw = getGroundTruthWay(*sr, gtwp, way2);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void setEval1() {
|
||||||
|
|
||||||
|
|
||||||
|
runName = "TODO";
|
||||||
|
|
||||||
|
// the particle filter's evaluation method
|
||||||
|
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
|
||||||
|
eval.get()->setUsage(true, false, false, false, false);
|
||||||
|
pf->setEvaluation( std::move(eval) );
|
||||||
|
|
||||||
|
// resampling step?
|
||||||
|
pf->setNEffThreshold(1.0);
|
||||||
|
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
|
||||||
|
|
||||||
|
// state estimation step
|
||||||
|
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // EVAL_H
|
||||||
262
code/eval/EvalBase.h
Normal file
262
code/eval/EvalBase.h
Normal file
@@ -0,0 +1,262 @@
|
|||||||
|
#ifndef EVALBASE_H
|
||||||
|
#define EVALBASE_H
|
||||||
|
|
||||||
|
#include "../Settings.h"
|
||||||
|
#include "../Helper.h"
|
||||||
|
#include "../Vis.h"
|
||||||
|
|
||||||
|
#include <KLib/math/filter/particles/ParticleFilter.h>
|
||||||
|
#include <KLib/math/statistics/Statistics.h>
|
||||||
|
|
||||||
|
#include "GroundTruthWay.h"
|
||||||
|
|
||||||
|
#include "../particles/P3.h"
|
||||||
|
#include "../particles/MyState.h"
|
||||||
|
#include "../particles/MyObservation.h"
|
||||||
|
#include "../particles/MyEvaluation.h"
|
||||||
|
#include "../particles/MyTransition.h"
|
||||||
|
#include "../particles/MyInitializer.h"
|
||||||
|
|
||||||
|
#include "../reader/SensorReader.h"
|
||||||
|
#include "../reader/SensorReaderStep.h"
|
||||||
|
#include "../reader/SensorReaderTurn.h"
|
||||||
|
|
||||||
|
#include "../lukas/TurnObservation.h"
|
||||||
|
#include "../lukas/StepObservation.h"
|
||||||
|
|
||||||
|
#include "../toni/BarometerSensorReader.h"
|
||||||
|
|
||||||
|
#include "../frank/WiFiSensorReader.h"
|
||||||
|
#include "../frank/BeaconSensorReader.h"
|
||||||
|
|
||||||
|
class EvalBase {
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
Grid<MyGridNode> grid;
|
||||||
|
Helper::FHWSFloors floors;
|
||||||
|
Vis vis;
|
||||||
|
|
||||||
|
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
|
||||||
|
|
||||||
|
SensorReader* sr;
|
||||||
|
SensorReaderTurn* srt;
|
||||||
|
SensorReaderStep* srs;
|
||||||
|
|
||||||
|
GroundTruthWay gtw;
|
||||||
|
|
||||||
|
std::string runName;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
|
||||||
|
|
||||||
|
// build the grid
|
||||||
|
Helper::buildTheGrid(grid, floors);
|
||||||
|
|
||||||
|
// setup the visualisation
|
||||||
|
vis.addFloor(floors.f0, floors.h0);
|
||||||
|
vis.addFloor(floors.f1, floors.h1);
|
||||||
|
vis.addFloor(floors.f2, floors.h2);
|
||||||
|
vis.addFloor(floors.f3, floors.h3);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void run() {
|
||||||
|
|
||||||
|
// read CSV input
|
||||||
|
// const int s_wifi = 0;
|
||||||
|
//SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv");
|
||||||
|
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
|
||||||
|
const int s_linearAcceleration = 2;
|
||||||
|
|
||||||
|
std::list<TurnObservation> turn_observations;
|
||||||
|
std::list<StepObservation> step_observations;
|
||||||
|
|
||||||
|
//Create an BarometerSensorReader
|
||||||
|
BarometerSensorReader baroSensorReader;
|
||||||
|
|
||||||
|
|
||||||
|
//Read all turn Observations
|
||||||
|
while(srt->hasNext()) {
|
||||||
|
|
||||||
|
SensorEntryTurn set = srt->getNext();
|
||||||
|
TurnObservation to;
|
||||||
|
|
||||||
|
to.ts = set.ts;
|
||||||
|
to.delta_heading = set.delta_heading;
|
||||||
|
to.delta_motion = set.delta_motion;
|
||||||
|
|
||||||
|
turn_observations.push_back(to);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//Step Observations
|
||||||
|
while(srs->hasNext()) {
|
||||||
|
|
||||||
|
SensorEntryStep ses = srs->getNext();
|
||||||
|
StepObservation so;
|
||||||
|
|
||||||
|
so.ts = ses.ts;
|
||||||
|
|
||||||
|
step_observations.push_back(so);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// the to-be-evaluated observation
|
||||||
|
MyObservation obs;
|
||||||
|
|
||||||
|
|
||||||
|
std::vector<Point3> pathEst;
|
||||||
|
|
||||||
|
uint64_t lastTransitionTS = 0;
|
||||||
|
bool firstReading = true;
|
||||||
|
int64_t start_time = -1;
|
||||||
|
|
||||||
|
K::Statistics<double> stats;
|
||||||
|
|
||||||
|
// process each sensor reading
|
||||||
|
while(sr->hasNext()) {
|
||||||
|
|
||||||
|
// get the next sensor reading from the CSV
|
||||||
|
const SensorEntry se = sr->getNext();
|
||||||
|
|
||||||
|
//start_time needed for time calculation of steps and turns
|
||||||
|
obs.latestSensorDataTS = se.ts;
|
||||||
|
if (start_time == -1) {start_time = se.ts;}
|
||||||
|
int64_t current_time = se.ts - start_time;
|
||||||
|
|
||||||
|
// ensure the graph timestamp starts with the first reading
|
||||||
|
if (firstReading) {
|
||||||
|
//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
|
||||||
|
firstReading = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch(se.idx) {
|
||||||
|
|
||||||
|
case s_wifi: {
|
||||||
|
obs.wifi = WiFiSensorReader::readWifi(se);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case s_beacons: {
|
||||||
|
BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
|
||||||
|
if (!boe.mac.empty()) {
|
||||||
|
obs.beacons.entries.push_back(boe);
|
||||||
|
} // add the observed beacon
|
||||||
|
obs.beacons.removeOld(obs.latestSensorDataTS);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case s_barometer: {
|
||||||
|
obs.barometer = baroSensorReader.readBarometer(se);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case s_linearAcceleration:{
|
||||||
|
baroSensorReader.readVerticalAcceleration(se);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// scheduled transition every 500 ms
|
||||||
|
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
|
||||||
|
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
|
||||||
|
|
||||||
|
//Steps are sorted in the list by timestamp.
|
||||||
|
//If the current observation timestamp is bigger/equal
|
||||||
|
//to the current step timestamp, use this step as observation
|
||||||
|
//and remove it from the list.
|
||||||
|
//The new first timestamp in the list will be then be the next one (timestamp-wise)
|
||||||
|
StepObservation so;
|
||||||
|
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
|
||||||
|
so.step = true;
|
||||||
|
so.ts = current_time;
|
||||||
|
|
||||||
|
obs.step = &so;
|
||||||
|
step_observations.pop_front();
|
||||||
|
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
so.step = false;
|
||||||
|
so.ts = current_time;
|
||||||
|
|
||||||
|
obs.step = &so;
|
||||||
|
}
|
||||||
|
|
||||||
|
TurnObservation to;
|
||||||
|
//same principal as for steps is applied for turns
|
||||||
|
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
|
||||||
|
to = turn_observations.front();
|
||||||
|
obs.turn = &to;
|
||||||
|
|
||||||
|
turn_observations.pop_front();
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
to.delta_heading = 0.0;
|
||||||
|
to.delta_motion = 0.0;
|
||||||
|
|
||||||
|
obs.turn = &to;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// let the transition know the current timestamp to determine the time since the last transition
|
||||||
|
//if (!useSimpleTrans) {
|
||||||
|
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
|
||||||
|
//} else {
|
||||||
|
// ((MyTransitionSimple*)pf->getTransition())->setCurrentTime(lastTransitionTS);
|
||||||
|
//}
|
||||||
|
|
||||||
|
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
|
||||||
|
const MyState est = pf->update(nullptr, obs);
|
||||||
|
const Point3 curEst = est.pCur;
|
||||||
|
pathEst.push_back(curEst);
|
||||||
|
|
||||||
|
// debug print current particle set.
|
||||||
|
//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
|
||||||
|
|
||||||
|
// error calculation. compare ground-truth to estimation
|
||||||
|
// const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
|
||||||
|
|
||||||
|
// // TODO
|
||||||
|
// const Point3 diff = curEst - curGT;
|
||||||
|
|
||||||
|
// //if (std::abs(diff.z) < 0.1) {
|
||||||
|
// const float err = diff.length();
|
||||||
|
// std::cout << err << std::endl;
|
||||||
|
// stats.add(err);
|
||||||
|
// std::cout << stats.asString() << std::endl;
|
||||||
|
// //}
|
||||||
|
|
||||||
|
vis.clearStates();
|
||||||
|
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
|
||||||
|
|
||||||
|
vis.show();;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
// vis.setShowParticles(false);
|
||||||
|
// vis.setShowTime(false);
|
||||||
|
// vis.setShowCurPos(false);
|
||||||
|
// vis.debugProcess(0, pathEst, gtw, pf, layers);
|
||||||
|
// std::ofstream out("/tmp/" + runName + ".data");
|
||||||
|
// out << vis.getDataset();
|
||||||
|
// out.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
sleep(1000);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // EVALBASE_H
|
||||||
24
code/eval/GroundTruthWay.h
Normal file
24
code/eval/GroundTruthWay.h
Normal file
@@ -0,0 +1,24 @@
|
|||||||
|
#ifndef GROUNDTRUTHWAY_H
|
||||||
|
#define GROUNDTRUTHWAY_H
|
||||||
|
|
||||||
|
#include <Indoor/math/Interpolator.h>
|
||||||
|
#include <Indoor/geo/Point3.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* interpolated ground-trouth based on timed check-points
|
||||||
|
*/
|
||||||
|
class GroundTruthWay : public Interpolator<uint64_t, Point3> {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
Point3 getPosAtTime(const uint64_t ts) const {
|
||||||
|
return get(ts);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** get the ground truth way */
|
||||||
|
const std::vector<Entry>& getWay() const {return entries;}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif // GROUNDTRUTHWAY_H
|
||||||
@@ -39,10 +39,12 @@ public:
|
|||||||
if (!beacon) {continue;}
|
if (!beacon) {continue;}
|
||||||
|
|
||||||
// distance (in meter) between particle and AP
|
// distance (in meter) between particle and AP
|
||||||
const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
|
//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
|
||||||
|
const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0;
|
||||||
|
|
||||||
// floor difference?
|
// floor difference?
|
||||||
const double floorDist = std::abs(beacon->zNr - state.z_nr);
|
//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
|
||||||
|
const float floorDist = std::round(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
|
||||||
|
|
||||||
// estimate the rssi depending on above distance
|
// estimate the rssi depending on above distance
|
||||||
const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
|
const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
#ifndef BEACONSENSORREADER_H
|
#ifndef BEACONSENSORREADER_H
|
||||||
#define BEACONSENSORREADER_H
|
#define BEACONSENSORREADER_H
|
||||||
|
|
||||||
#include "../SensorReader.h"
|
#include "../reader/SensorReader.h"
|
||||||
#include "BeaconObservation.h"
|
#include "BeaconObservation.h"
|
||||||
#include "Settings.h"
|
#include "Settings.h"
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
|||||||
@@ -2,19 +2,26 @@
|
|||||||
#define POSITIONEDBEACON_H
|
#define POSITIONEDBEACON_H
|
||||||
|
|
||||||
#include "WiFiAP.h"
|
#include "WiFiAP.h"
|
||||||
#include "Position3D.h"
|
//#include "Position3D.h"
|
||||||
|
#include <Indoor/geo/Point3.h>
|
||||||
|
|
||||||
class PositionedBeacon : public Position3D {
|
class PositionedBeacon : public Point3 {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
MACAddress mac;
|
MACAddress mac;
|
||||||
double tx;
|
float tx;
|
||||||
double pl;
|
float pl;
|
||||||
|
|
||||||
|
// /** ctor */
|
||||||
|
// PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||||
|
// mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
|
||||||
|
// ;
|
||||||
|
// }
|
||||||
|
|
||||||
/** ctor */
|
/** ctor */
|
||||||
PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
|
||||||
mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
|
mac(mac), tx(tx), pl(pl), Point3(x_cm, y_cm, z_cm) {
|
||||||
;
|
;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -2,16 +2,22 @@
|
|||||||
#define POSITIONEDWIFIAP_H
|
#define POSITIONEDWIFIAP_H
|
||||||
|
|
||||||
#include "WiFiAP.h"
|
#include "WiFiAP.h"
|
||||||
#include "Position3D.h"
|
//#include "Position3D.h"
|
||||||
|
#include <Indoor/geo/Point3.h>
|
||||||
|
|
||||||
|
class PositionedWifiAP : public WiFiAP, public Point3 {
|
||||||
class PositionedWifiAP : public WiFiAP, public Position3D {
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
// /** ctor */
|
||||||
|
// PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||||
|
// WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
|
||||||
|
// ;
|
||||||
|
// }
|
||||||
|
|
||||||
/** ctor */
|
/** ctor */
|
||||||
PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
|
||||||
WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
|
WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) {
|
||||||
;
|
;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -7,6 +7,8 @@
|
|||||||
|
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
|
||||||
|
#include "../Helper.h"
|
||||||
|
|
||||||
class Settings {
|
class Settings {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
@@ -21,49 +23,49 @@ public:
|
|||||||
const double pl = 2.7;
|
const double pl = 2.7;
|
||||||
const double tx = -46;
|
const double tx = -46;
|
||||||
|
|
||||||
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, 3, tx, pl-0.5);
|
addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl-0.5);
|
||||||
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, 3, tx, pl-0.5);
|
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl-0.5);
|
||||||
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5);
|
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.5);
|
||||||
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl);
|
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl);
|
||||||
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, 3, tx, pl);
|
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl);
|
||||||
|
|
||||||
// 2nd floor (vague AP position)
|
// 2nd floor (vague AP position)
|
||||||
addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, 2, tx, pl-0.5);
|
addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl-0.5);
|
||||||
addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, 2, tx, pl);
|
addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, Helper::getHeight(2), tx, pl);
|
||||||
addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, 2, tx, pl);
|
addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, Helper::getHeight(2), tx, pl);
|
||||||
addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2, tx, pl-0.5);
|
addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl-0.5);
|
||||||
addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 2, tx, pl-0.5);
|
addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5);
|
||||||
|
|
||||||
// 1st floor (vague AP position)
|
// 1st floor (vague AP position)
|
||||||
addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, 1, tx, pl);
|
addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, Helper::getHeight(1), tx, pl);
|
||||||
addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1, tx, pl);
|
addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, Helper::getHeight(1), tx, pl);
|
||||||
addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, 1, tx, pl-0.25);
|
addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl-0.25);
|
||||||
addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1, tx, pl-0.25);
|
addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl-0.25);
|
||||||
addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1, tx, pl);
|
addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, Helper::getHeight(1), tx, pl);
|
||||||
addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl);
|
addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl);
|
||||||
|
|
||||||
// 0th floor (exact AP position)
|
// 0th floor (exact AP position)
|
||||||
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, 0, tx, pl);
|
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, Helper::getHeight(0), tx, pl);
|
||||||
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, 0, tx, pl);
|
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, Helper::getHeight(0), tx, pl);
|
||||||
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, 0, tx, pl);
|
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, Helper::getHeight(0), tx, pl);
|
||||||
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, 0, tx, pl);
|
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, Helper::getHeight(0), tx, pl);
|
||||||
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, 0, tx, pl);
|
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
|
||||||
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 0, tx, pl); // vague!!
|
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
|
||||||
|
|
||||||
const int ibOff = +2;
|
const int ibOff = +2;
|
||||||
const float ibPLE = 1.9;
|
const float ibPLE = 1.9;
|
||||||
addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, 3); // id:16
|
addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3)); // id:16
|
||||||
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2); // id:18
|
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2)); // id:18
|
||||||
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1); // id:11
|
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1)); // id:11
|
||||||
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0); // id:20
|
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0)); // id:20
|
||||||
|
|
||||||
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, 2); // idis switchboard
|
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2)); // idis switchboard
|
||||||
addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2); // idis outside
|
addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2)); // idis outside
|
||||||
addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2); // idis toni
|
addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2)); // idis toni
|
||||||
|
|
||||||
addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, 1); // i.1.47
|
addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47
|
||||||
addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1); // fachschaft
|
addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft
|
||||||
addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 1); // neben fachschaft
|
addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft
|
||||||
|
|
||||||
// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
|
// addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3);
|
||||||
// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
|
// addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);
|
||||||
|
|||||||
@@ -37,14 +37,14 @@ public:
|
|||||||
|
|
||||||
//const double tx = -48; // tablet
|
//const double tx = -48; // tablet
|
||||||
//const double pl = 3.15;
|
//const double pl = 3.15;
|
||||||
const double waf = 7;//10.0;
|
const float waf = 7;//10.0; // was 7 before?! has something todo with the floor heights / levels
|
||||||
const double floor_height_cm = 350;
|
// const int floor_height_cm = 350;
|
||||||
|
|
||||||
// get the ap the client had the strongest measurement for
|
// get the ap the client had the strongest measurement for
|
||||||
const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
|
const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
|
||||||
const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
|
//const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
|
||||||
const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
|
//const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
|
||||||
const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
|
//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
|
||||||
|
|
||||||
// process each detected AP
|
// process each detected AP
|
||||||
for (const WiFiObservationEntry& entry : obs.entries) {
|
for (const WiFiObservationEntry& entry : obs.entries) {
|
||||||
@@ -53,20 +53,23 @@ public:
|
|||||||
const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);
|
const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);
|
||||||
|
|
||||||
// distance (in meter) between particle and AP
|
// distance (in meter) between particle and AP
|
||||||
const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
|
//const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
|
||||||
|
const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0;
|
||||||
|
|
||||||
// floor difference?
|
// floor difference?
|
||||||
const double floorDist = std::abs(ap->zNr - state.z_nr);
|
const float floorDiff = std::round(
|
||||||
|
std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z))
|
||||||
|
);
|
||||||
|
|
||||||
// estimate the rssi depending on above distance
|
// estimate the rssi depending on above distance
|
||||||
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDist * waf);
|
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);
|
||||||
|
|
||||||
// the measured rssi
|
// the measured rssi
|
||||||
const double realRSSI = entry.rssi;
|
const double realRSSI = entry.rssi;
|
||||||
|
|
||||||
// the measured relative rssi
|
// the measured relative rssi
|
||||||
const double realRelRSSI = strongest.rssi - realRSSI;
|
//const double realRelRSSI = strongest.rssi - realRSSI;
|
||||||
const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
|
//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
|
||||||
|
|
||||||
// probability? (sigma grows with measurement's age)
|
// probability? (sigma grows with measurement's age)
|
||||||
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
|
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
#ifndef WIFISENSORREADER_H
|
#ifndef WIFISENSORREADER_H
|
||||||
#define WIFISENSORREADER_H
|
#define WIFISENSORREADER_H
|
||||||
|
|
||||||
#include "../SensorReader.h"
|
#include "../reader/SensorReader.h"
|
||||||
#include "WiFiObservation.h"
|
#include "WiFiObservation.h"
|
||||||
|
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
|||||||
@@ -8,10 +8,10 @@
|
|||||||
#include "StepObservation.h"
|
#include "StepObservation.h"
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
static double mu_walk = 40;
|
static constexpr double mu_walk = 40;
|
||||||
static double sigma_walk = 15;
|
static constexpr double sigma_walk = 15;
|
||||||
static double mu_stop = 0;
|
static constexpr double mu_stop = 0;
|
||||||
static double sigma_stop = 5;
|
static constexpr double sigma_stop = 5;
|
||||||
|
|
||||||
class StepEvaluation {
|
class StepEvaluation {
|
||||||
|
|
||||||
@@ -19,29 +19,31 @@ public:
|
|||||||
|
|
||||||
double getProbability(const MyState& state, const StepObservation* obs) const {
|
double getProbability(const MyState& state, const StepObservation* obs) const {
|
||||||
|
|
||||||
double distance = state.distanceWalkedCM;
|
return 1;
|
||||||
|
|
||||||
double a = 1.0;
|
// double distance = state.distanceWalkedCM;
|
||||||
double mu_distance = 0; //cm
|
|
||||||
double sigma_distance = 10.0; //cm
|
|
||||||
|
|
||||||
if(obs->step) {
|
// double a = 1.0;
|
||||||
a = 1.0;
|
// double mu_distance = 0; //cm
|
||||||
mu_distance = mu_walk;//80.0; //cm
|
// double sigma_distance = 10.0; //cm
|
||||||
sigma_distance = sigma_walk;//40.0; //cm
|
|
||||||
}
|
|
||||||
|
|
||||||
else {
|
// if(obs->step) {
|
||||||
a = 0.0;
|
// a = 1.0;
|
||||||
mu_distance = mu_stop; //cm
|
// mu_distance = mu_walk;//80.0; //cm
|
||||||
sigma_distance = sigma_stop; //cm
|
// sigma_distance = sigma_walk;//40.0; //cm
|
||||||
}
|
// }
|
||||||
|
|
||||||
//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
|
// else {
|
||||||
const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
|
// a = 0.0;
|
||||||
(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
|
// mu_distance = mu_stop; //cm
|
||||||
|
// sigma_distance = sigma_stop; //cm
|
||||||
|
// }
|
||||||
|
|
||||||
return p;
|
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
|
||||||
|
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
|
||||||
|
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
|
||||||
|
|
||||||
|
// return p;
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -4,10 +4,10 @@
|
|||||||
|
|
||||||
#include "../particles/MyState.h"
|
#include "../particles/MyState.h"
|
||||||
#include "TurnObservation.h"
|
#include "TurnObservation.h"
|
||||||
#include <boost/math/special_functions/bessel.hpp>
|
//#include <boost/math/special_functions/bessel.hpp>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
static double sigma_heading = 35;
|
static constexpr double sigma_heading = 35;
|
||||||
|
|
||||||
class TurnEvaluation {
|
class TurnEvaluation {
|
||||||
|
|
||||||
@@ -17,55 +17,33 @@ public:
|
|||||||
|
|
||||||
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
|
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
|
||||||
|
|
||||||
//Particle's heading change
|
return 1;
|
||||||
double delta_heading_particle = state.heading - state.heading_old;
|
|
||||||
|
// //Particle's heading change
|
||||||
|
// double delta_heading_particle = state.heading - state.heading_old;
|
||||||
|
|
||||||
|
|
||||||
//Correct offset of the heading change
|
// //Correct offset of the heading change
|
||||||
if (delta_heading_particle < -180) {
|
// if (delta_heading_particle < -180) {
|
||||||
delta_heading_particle += 360;
|
// delta_heading_particle += 360;
|
||||||
}
|
// }
|
||||||
else if (delta_heading_particle > 180) {
|
// else if (delta_heading_particle > 180) {
|
||||||
delta_heading_particle -= 360;
|
// delta_heading_particle -= 360;
|
||||||
}
|
// }
|
||||||
|
|
||||||
|
|
||||||
//Switch between simple and improved evaluation
|
// //Switch between simple and improved evaluation
|
||||||
//"Simple" only evaluates the deviation between the measured heading and the particle heading change using
|
// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
|
||||||
//normal distribution
|
// //normal distribution
|
||||||
if(simple) {
|
// //if(simple) {
|
||||||
|
|
||||||
double sigma_delta_heading = sigma_heading;
|
// double sigma_delta_heading = sigma_heading;
|
||||||
|
|
||||||
const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
|
// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
|
||||||
|
|
||||||
|
|
||||||
return p;
|
// return p;
|
||||||
}
|
// // }
|
||||||
|
|
||||||
//use the von Mises distribution
|
|
||||||
else {
|
|
||||||
//Here some calculations must be done in rad
|
|
||||||
|
|
||||||
double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
|
|
||||||
double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
|
|
||||||
|
|
||||||
//Equation for estimating kappa value of von Mises distribution
|
|
||||||
//empirically estimated
|
|
||||||
double kappa = 0.0;
|
|
||||||
|
|
||||||
kappa = 5.0 / exp(2 * delta_motion_rad);
|
|
||||||
|
|
||||||
double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
|
|
||||||
const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
|
|
||||||
|
|
||||||
return p;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
#ifndef TURNREADER_H
|
#ifndef TURNREADER_H
|
||||||
#define TURNREADER_H
|
#define TURNREADER_H
|
||||||
|
|
||||||
#include "../SensorReaderTurn.h"
|
#include "../reader/SensorReaderTurn.h"
|
||||||
#include "TurnObservation.h"
|
#include "TurnObservation.h"
|
||||||
|
|
||||||
class TurnReader {
|
class TurnReader {
|
||||||
|
|||||||
113
code/main.cpp
113
code/main.cpp
@@ -1,58 +1,87 @@
|
|||||||
#include <Indoor/grid/factory/GridFactory.h>
|
|
||||||
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalkPushForward.h>
|
||||||
|
|
||||||
#include "Vis.h"
|
#include "Vis.h"
|
||||||
|
#include "Helper.h"
|
||||||
|
#include "MyGridNode.h"
|
||||||
|
#include "Helper.h"
|
||||||
|
#include "DijkstraMapper.h"
|
||||||
|
|
||||||
namespace Settings {
|
#include "eval/Eval.h"
|
||||||
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
|
#include "eval/EvalBase.h"
|
||||||
const int gridSize_cm = 200;
|
|
||||||
|
Settings settings;
|
||||||
|
|
||||||
|
void testModelWalk() {
|
||||||
|
|
||||||
|
Grid<MyGridNode> grid(MiscSettings::gridSize_cm);
|
||||||
|
|
||||||
|
Helper::FHWSFloors floors = Helper::getFloors();
|
||||||
|
Helper::buildTheGrid(grid, floors);
|
||||||
|
|
||||||
|
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
|
||||||
|
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
|
||||||
|
|
||||||
|
Vis vis;
|
||||||
|
vis.addFloor(floors.f0, floors.h0);
|
||||||
|
vis.addFloor(floors.f1, floors.h1);
|
||||||
|
vis.addFloor(floors.f2, floors.h2);
|
||||||
|
vis.addFloor(floors.f3, floors.h3);
|
||||||
|
|
||||||
|
// vis.gp << "set xrange [1000:4000]\n";
|
||||||
|
// vis.gp << "set yrange [1000:4000]\n";
|
||||||
|
// vis.gp << "set zrange [0:600]\n";
|
||||||
|
|
||||||
|
// switch between different grid-walkers
|
||||||
|
GridWalkRandomHeadingUpdate<MyGridNode> walk;
|
||||||
|
//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
|
||||||
|
//GridWalkPushForward<MyGridNode> walk;
|
||||||
|
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);
|
||||||
|
|
||||||
|
|
||||||
|
std::vector<GridWalkState<MyGridNode>> states;
|
||||||
|
for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }
|
||||||
|
|
||||||
|
// track the number-of-visits for each node to draw something like a particle-heat-map?
|
||||||
|
|
||||||
|
// show the importance factors
|
||||||
|
// vis.addGrid(grid);
|
||||||
|
// vis.show();
|
||||||
|
// sleep(100);
|
||||||
|
// vis.removeGrid();
|
||||||
|
|
||||||
|
Distribution::Normal<float> wDist(0.3, 0.3);
|
||||||
|
|
||||||
|
while(true) {
|
||||||
|
for (GridWalkState<MyGridNode>& state : states) {
|
||||||
|
state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
|
||||||
|
}
|
||||||
|
usleep(1000*80);
|
||||||
|
vis.showStates(states);
|
||||||
|
vis.show();
|
||||||
}
|
}
|
||||||
|
|
||||||
struct MyNode : public GridNode, public GridPoint {
|
|
||||||
public:
|
|
||||||
MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
int align(const int val) {
|
sleep(1000);
|
||||||
return val / Settings::gridSize_cm * Settings::gridSize_cm;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int main(void) {
|
int main(void) {
|
||||||
|
|
||||||
Grid<MyNode> grid(Settings::gridSize_cm);
|
//testModelWalk();
|
||||||
GridFactory<MyNode> gridFac(grid);
|
|
||||||
|
|
||||||
|
Eval eval;
|
||||||
FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);
|
eval.setEval1();
|
||||||
|
eval.run();
|
||||||
Floor f0 = fpFac.getFloor("floor_0");
|
|
||||||
Floor f1 = fpFac.getFloor("floor_1");
|
|
||||||
Floor f2 = fpFac.getFloor("floor_2");
|
|
||||||
Floor f3 = fpFac.getFloor("floor_3");
|
|
||||||
|
|
||||||
Stairs f01 = fpFac.getStairs("staircase_0_1");
|
|
||||||
Stairs f12 = fpFac.getStairs("staircase_1_2");
|
|
||||||
Stairs f23 = fpFac.getStairs("staircase_2_3");
|
|
||||||
|
|
||||||
const LengthF h0 = LengthF::cm(align(0));
|
|
||||||
const LengthF h1 = LengthF::cm(align(360));
|
|
||||||
const LengthF h2 = LengthF::cm(align(360+340));
|
|
||||||
const LengthF h3 = LengthF::cm(align(360+340+340));
|
|
||||||
|
|
||||||
gridFac.addFloor(f0, h0.cm());
|
|
||||||
gridFac.addFloor(f1, h1.cm());
|
|
||||||
gridFac.addFloor(f2, h2.cm());
|
|
||||||
gridFac.addFloor(f3, h3.cm());
|
|
||||||
//gridFac.removeIsolated();
|
|
||||||
|
|
||||||
Vis vis;
|
|
||||||
vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3);
|
|
||||||
vis.addGrid(grid);
|
|
||||||
vis.show();
|
|
||||||
|
|
||||||
sleep(1000);
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
8
code/particles/MyControl.h
Executable file
8
code/particles/MyControl.h
Executable file
@@ -0,0 +1,8 @@
|
|||||||
|
#ifndef MYCONTROL_H
|
||||||
|
#define MYCONTROL_H
|
||||||
|
|
||||||
|
struct MyControl {
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYCONTROL_H
|
||||||
85
code/particles/MyEvaluation.h
Executable file
85
code/particles/MyEvaluation.h
Executable file
@@ -0,0 +1,85 @@
|
|||||||
|
#ifndef MYEVALUATION_H
|
||||||
|
#define MYEVALUATION_H
|
||||||
|
|
||||||
|
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
|
||||||
|
#include "MyObservation.h"
|
||||||
|
#include "MyState.h"
|
||||||
|
|
||||||
|
#include "../frank/WiFiEvaluation.h"
|
||||||
|
#include "../frank/BeaconEvaluation.h"
|
||||||
|
#include "../toni/BarometerEvaluation.h"
|
||||||
|
#include "../lukas/StepEvaluation.h"
|
||||||
|
#include "../lukas/TurnEvaluation.h"
|
||||||
|
|
||||||
|
class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
WiFiEvaluation wifiEval;
|
||||||
|
BeaconEvaluation beaconEval;
|
||||||
|
BarometerEvaluation barometerEval;
|
||||||
|
StepEvaluation stepEval;
|
||||||
|
TurnEvaluation turnEval;
|
||||||
|
|
||||||
|
bool useWifi = true;
|
||||||
|
bool useStep = true;
|
||||||
|
bool useTurn = true;
|
||||||
|
bool useBaro = true;
|
||||||
|
bool useIB = true;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
|
||||||
|
this->useWifi = useWifi;
|
||||||
|
this->useStep = useStep;
|
||||||
|
this->useTurn = useTurn;
|
||||||
|
this->useBaro = useBaro;
|
||||||
|
this->useIB = useIB;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
|
||||||
|
|
||||||
|
//if (observation.wifi) {
|
||||||
|
wifiEval.nextObservation(observation.wifi);
|
||||||
|
//}
|
||||||
|
|
||||||
|
// evalulate each particle
|
||||||
|
double sum = 0;
|
||||||
|
for (K::Particle<MyState>& p : particles) {
|
||||||
|
|
||||||
|
double weight = 1.0;
|
||||||
|
|
||||||
|
if (useWifi) {
|
||||||
|
weight *= wifiEval.getProbability(p.state, observation);
|
||||||
|
}
|
||||||
|
|
||||||
|
// if (useBaro && observation.barometer) {
|
||||||
|
// weight *= barometerEval.getProbability(p.state, observation.barometer);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (useIB) {
|
||||||
|
// weight *= beaconEval.getProbability(p.state, observation);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (useStep) {
|
||||||
|
// weight *= stepEval.getProbability(p.state, observation.step);
|
||||||
|
// p.state.distanceWalkedCM = 0.0;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (useTurn) {
|
||||||
|
// weight *= turnEval.getProbability(p.state, observation.turn, true);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// set and accumulate
|
||||||
|
p.weight = weight;
|
||||||
|
sum += p.weight;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return sum;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYEVALUATION_H
|
||||||
59
code/particles/MyInitializer.h
Executable file
59
code/particles/MyInitializer.h
Executable file
@@ -0,0 +1,59 @@
|
|||||||
|
#ifndef MYINITIALIZER3_H
|
||||||
|
#define MYINITIALIZER3_H
|
||||||
|
|
||||||
|
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
||||||
|
#include "MyState.h"
|
||||||
|
|
||||||
|
#include <Indoor/grid/Grid.h>
|
||||||
|
|
||||||
|
class MyInitializer : public K::ParticleFilterInitializer<MyState> {
|
||||||
|
|
||||||
|
private:
|
||||||
|
int x_cm;
|
||||||
|
int y_cm;
|
||||||
|
int z_cm;
|
||||||
|
int heading;
|
||||||
|
|
||||||
|
Grid<MyGridNode>& grid;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** q0 = random */
|
||||||
|
MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/** q0 = given */
|
||||||
|
MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
|
||||||
|
grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
||||||
|
|
||||||
|
std::minstd_rand gen;
|
||||||
|
std::uniform_int_distribution<> dist(0, grid.getNumNodes());
|
||||||
|
|
||||||
|
for (K::Particle<MyState>& p : particles) {
|
||||||
|
|
||||||
|
MyGridNode& n = grid[dist(gen)];
|
||||||
|
|
||||||
|
//p.state.pCur = Point3(x_cm, y_cm, z_cm);
|
||||||
|
//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
|
||||||
|
//p.state.walkState.node = &grid.getNodeFor(gp);
|
||||||
|
|
||||||
|
p.state.pCur = (Point3) n;
|
||||||
|
p.state.walkState.node = &n;
|
||||||
|
|
||||||
|
p.state.pOld = p.state.pCur;
|
||||||
|
p.state.walkState.heading = Heading::rnd();
|
||||||
|
p.state.distanceWalkedCM = 0;
|
||||||
|
p.state.hPa = 0;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYINITIALIZER_H
|
||||||
49
code/particles/MyObservation.h
Executable file
49
code/particles/MyObservation.h
Executable file
@@ -0,0 +1,49 @@
|
|||||||
|
#ifndef MYOBSERVATION_H
|
||||||
|
#define MYOBSERVATION_H
|
||||||
|
|
||||||
|
#include "../frank/WiFiObservation.h"
|
||||||
|
#include "../frank/BeaconObservation.h"
|
||||||
|
#include "../toni/BarometerObservation.h"
|
||||||
|
#include "../lukas/StepObservation.h"
|
||||||
|
#include "../lukas/TurnObservation.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* all available sensor readings
|
||||||
|
*/
|
||||||
|
struct MyObservation {
|
||||||
|
|
||||||
|
/** wifi observation */
|
||||||
|
WiFiObservation wifi;
|
||||||
|
|
||||||
|
/** barometer observation data (if any) */
|
||||||
|
BarometerObservation* barometer = nullptr;
|
||||||
|
|
||||||
|
/** beacon observation data */
|
||||||
|
BeaconObservation beacons;
|
||||||
|
|
||||||
|
/** step observation data (if any) */
|
||||||
|
StepObservation* step = nullptr;
|
||||||
|
|
||||||
|
/** turn observation data (if any) */
|
||||||
|
TurnObservation* turn = nullptr;
|
||||||
|
|
||||||
|
/** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */
|
||||||
|
uint64_t latestSensorDataTS = 0;
|
||||||
|
|
||||||
|
/** ctor */
|
||||||
|
MyObservation() {
|
||||||
|
// reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
// /** set all observations to null */
|
||||||
|
// void reset() {
|
||||||
|
// //delete wifi; wifi = nullptr;
|
||||||
|
// delete barometer; barometer = nullptr;
|
||||||
|
// delete beacons; beacons = nullptr;
|
||||||
|
// //delete step; step = nullptr;
|
||||||
|
// //delete turn; turn = nullptr;
|
||||||
|
// }
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYOBSERVATION_H
|
||||||
136
code/particles/MyState.h
Executable file
136
code/particles/MyState.h
Executable file
@@ -0,0 +1,136 @@
|
|||||||
|
#ifndef MYSTATE_H
|
||||||
|
#define MYSTATE_H
|
||||||
|
|
||||||
|
#include <KLib/math/distribution/Normal.h>
|
||||||
|
#include <KLib/math/optimization/NumOptVector.h>
|
||||||
|
|
||||||
|
#include <Indoor/grid/walk/GridWalkState.h>
|
||||||
|
|
||||||
|
#include "../MyGridNode.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* one possible state for the pedestrian
|
||||||
|
* 3D position (x, y, floor-nr)
|
||||||
|
*/
|
||||||
|
struct MyState {
|
||||||
|
|
||||||
|
// current position
|
||||||
|
Point3 pCur;
|
||||||
|
|
||||||
|
// previous position
|
||||||
|
Point3 pOld;
|
||||||
|
|
||||||
|
// the grid-walk state
|
||||||
|
GridWalkState<MyGridNode> walkState;
|
||||||
|
|
||||||
|
int distanceWalkedCM;
|
||||||
|
|
||||||
|
|
||||||
|
// double heading_old;
|
||||||
|
|
||||||
|
// //double transHeading;
|
||||||
|
// float numZChanges;
|
||||||
|
|
||||||
|
// // cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
|
||||||
|
// double distanceWalkedCM;
|
||||||
|
|
||||||
|
double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h
|
||||||
|
|
||||||
|
// double vertical_acc; //vertical acceleration
|
||||||
|
|
||||||
|
// /** the pedestrian's current heading */
|
||||||
|
// double heading;
|
||||||
|
|
||||||
|
/** empty ctor */
|
||||||
|
MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
// /** get the 2D distance between this state and the given x,y (in centimter) */
|
||||||
|
// double getDistance2D(const double x_cm, const double y_cm) const {
|
||||||
|
// const double dx = (x_cm - this->x_cm);
|
||||||
|
// const double dy = (y_cm - this->y_cm);
|
||||||
|
// return std::sqrt( (dx*dx) + (dy*dy) );
|
||||||
|
// }
|
||||||
|
|
||||||
|
// /** get the 3D distance between this state and the given x,y,floor (in centimter) */
|
||||||
|
// double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
|
||||||
|
// const double dx = (x_cm - this->x_cm);
|
||||||
|
// const double dy = (y_cm - this->y_cm);
|
||||||
|
// const double dz = (z_nr - this->z_nr) * floor_height_cm;
|
||||||
|
// return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
|
||||||
|
// }
|
||||||
|
|
||||||
|
/** -------- METHODS FOR THE PARTICLE FILTER -------- */
|
||||||
|
|
||||||
|
MyState& operator += (const MyState& o) {
|
||||||
|
pCur += o.pCur;
|
||||||
|
//hPa += o.hPa;
|
||||||
|
//distanceWalked += o.distanceWalked;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
MyState& operator /= (const double d) {
|
||||||
|
pCur /= d;
|
||||||
|
//hPa /= d;
|
||||||
|
//distanceWalked /= d;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
MyState operator * (const double d) const {
|
||||||
|
MyState s = MyState(*this);
|
||||||
|
s.pCur *= d;
|
||||||
|
//s.hPa *= d;
|
||||||
|
//distanceWalked *= d;
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// use the default one
|
||||||
|
// MyState& operator = (const MyState& o) {
|
||||||
|
// x_cm = o.x_cm;
|
||||||
|
// y_cm = o.y_cm;
|
||||||
|
// z_nr = o.z_nr;
|
||||||
|
// x_cm_old = o.x_cm_old;
|
||||||
|
// y_cm_old = o.y_cm_old;
|
||||||
|
// z_nr_old = o.z_nr_old;
|
||||||
|
// hPa = o.hPa;
|
||||||
|
// heading_old = o.heading_old;
|
||||||
|
// heading = o.heading;
|
||||||
|
// distanceWalkedCM = o.distanceWalkedCM;
|
||||||
|
// return *this;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// /** rejection for the regional estimator. reject after 150cm distance */
|
||||||
|
// bool belongsToRegion(const MyState& o) const {
|
||||||
|
|
||||||
|
//// // do NOT group particles in distinct floors!
|
||||||
|
//// if (z_nr != o.z_nr) {return false;}
|
||||||
|
|
||||||
|
//// // get the 2D distance
|
||||||
|
//// double d = (x_cm - o.x_cm)*(x_cm - o.x_cm) +
|
||||||
|
//// (y_cm - o.y_cm)*(y_cm - o.y_cm);
|
||||||
|
//// d = std::sqrt(d);
|
||||||
|
|
||||||
|
//// // 2D distance below grouping threshold?
|
||||||
|
//// return d < 350.0;
|
||||||
|
|
||||||
|
// const double dx = (x_cm - o.x_cm);
|
||||||
|
// const double dy = (y_cm - o.y_cm);
|
||||||
|
// const double dz = (z_nr - o.z_nr) * 3000;
|
||||||
|
|
||||||
|
// // get the 2D distance
|
||||||
|
// double d = dx*dx + dy*dy + dz*dz;
|
||||||
|
// d = std::sqrt(d);
|
||||||
|
// return d < 350.0;
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYSTATE_H
|
||||||
189
code/particles/MyTransition.h
Executable file
189
code/particles/MyTransition.h
Executable file
@@ -0,0 +1,189 @@
|
|||||||
|
#ifndef MYTRANSITION_H
|
||||||
|
#define MYTRANSITION_H
|
||||||
|
|
||||||
|
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||||
|
#include <KLib/math/distribution/Normal.h>
|
||||||
|
#include <KLib/math/distribution/Uniform.h>
|
||||||
|
|
||||||
|
|
||||||
|
#include <Indoor/grid/Grid.h>
|
||||||
|
#include <Indoor/grid/walk/GridWalk.h>
|
||||||
|
|
||||||
|
#include "MyState.h"
|
||||||
|
#include "MyControl.h"
|
||||||
|
//#include "Helper.h"
|
||||||
|
|
||||||
|
#include "../toni/barometric.h"
|
||||||
|
|
||||||
|
#include "../MyGridNode.h"
|
||||||
|
|
||||||
|
inline double sgn(double x){
|
||||||
|
return ((x>0)?1 : ((x<0)?-1 : 1));
|
||||||
|
}
|
||||||
|
|
||||||
|
class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
Grid<MyGridNode>& grid;
|
||||||
|
GridWalk<MyGridNode>& walker;
|
||||||
|
|
||||||
|
|
||||||
|
/** a simple normal distribution */
|
||||||
|
K::UniformDistribution distWalkStop;
|
||||||
|
K::NormalDistribution distWalk;
|
||||||
|
K::NormalDistribution distStop;
|
||||||
|
|
||||||
|
|
||||||
|
/** normal distribution for barometer */
|
||||||
|
K::NormalDistribution distBaro;
|
||||||
|
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/**
|
||||||
|
* ctor
|
||||||
|
* @param choice the choice to use for randomly drawing nodes
|
||||||
|
* @param fp the underlying floorplan
|
||||||
|
*/
|
||||||
|
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
|
||||||
|
grid(grid), walker(walker),
|
||||||
|
distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
|
||||||
|
|
||||||
|
distWalkStop.setSeed(1234);
|
||||||
|
distWalk.setSeed(1234);
|
||||||
|
distStop.setSeed(1234);
|
||||||
|
|
||||||
|
distBaro.setSeed(5678);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
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 {
|
||||||
|
deltaMS = ts - this->ts;
|
||||||
|
this->ts = ts;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||||
|
|
||||||
|
for (K::Particle<MyState>& p : particles) {
|
||||||
|
|
||||||
|
// TODO: depending on the time since the last update
|
||||||
|
// random distance to move
|
||||||
|
// const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
|
||||||
|
// double dist_m = distance * deltaMS / 1000.0;
|
||||||
|
|
||||||
|
// if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
|
||||||
|
|
||||||
|
// update the old heading and the other old values
|
||||||
|
//p.state.walkState.heading = p.state.heading;
|
||||||
|
p.state.pOld = p.state.pCur;
|
||||||
|
|
||||||
|
|
||||||
|
// 10% stand still, 90% walk
|
||||||
|
double dist_m;
|
||||||
|
if (distWalkStop.draw() > 0.9) {
|
||||||
|
dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
|
||||||
|
} else {
|
||||||
|
dist_m = std::abs(distWalk.draw() * deltaMS / 1000.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// update cumulative distance
|
||||||
|
p.state.distanceWalkedCM += std::abs(dist_m * 100.0);
|
||||||
|
|
||||||
|
// find the node (square) the particle is within
|
||||||
|
// just to be safe, we round z to the nearest floor
|
||||||
|
//Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr));
|
||||||
|
const MyGridNode* src = p.state.walkState.node;
|
||||||
|
|
||||||
|
// might happen during initialization:
|
||||||
|
// the particle is nowhere near the grid.. replace it with a random on on the grid
|
||||||
|
// alternative: just ignore.. resampling will fix this issue quickly ;)
|
||||||
|
// if (!src) {
|
||||||
|
// auto it = graph->getNodes().begin();
|
||||||
|
// std::advance(it, rand() % graph->getNodes().size());
|
||||||
|
// src = it->second;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// get new destination
|
||||||
|
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
|
||||||
|
p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );
|
||||||
|
|
||||||
|
// randomly move the particle within its target grid (box)
|
||||||
|
// (z remains unchanged!)
|
||||||
|
const int grid_size_cm = grid.getGridSize_cm();
|
||||||
|
|
||||||
|
// new position (x,y) is randomly distributed within the target node
|
||||||
|
Point3 noise = Point3(0,0,0); // TODO
|
||||||
|
p.state.pCur = (Point3) *p.state.walkState.node + noise;
|
||||||
|
|
||||||
|
|
||||||
|
// --- ATTENTION HORRIBLE CODE INCOMING. ---
|
||||||
|
|
||||||
|
// //how many floors are changed? and in what direction (given by the sign)
|
||||||
|
// double numFloorChanged = p.state.z_nr_old - p.state.z_nr;
|
||||||
|
|
||||||
|
// //The height of the single floor levels.
|
||||||
|
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||||
|
|
||||||
|
// //update barometer
|
||||||
|
// if(USE_BAROMETRIC_FORMULAR){
|
||||||
|
// //height the particle has climbed.
|
||||||
|
// double h_1 = 0.0;
|
||||||
|
// double mu = 0.0;
|
||||||
|
|
||||||
|
// //we need only the sign of the floors changed, since the pressure change between the floors
|
||||||
|
// //is calculated within s_getAtmosphericPressure
|
||||||
|
// numFloorChanged = sgn(numFloorChanged);
|
||||||
|
|
||||||
|
// for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
|
||||||
|
// h_1 += floor_height[i];
|
||||||
|
// }
|
||||||
|
|
||||||
|
// {
|
||||||
|
// // use the barometric formular to calculate the relative pressure
|
||||||
|
// // the calculation is done assuming sea level height at every floor.
|
||||||
|
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||||
|
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||||
|
// mu = std::abs(mslp - pressure);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||||
|
// p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
|
||||||
|
// else
|
||||||
|
// p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
|
||||||
|
// }
|
||||||
|
// else{
|
||||||
|
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||||
|
// p.state.hPa += numFloorChanged * distBaro.draw();
|
||||||
|
// else
|
||||||
|
// p.state.hPa = numFloorChanged * distBaro.draw();
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // sanity check
|
||||||
|
// if (p.state.heading != p.state.heading) {throw "detected NaN";}
|
||||||
|
// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
|
||||||
|
// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
|
||||||
|
// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
|
||||||
|
|
||||||
|
// // ensure p.state.z_nr IS discreet
|
||||||
|
// if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MYTRANSITION_H
|
||||||
83
code/particles/MyTransitionSimple.h
Executable file
83
code/particles/MyTransitionSimple.h
Executable file
@@ -0,0 +1,83 @@
|
|||||||
|
#ifndef MYTRANSITIONSIMPLE_H
|
||||||
|
#define MYTRANSITIONSIMPLE_H
|
||||||
|
|
||||||
|
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||||
|
#include <KLib/math/distribution/Normal.h>
|
||||||
|
#include "MyState.h"
|
||||||
|
#include "MyControl.h"
|
||||||
|
|
||||||
|
class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/** a simple normal distribution */
|
||||||
|
K::NormalDistribution distX;
|
||||||
|
K::NormalDistribution distY;
|
||||||
|
K::NormalDistribution distZ;
|
||||||
|
K::NormalDistribution distBaro;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** ctor */
|
||||||
|
MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
|
||||||
|
distX.setSeed(1234);
|
||||||
|
distY.setSeed(1235);
|
||||||
|
distZ.setSeed(1236);
|
||||||
|
distBaro.setSeed(5678);
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
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 {
|
||||||
|
deltaMS = ts - this->ts;
|
||||||
|
this->ts = ts;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||||
|
|
||||||
|
for (K::Particle<MyState>& p : particles) {
|
||||||
|
|
||||||
|
p.state.heading_old = p.state.heading;
|
||||||
|
p.state.x_cm_old = p.state.x_cm;
|
||||||
|
p.state.y_cm_old = p.state.y_cm;
|
||||||
|
p.state.z_nr_old = p.state.z_nr;
|
||||||
|
|
||||||
|
p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
|
||||||
|
p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
|
||||||
|
p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
|
||||||
|
p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old, p.state.x_cm, p.state.y_cm);
|
||||||
|
|
||||||
|
// if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
|
||||||
|
// if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
|
||||||
|
// if (p.state.x_cm < 0) {p.state.x_cm = 0;}
|
||||||
|
// if (p.state.y_cm < 0) {p.state.y_cm = 0;}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//update barometer
|
||||||
|
p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
|
||||||
|
|
||||||
|
// update walked distance (2D)
|
||||||
|
const double dx = p.state.x_cm_old - p.state.x_cm;
|
||||||
|
const double dy = p.state.y_cm_old - p.state.y_cm;
|
||||||
|
p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif // MYTRANSITIONSIMPLE_H
|
||||||
31
code/particles/P3.h
Executable file
31
code/particles/P3.h
Executable file
@@ -0,0 +1,31 @@
|
|||||||
|
#ifndef P3_H
|
||||||
|
#define P3_H
|
||||||
|
|
||||||
|
struct P3 {
|
||||||
|
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
double z;
|
||||||
|
|
||||||
|
P3() : x(0), y(0), z(0) {;}
|
||||||
|
P3(const double x, const double y, const double z) : x(x), y(y), z(z) {;}
|
||||||
|
|
||||||
|
P3 operator - (const P3& o) const {
|
||||||
|
return P3(x-o.x, y-o.y, z-o.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
P3 operator + (const P3& o) const {
|
||||||
|
return P3(x+o.x, y+o.y, z+o.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
P3 operator * (const double v) const {
|
||||||
|
return P3(x*v, y*v, z*v);
|
||||||
|
}
|
||||||
|
|
||||||
|
double getLength(const double floorHeight_cm) const {
|
||||||
|
return std::sqrt(x*x + y*y + z*floorHeight_cm*z*floorHeight_cm);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // P3_H
|
||||||
377
code/plan.svg
377
code/plan.svg
@@ -42,23 +42,23 @@
|
|||||||
guidetolerance="10"
|
guidetolerance="10"
|
||||||
inkscape:pageopacity="0"
|
inkscape:pageopacity="0"
|
||||||
inkscape:pageshadow="2"
|
inkscape:pageshadow="2"
|
||||||
inkscape:window-width="1366"
|
inkscape:window-width="1600"
|
||||||
inkscape:window-height="672"
|
inkscape:window-height="782"
|
||||||
id="namedview3234"
|
id="namedview3234"
|
||||||
showgrid="true"
|
showgrid="true"
|
||||||
inkscape:zoom="2.4027176"
|
inkscape:zoom="2.4027176"
|
||||||
inkscape:cx="422.22336"
|
inkscape:cx="555.925"
|
||||||
inkscape:cy="1231.7432"
|
inkscape:cy="388.39069"
|
||||||
inkscape:window-x="0"
|
inkscape:window-x="0"
|
||||||
inkscape:window-y="33"
|
inkscape:window-y="0"
|
||||||
inkscape:window-maximized="1"
|
inkscape:window-maximized="1"
|
||||||
inkscape:current-layer="layer11"
|
inkscape:current-layer="layer16"
|
||||||
inkscape:object-nodes="true"
|
inkscape:object-nodes="true"
|
||||||
units="px"
|
units="px"
|
||||||
showborder="true"
|
showborder="true"
|
||||||
inkscape:snap-to-guides="true"
|
inkscape:snap-to-guides="true"
|
||||||
inkscape:snap-text-baseline="true"
|
inkscape:snap-text-baseline="true"
|
||||||
showguides="true"
|
showguides="false"
|
||||||
inkscape:guide-bbox="true"
|
inkscape:guide-bbox="true"
|
||||||
inkscape:snap-nodes="true"
|
inkscape:snap-nodes="true"
|
||||||
inkscape:snap-smooth-nodes="true">
|
inkscape:snap-smooth-nodes="true">
|
||||||
@@ -238,7 +238,7 @@
|
|||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
id="layer2"
|
id="layer2"
|
||||||
inkscape:label="roomnames_2"
|
inkscape:label="roomnames_2"
|
||||||
style="display:inline">
|
style="display:none">
|
||||||
<text
|
<text
|
||||||
xml:space="preserve"
|
xml:space="preserve"
|
||||||
style="font-style:normal;font-weight:normal;font-size:16px;line-height:125%;font-family:Times;-inkscape-font-specification:Times;text-align:center;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none"
|
style="font-style:normal;font-weight:normal;font-size:16px;line-height:125%;font-family:Times;-inkscape-font-specification:Times;text-align:center;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none"
|
||||||
@@ -1680,7 +1680,7 @@
|
|||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
id="layer16"
|
id="layer16"
|
||||||
inkscape:label="floor_0"
|
inkscape:label="floor_0"
|
||||||
style="display:none">
|
style="display:inline">
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 386.22047,67.322799 0,42.519681"
|
d="m 386.22047,67.322799 0,42.519681"
|
||||||
@@ -1840,12 +1840,6 @@
|
|||||||
d="m 623.62205,279.92122 0,-212.598421"
|
d="m 623.62205,279.92122 0,-212.598421"
|
||||||
id="path4544"
|
id="path4544"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 538.58268,198.42516 0,81.49606 m 85.03937,-81.49606 -173.62205,0 0,81.49606"
|
|
||||||
id="path4546"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="cccc" />
|
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 577.55906,279.92122 -49.6063,0"
|
d="m 577.55906,279.92122 -49.6063,0"
|
||||||
@@ -1907,9 +1901,10 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 655.51181,1853.1496 0,-375.5906"
|
d="m 655.51181,1853.1496 0,-432.2835"
|
||||||
id="path4570"
|
id="path4570"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 655.51181,1420.8661 0,-683.85826"
|
d="m 655.51181,1420.8661 0,-683.85826"
|
||||||
@@ -1949,9 +1944,10 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 584.64567,1477.559 0,106.2992"
|
d="m 595.27559,1477.559 0,106.2992"
|
||||||
id="path4586"
|
id="path4586"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 446.45669,1350 0,-35.4331"
|
d="m 446.45669,1350 0,-35.4331"
|
||||||
@@ -2198,14 +2194,16 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 368.50394,992.12595 0,800.78745"
|
d="m 368.50394,992.12595 0,797.24405"
|
||||||
id="path4684"
|
id="path4684"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 368.50394,1828.3464 0,24.8032"
|
d="m 368.50394,1831.8897 0,21.2599"
|
||||||
id="path4686"
|
id="path4686"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 446.45669,1332.2834 209.05512,0"
|
d="m 446.45669,1332.2834 209.05512,0"
|
||||||
@@ -2221,11 +2219,6 @@
|
|||||||
d="m 99.212598,403.93697 -49.606299,0"
|
d="m 99.212598,403.93697 -49.606299,0"
|
||||||
id="path4692"
|
id="path4692"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 446.45669,67.322799 0,67.322831"
|
|
||||||
id="path4694"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 549.2126,134.64563 -102.75591,0"
|
d="m 549.2126,134.64563 -102.75591,0"
|
||||||
@@ -2233,14 +2226,63 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 2402.3622,77.95272 0,56.69291 -92.126,0"
|
d="m 2402.3622,134.64563 -92.126,0"
|
||||||
id="path4698"
|
id="path4698"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 49.606299,1683.0708 318.897641,0"
|
d="m 49.606299,1683.0708 318.897641,0"
|
||||||
id="path4404"
|
id="path4404"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2785.0394,386.22044 0,-106.29922"
|
||||||
|
id="path5825"
|
||||||
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,198.42516 0,-131.102361"
|
||||||
|
id="path5829"
|
||||||
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
|
<path
|
||||||
|
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 450,279.9212 0,-81.49606"
|
||||||
|
id="path5851"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 450,198.42514 173.62205,0"
|
||||||
|
id="path5853"
|
||||||
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 538.58268,279.92122 0,-81.49606"
|
||||||
|
id="path5873"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,198.42516 0,-63.77953"
|
||||||
|
id="path5875"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,77.95272 0,56.69291"
|
||||||
|
id="path5877"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 531.49606,1491.7322 63.77953,0"
|
||||||
|
id="path5899"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 595.27559,1491.7322 60.23622,0"
|
||||||
|
id="path5901"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
@@ -2677,7 +2719,7 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 584.64567,1477.559 0,106.2992"
|
d="m 595.27559,1477.559 0,106.2992"
|
||||||
id="path12844"
|
id="path12844"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
@@ -2867,21 +2909,6 @@
|
|||||||
d="m 2462.5984,460.62989 0,276.37795"
|
d="m 2462.5984,460.62989 0,276.37795"
|
||||||
id="path12918"
|
id="path12918"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2398.8189,134.64563 0,-67.322831"
|
|
||||||
id="path13024"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 446.45669,134.64563 0,-67.322831"
|
|
||||||
id="path13061"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 584.64567,1477.559 70.86614,0"
|
|
||||||
id="path13065"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
<path
|
||||||
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 442.91339,336.61414 0,67.32283 389.76378,0 0,-67.32283 -389.76378,0"
|
d="m 442.91339,336.61414 0,67.32283 389.76378,0 0,-67.32283 -389.76378,0"
|
||||||
@@ -2893,6 +2920,36 @@
|
|||||||
d="m 2207.4803,279.92122 17.7166,0"
|
d="m 2207.4803,279.92122 17.7166,0"
|
||||||
id="path3498"
|
id="path3498"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,198.42516 0,-63.77953"
|
||||||
|
id="path5831"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,134.64563 0,-67.322831"
|
||||||
|
id="path5845"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,198.42516 0,-63.77953"
|
||||||
|
id="path5879"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,67.322799 0,67.322831"
|
||||||
|
id="path5881"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 531.49606,1491.7322 63.77953,0"
|
||||||
|
id="path5903"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 595.27559,1491.7322 60.23622,0"
|
||||||
|
id="path5905"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
@@ -2914,13 +2971,13 @@
|
|||||||
sodipodi:nodetypes="ccc" />
|
sodipodi:nodetypes="ccc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 368.50394,1619.2913 0,-42.5197"
|
d="m 368.50394,1619.2913 0,-38.9764"
|
||||||
id="path11569"
|
id="path11569"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 368.50394,1548.4252 0,-467.7166"
|
d="m 368.50394,1544.8818 0,-460.6299"
|
||||||
id="path11571"
|
id="path11571"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
@@ -3055,7 +3112,7 @@
|
|||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 584.64567,1477.559 0,113.3858"
|
d="m 595.27559,1477.559 0,113.3858"
|
||||||
id="path11625"
|
id="path11625"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
@@ -3545,24 +3602,6 @@
|
|||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
transform="translate(0,-4.2364502e-5)"
|
transform="translate(0,-4.2364502e-5)"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2398.8189,134.64563 0,-67.322831"
|
|
||||||
id="path13022"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 446.45669,134.64563 0,-67.322831"
|
|
||||||
id="path13059"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 584.64567,1477.559 70.86614,0"
|
|
||||||
id="path13063"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 1729.1339,336.6141 -198.4252,0 0,67.32283 198.4252,0"
|
d="m 1729.1339,336.6141 -198.4252,0 0,67.32283 198.4252,0"
|
||||||
@@ -3609,12 +3648,42 @@
|
|||||||
id="path3487"
|
id="path3487"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
transform="translate(0,-4.2364502e-5)" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,198.42512 0,-63.77953"
|
||||||
|
id="path5833"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,134.64559 0,-67.322833"
|
||||||
|
id="path5847"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,198.42512 0,-63.77953"
|
||||||
|
id="path5883"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,67.322757 0,67.322833"
|
||||||
|
id="path5885"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 531.49606,1491.7322 63.77953,0"
|
||||||
|
id="path5907"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 595.27559,1491.7322 60.23622,0"
|
||||||
|
id="path5909"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
id="layer8"
|
id="layer8"
|
||||||
inkscape:label="floor_3"
|
inkscape:label="floor_3"
|
||||||
style="display:inline"
|
style="display:none"
|
||||||
transform="translate(0,4.2364502e-5)">
|
transform="translate(0,4.2364502e-5)">
|
||||||
<path
|
<path
|
||||||
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
@@ -4335,7 +4404,7 @@
|
|||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
<path
|
||||||
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 584.64567,1477.559 0,113.3858"
|
d="m 595.27559,1477.559 0,113.3858"
|
||||||
id="path3459-9"
|
id="path3459-9"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
@@ -4352,30 +4421,42 @@
|
|||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
transform="translate(0,-4.2364502e-5)"
|
transform="translate(0,-4.2364502e-5)"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="cc" />
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2398.8189,134.64563 0,-67.322831"
|
|
||||||
id="path13020"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 584.64567,1477.559 70.86614,0"
|
|
||||||
id="path13055"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)"
|
|
||||||
sodipodi:nodetypes="cc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 446.45669,134.64563 0,-67.322831"
|
|
||||||
id="path13057"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)" />
|
|
||||||
<path
|
<path
|
||||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
d="m 506.69291,460.62985 -67.32283,0"
|
d="m 506.69291,460.62985 -67.32283,0"
|
||||||
id="path4163"
|
id="path4163"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,198.42512 0,-63.77953"
|
||||||
|
id="path5835"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 467.71654,134.64559 0,-67.322833"
|
||||||
|
id="path5849"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,198.42512 0,-63.77953"
|
||||||
|
id="path5887"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 2381.1024,67.322757 0,67.322833"
|
||||||
|
id="path5889"
|
||||||
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="cc" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 531.49606,1491.7322 63.77953,0"
|
||||||
|
id="path5911"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||||
|
d="m 595.27559,1491.7322 60.23622,0"
|
||||||
|
id="path5913"
|
||||||
|
inkscape:connector-curvature="0" />
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
@@ -4383,73 +4464,34 @@
|
|||||||
inkscape:label="staircase_0_1"
|
inkscape:label="staircase_0_1"
|
||||||
style="display:none">
|
style="display:none">
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
d="m 535.03937,1477.559 0,177.1654 116.92913,0 0,-177.1654 -109.84252,0"
|
d="m 2664.5669,655.51178 102.7559,0 0,-155.90552"
|
||||||
id="path4702"
|
id="path4988"
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 439.37008,570.47241 0,56.69291 63.77953,0 0,-56.69291 -56.69292,0"
|
|
||||||
id="path4704"
|
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="ccccc" />
|
sodipodi:nodetypes="ccc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
d="m 457.08661,74.409413 0,120.472437 162.99213,0 0,-124.015744 -162.99213,0"
|
d="m 503.14961,457.08658 -46.063,0 0,276.37795"
|
||||||
id="path4706"
|
id="path4992"
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0"
|
||||||
<path
|
sodipodi:nodetypes="ccc" />
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2228.7402,77.95272 0,116.92913 170.0787,0 0,-124.015744 -170.0787,0"
|
|
||||||
id="path4708"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2661.0236,655.51178 0,-159.44882 109.8425,0 0,159.44882 -106.2992,0"
|
|
||||||
id="path4710"
|
|
||||||
inkscape:connector-curvature="0" />
|
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
id="layer11"
|
id="layer11"
|
||||||
inkscape:label="staircase_1_2"
|
inkscape:label="staircase_1_2"
|
||||||
style="display:inline">
|
style="display:none">
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
d="m 457.08661,194.88185 162.99213,0"
|
d="m 2125.9843,343.70075 0,53.14961 -226.7717,0"
|
||||||
id="path12930"
|
id="path4998"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0"
|
||||||
sodipodi:nodetypes="cc" />
|
sodipodi:nodetypes="ccc" />
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
d="m 620.07874,194.88185 0,-124.015744 -162.99213,0 0,124.015744"
|
d="m 503.14961,460.62989 -56.69292,0 0,276.37795"
|
||||||
id="path12932"
|
id="path5000"
|
||||||
inkscape:connector-curvature="0"
|
inkscape:connector-curvature="0" />
|
||||||
sodipodi:nodetypes="cccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2125.9843,333.07083 -226.7717,0 0,77.95276 233.8583,0 0,-63.77953"
|
|
||||||
id="path12934"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2398.8189,70.866106 -170.0787,0 0,124.015744 170.0787,0 0,-124.015744"
|
|
||||||
id="path12936"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 535.03937,1477.559 116.92913,0 0,177.1654 -116.92913,0 0,-177.1654"
|
|
||||||
id="path12959"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 446.45669,566.9291 56.69292,0 0,74.40945 -63.77953,0 0,-67.32284"
|
|
||||||
id="path4157"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
@@ -4458,43 +4500,16 @@
|
|||||||
style="display:none"
|
style="display:none"
|
||||||
transform="translate(0,4.2364502e-5)">
|
transform="translate(0,4.2364502e-5)">
|
||||||
<path
|
<path
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
d="m 524.40945,1477.559 0,170.0788 127.55905,0 0,-170.0788"
|
d="m 503.14961,460.62985 -56.69292,0 0,276.37795"
|
||||||
id="path11647"
|
id="path5802"
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="cccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 457.08661,70.866106 0,124.015744 162.99213,0 0,-124.015744 -162.99213,0"
|
|
||||||
id="path13002"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2228.7402,70.866106 0,124.015744 170.0787,0 0,-124.015744 -170.0787,0"
|
|
||||||
id="path13006"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 2331.4961,279.92122 0,-77.95275 63.7795,0 0,77.95275 -63.7795,0"
|
|
||||||
id="path13008"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
transform="translate(0,-4.2364502e-5)"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 1697.2441,283.46449 0,173.62205 17.7165,0 0,-173.62205 -14.1732,0"
|
|
||||||
id="path4161"
|
|
||||||
inkscape:connector-curvature="0"
|
|
||||||
sodipodi:nodetypes="ccccc" />
|
|
||||||
<path
|
|
||||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
|
||||||
d="m 503.14961,641.33851 0,-67.32284 -56.69292,0 0,67.32284 53.14961,0"
|
|
||||||
id="path4165"
|
|
||||||
inkscape:connector-curvature="0" />
|
inkscape:connector-curvature="0" />
|
||||||
|
<path
|
||||||
|
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||||
|
d="m 1729.1339,340.1574 0,60.23622 -194.8819,0"
|
||||||
|
id="path5804"
|
||||||
|
inkscape:connector-curvature="0"
|
||||||
|
sodipodi:nodetypes="ccc" />
|
||||||
</g>
|
</g>
|
||||||
<g
|
<g
|
||||||
inkscape:groupmode="layer"
|
inkscape:groupmode="layer"
|
||||||
|
|||||||
|
Before Width: | Height: | Size: 223 KiB After Width: | Height: | Size: 224 KiB |
@@ -2,10 +2,10 @@
|
|||||||
|
|
||||||
#include "../particles/MyState.h"
|
#include "../particles/MyState.h"
|
||||||
#include "BarometerObservation.h"
|
#include "BarometerObservation.h"
|
||||||
#include "barometric.h"
|
//#include "barometric.h"
|
||||||
#include <KLib/math/distribution/Normal.h>
|
//#include <KLib/math/distribution/Normal.h>
|
||||||
|
|
||||||
double g_BarometerObservation = 0.0;
|
static constexpr double g_BarometerObservation = 0.0;
|
||||||
|
|
||||||
class BarometerEvaluation {
|
class BarometerEvaluation {
|
||||||
|
|
||||||
@@ -13,46 +13,50 @@ public:
|
|||||||
|
|
||||||
double getProbability(const MyState& state, const BarometerObservation* obs) const {
|
double getProbability(const MyState& state, const BarometerObservation* obs) const {
|
||||||
|
|
||||||
//rho_z
|
return 1;
|
||||||
double barometerSigma = 0.3;
|
|
||||||
|
|
||||||
//The height of the single floor levels.
|
// //rho_z
|
||||||
const static double floor_height[3] = {4.1, 3.4, 3.4};
|
// double barometerSigma = 0.3;
|
||||||
|
|
||||||
if(USE_BAROMETRIC_FORMULAR){
|
// //The height of the single floor levels.
|
||||||
//height the particle has climbed.
|
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||||
double h_1 = 0.0;
|
|
||||||
for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
|
|
||||||
h_1 += floor_height[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
if(h_1 != 0.0){
|
// if(USE_BAROMETRIC_FORMULAR){
|
||||||
// use the barometric formular to calculate the relative pressure
|
// //height the particle has climbed.
|
||||||
// the calculation is done assuming sea level height at every floor.
|
// double h_1 = 0.0;
|
||||||
double mslp = BarometricFormular::s_getSeaLevelPressure();
|
// for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
|
||||||
double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
// h_1 += floor_height[i];
|
||||||
barometerSigma = std::abs(mslp - pressure);
|
// }
|
||||||
}
|
|
||||||
|
// if(h_1 != 0.0){
|
||||||
|
// // use the barometric formular to calculate the relative pressure
|
||||||
|
// // the calculation is done assuming sea level height at every floor.
|
||||||
|
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||||
|
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||||
|
// barometerSigma = std::abs(mslp - pressure);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// // constant value for sigma if we assume all floors are same in height
|
||||||
|
// barometerSigma = 0.30 / 1.0; //hPa
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // evaluate the current particle with a normal distribution
|
||||||
|
// const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
|
||||||
|
|
||||||
|
// //Just for the visualization. i'm a lazy bastard
|
||||||
|
// g_BarometerObservation = obs->hpa;
|
||||||
|
|
||||||
|
// assert(barometerProbability == barometerProbability);
|
||||||
|
// assert(state.hPa == state.hPa);
|
||||||
|
// assert(obs->hpa == obs->hpa);
|
||||||
|
|
||||||
|
// //std::cout << barometerProbability << std::endl;
|
||||||
|
|
||||||
|
// return pow(2.0, barometerProbability);
|
||||||
|
// //return barometerProbability;
|
||||||
|
|
||||||
}
|
}
|
||||||
else {
|
|
||||||
// constant value for sigma if we assume all floors are same in height
|
|
||||||
barometerSigma = 0.30 / 1.0; //hPa
|
|
||||||
}
|
|
||||||
|
|
||||||
// evaluate the current particle with a normal distribution
|
|
||||||
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
|
|
||||||
|
|
||||||
//Just for the visualization. i'm a lazy bastard
|
|
||||||
g_BarometerObservation = obs->hpa;
|
|
||||||
|
|
||||||
assert(barometerProbability == barometerProbability);
|
|
||||||
assert(state.hPa == state.hPa);
|
|
||||||
assert(obs->hpa == obs->hpa);
|
|
||||||
|
|
||||||
//std::cout << barometerProbability << std::endl;
|
|
||||||
|
|
||||||
return pow(2.0, barometerProbability);
|
|
||||||
//return barometerProbability;
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
#include "circular.h"
|
#include "circular.h"
|
||||||
#include "BarometerObservation.h"
|
#include "BarometerObservation.h"
|
||||||
#include "../SensorReader.h"
|
#include "../reader/SensorReader.h"
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
|
|
||||||
//circular_buffer<double> measurementHistory(1000);
|
//circular_buffer<double> measurementHistory(1000);
|
||||||
|
|||||||
Reference in New Issue
Block a user