updates the visualisation

removed obsolte parts
fixed baromter stuff
worked on eval
added ground-truth

code/Helper.h

@@ -9,6 +9,7 @@
 
 #include "Settings.h"
 #include "MyGridNode.h"
+#include "OldGroundTruth.h"
 
 class Helper {
 
@@ -62,6 +63,9 @@ public:
 		const LengthF h2 = LengthF::cm(align(getHeight(2)));
 		const LengthF h3 = LengthF::cm(align(getHeight(3)));
 
+		// all ground-truth points
+		std::unordered_map<int, Point3> gtwp;
+
 		FHWSFloors() {;}
 
 	};
@@ -81,6 +85,16 @@ public:
 		f.s12 = fpFac.getStairs("staircase_1_2");
 		f.s23 = fpFac.getStairs("staircase_2_3");
 
+		OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222);
+		OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222);
+		OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222);
+		OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222);
+
+		for (auto it : gtwp0.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
+		for (auto it : gtwp1.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
+		for (auto it : gtwp2.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
+		for (auto it : gtwp3.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}
+
 		return f;
 
 	}

code/OldGroundTruth.h

@@ -0,0 +1,138 @@
+#ifndef OLDGROUNDTRUTH_H
+#define OLDGROUNDTRUTH_H
+
+#include <KLib/geo/Point.h>
+#include <unordered_map>
+#include <KLib/gfx/svg/SVGLoader.h>
+#include <Indoor/geo/Point2.h>
+
+/**
+ * TODO: REMOVE
+ */
+class OldGroundTruth {
+
+	/**
+	 * helper class for SVG floorplans.
+	 *
+	 * converts between the SVG's scale and real-world scale
+	 */
+	class SVGScaler {
+
+	private:
+
+		/** the scaling factor to apply to the svg data */
+		double scalingFactor;
+
+	public:
+
+		/** ctor */
+		SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) {
+			;
+		}
+
+		/** scale (x, y) into (_x, _y) */
+		void scale(const double x, const double y, float& _x, float& _y) const {
+			_x = x * scalingFactor;
+			_y = y * scalingFactor;
+		}
+
+		/** scale the given point into a new output point */
+		Point2 scale(const K::Point p) const {
+			Point2 ret;
+			scale (p.x, p.y, ret.x, ret.y);
+			return ret;
+		}
+
+		/** scale the given line into a new output line */
+		Line2 scale(const K::Line l) const {
+			Line2 ret;
+			scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y);
+			scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y);
+			return ret;
+		}
+
+	};
+
+private:
+
+	/** helper to scale the SVG into real-world-scale */
+	SVGScaler scaler;
+
+	/** all ground-truth waypoints within the floorplan */
+	std::unordered_map<int, Point2> points;
+
+public:
+
+	OldGroundTruth() :scaler(0) {;}
+
+	/**
+	 * ctor
+	 * @param file the svg's filename
+	 * @param layerName the name of the layer (within the SVG) to load
+	 * @param scalingFactor the scaling to apply to convert between SVG and real-world scale
+	 */
+	OldGroundTruth(const std::string& file, const std::string& layerName, const double scalingFactor) : scaler(scalingFactor) {
+
+		K::SVGFile svg;
+		K::SVGLoader::load(K::File(file), &svg);
+		K::SVGComposite* sc = svg.getLayers();
+
+		K::SVGLayer* layer = sc->getContainedLayerNamed(layerName);
+		if (!layer) {throw "svg has no layer named '" + layerName + "'";}
+		load(layer);
+
+	}
+
+	/** get all waypoints */
+	const std::unordered_map<int, Point2>& getWaypoints() const {
+		return points;
+	}
+
+
+private:
+
+
+
+	/** recursive loading/parsing of nested SVG elements */
+	void load(K::SVGElement* el) {
+
+		switch (el->getType()) {
+
+			case SVGElementType::COMPOSITE: {
+				for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
+					load(sub);
+				}
+				break;
+			}
+
+			case SVGElementType::LAYER: {
+				K::SVGLayer* layer = (K::SVGLayer*) el;
+				for (K::SVGElement* sub : layer->getChilds()) {
+					load(sub);
+				}
+				break;
+			}
+
+			case SVGElementType::TEXT: {
+				const K::SVGText* text = (K::SVGText*) el;
+				if (text->getText().empty()) {break;}
+				const int id = std::stoi(text->getText());
+				points[id] = scaler.scale(K::Point(text->getPosition().x, text->getPosition().y));
+				break;
+			}
+
+			case SVGElementType::PATH: {
+				break;
+			}
+
+			default:
+				throw "should not happen!";
+
+		}
+
+	}
+
+
+};
+
+#endif // OLDGROUNDTRUTH_H

code/Settings.h

@@ -16,7 +16,7 @@ namespace MiscSettings {
 
 	const int timeSteps = 500;
 
-	const int numParticles = 1000;
+	const int numParticles = 2500;
 
 }
 

code/Vis.h

@@ -10,6 +10,8 @@
 #include <Indoor/geo/Length.h>
 #include <Indoor/floorplan/Floor.h>
 
+#include "eval/GroundTruthWay.h"
+
 class Vis {
 
 public:
@@ -20,6 +22,8 @@ public:
 	K::GnuplotSplotElementColorPoints gridNodes;
 	K::GnuplotSplotElementLines gridEdges;
 	K::GnuplotSplotElementPoints particles;
+	K::GnuplotSplotElementLines groundTruth;
+	K::GnuplotSplotElementLines estPath;
 
 public:
 
@@ -35,11 +39,18 @@ public:
 		gp << "unset ztics\n";
 		gp << "unset border\n";
 
+		groundTruth.setLineWidth(2);
+		groundTruth.setColorHex("#666666");
+
+		estPath.setLineWidth(2);
+
 		// attach all layers
 		splot.add(&floors);
 		splot.add(&gridNodes);
 		splot.add(&gridEdges);
 		splot.add(&particles);
+		splot.add(&groundTruth);
+		splot.add(&estPath);
 
 	}
 
@@ -81,6 +92,27 @@ public:
 		return *this;
 	}
 
+	void addGroundTruth(GroundTruthWay& gtw) {
+		groundTruth.clear();
+		for (auto it : gtw.getWay()) {
+			K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
+			groundTruth.add(gp);
+		}
+	}
+
+	void addEstPath(std::vector<Point3>& est) {
+		estPath.clear();;
+		for (const Point3& p : est) {
+			K::GnuplotPoint3 gp(p.x, p.y, p.z);
+			estPath.add(gp);
+		}
+	}
+
+	void setTimestamp(uint64_t ts) {
+		static uint64_t firstTs = ts;
+		gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";
+	}
+
 	void removeGrid() {
 		gridNodes.clear();;
 	}
@@ -89,6 +121,20 @@ public:
 		particles.clear();
 	}
 
+	void addObject(const int idx, const Point3& p) {
+		gp << "set object " << idx << " polygon ";
+		gp << "from " << p.x << "," << p.y << "," << p.z;
+		gp << "  to " << p.x << "," << p.y << "," << p.z + 200;
+		gp << "  to " << p.x << "," << p.y << "," << p.z;				// close
+		gp << " lw 2 ";
+		gp << "\n";
+	}
+
+	void setEstAndShould(const Point3& est, const Point3& should) {
+		addObject(2,est);
+		addObject(3,should);
+	}
+
 	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));
 	}

code/eval/Eval.h

@@ -9,6 +9,8 @@
 #include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
 #include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
 #include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
 
 class Eval : public EvalBase {
 
@@ -26,15 +28,16 @@ public:
 
 		//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
 		//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
-		//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
+		GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
-		GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<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);
+
+		gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
 
 	}
 
@@ -47,7 +50,7 @@ public:
 
 		// 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);
+		eval.get()->setUsage(true, false, false, true, true);
 		pf->setEvaluation( std::move(eval) );
 
 		// resampling step?
@@ -56,6 +59,7 @@ public:
 
 		// state estimation step
 		pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
+		//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
 
 	}
 

code/eval/EvalBase.h

@@ -10,7 +10,6 @@
 
 #include "GroundTruthWay.h"
 
-#include "../particles/P3.h"
 #include "../particles/MyState.h"
 #include "../particles/MyObservation.h"
 #include "../particles/MyEvaluation.h"
@@ -43,10 +42,15 @@ protected:
 	SensorReaderTurn* srt;
 	SensorReaderStep* srs;
 
-	GroundTruthWay gtw;
+
 
 	std::string runName;
 
+	GroundTruthWay gtw;
+	std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
+	std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
+	std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
+
 public:
 
 	EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
@@ -62,11 +66,39 @@ public:
 
 	}
 
+	GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
+
+		// construct the ground-truth-path by using all contained waypoint ids
+		std::vector<Point3> path;
+		for (int id : ids) {
+			auto it = waypoints.find(id);
+			assert(it != waypoints.end());
+			path.push_back(it->second);
+		}
+
+		// new created the timed path
+		GroundTruthWay gtw;
+		int i = 0;
+		while (sr.hasNext()) {
+			const SensorEntry se = sr.getNext();
+			if (se.data.empty()) {continue;}		// why necessary??
+			if (se.idx == 99) {
+				gtw.add(se.ts, path[i]);
+				++i;
+			}
+		}
+
+		// ensure the sensor-data contained usable timestamps for the ground-truth mapping
+		assert(i>0);
+
+		sr.rewind();
+		return gtw;
+
+	}
+
 	void run() {
 
-			// read CSV input
+			// sensor numbers
-			// 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;
 
@@ -111,10 +143,10 @@ public:
 			std::vector<Point3> pathEst;
 
 			uint64_t lastTransitionTS = 0;
-			bool firstReading = true;
 			int64_t start_time = -1;
 
 			K::Statistics<double> stats;
+			int cnt = 0;
 
 			// process each sensor reading
 			while(sr->hasNext()) {
@@ -127,12 +159,6 @@ public:
 				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: {
@@ -203,37 +229,33 @@ public:
 					}
 
 
-					// let the transition know the current timestamp to determine the time since the last transition
+					// timed updates
-					//if (!useSimpleTrans) {
+					((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
-						((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);
+					const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
+					const Point3 diff = curEst - curGT;
 
-//					// TODO
+					// skip the first 8 scans due to uniform distribution start
-//					const Point3 diff = curEst - curGT;
+					if (++cnt > 8) {
-
+					pathEst.push_back(curEst);
-//					//if (std::abs(diff.z) < 0.1) {
+					const float err = diff.length();
-//						const float err = diff.length();
+					stats.add(err);
-//						std::cout << err << std::endl;
+					std::cout << stats.asString() << std::endl;
-//						stats.add(err);
+					}
-//						std::cout << stats.asString() << std::endl;
-//					//}
 
+					// plot
 					vis.clearStates();
 					for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
-
+					vis.setTimestamp(se.ts);
+					vis.addGroundTruth(gtw);
+					vis.addEstPath(pathEst);
+					vis.setEstAndShould(curEst, curGT);
 					vis.show();;
 
 				}

code/eval/GroundTruthWay.h

@@ -16,7 +16,7 @@ public:
 	}
 
 	/** get the ground truth way */
-	const std::vector<Entry>& getWay() const {return entries;}
+	const std::vector<InterpolatorEntry>& getWay() const {return entries;}
 
 };
 

code/frank/BeaconEvaluation.h

@@ -23,7 +23,7 @@ public:
 		double prob = 1.0;
 
 //		const double tx = -74;
-		const double waf = 20.0;
+		const double waf = 8.0;
 
 //		// get the ap the client had the strongest measurement for
 //		const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
@@ -40,7 +40,7 @@ public:
 
 			// distance (in meter) between particle and AP
 			//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;
+			const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0;
 
 			// floor difference?
 			//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());

code/frank/PositionedBeacon.h

@@ -21,7 +21,7 @@ public:
 
 	/** ctor */
 	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), Point3(x_cm, y_cm, z_cm) {
+		Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) {
 		;
 	}
 

code/frank/Settings.h

@@ -20,27 +20,27 @@ public:
 
 	Settings() {
 
-		const double pl = 2.7;
+		const double pl = 2.7;		// 2.7
 		const double tx = -46;
 
-		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300,  Helper::getHeight(3), tx, pl);
-		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930,  Helper::getHeight(3), tx, pl);
-		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.5);
+		addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1);
 		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, Helper::getHeight(3), tx, pl);
 
 		// 2nd floor (vague AP position)
-		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, Helper::getHeight(2), tx, pl-0.5);
+		addAP(("00:04:96:6c:3a:a9"), "I.2.1",  6750, 3350, Helper::getHeight(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, Helper::getHeight(2), tx, pl);
-		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, Helper::getHeight(2), tx, pl-0.5);
+		addAP(("00:04:96:6b:0c:c9"), "I.2.19",  300, 4000, Helper::getHeight(2), tx, pl);
-		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5);
+		addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl);
 
 		// 1st floor (vague AP position)
 		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, Helper::getHeight(1), tx, pl);
-		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, Helper::getHeight(1), tx, pl-0.25);
+		addAP(("00:04:96:69:48:c9"), "I.1.17", 1500,  300, Helper::getHeight(1), tx, pl);
-		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, Helper::getHeight(1), tx, pl-0.25);
+		addAP(("00:04:96:77:eb:99"), "I.1.21",  500, 1700, Helper::getHeight(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, Helper::getHeight(1), tx, pl);
 
@@ -63,9 +63,9 @@ public:
 		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, Helper::getHeight(2));		// idis toni
 
-		addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(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, Helper::getHeight(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, Helper::getHeight(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("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3);

code/frank/WiFiEvaluation.h

@@ -38,10 +38,9 @@ public:
 		//const double tx = -48;		// tablet
 		//const double pl = 3.15;
 		const float waf = 7;//10.0;		// was 7 before?! has something todo with the floor heights / levels
-	//	const int floor_height_cm = 350;
 
 		// 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 strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
 		//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);
@@ -53,13 +52,13 @@ public:
 			const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);
 
 			// distance (in meter) between particle and AP
-			//const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0;
+			const float distToAP_m = state.pCur.getDistance(*ap) / 100.0;
-			const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0;
 
 			// floor difference?
-			const float floorDiff = std::round(
+			const float floorDiff = //std::ceil(
-						std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z))
+						std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
-		);
+			//);
+			//const float floorDiff = std::abs(ap->z - state.pCur.z) / 340;
 
 			// estimate the rssi depending on above distance
 			const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);

code/particles/MyEvaluation.h

@@ -53,13 +53,13 @@ public:
 				weight *= wifiEval.getProbability(p.state, observation);
 			}
 
-//			if (useBaro && observation.barometer) {
+			if (useBaro && observation.barometer) {
-//				weight *= barometerEval.getProbability(p.state, observation.barometer);
+				weight *= barometerEval.getProbability(p.state, observation.barometer);
-//            }
+			}
 
-//			if (useIB) {
+			if (useIB) {
-//				weight *= beaconEval.getProbability(p.state, observation);
+				weight *= beaconEval.getProbability(p.state, observation);
-//			}
+			}
 
 //			if (useStep) {
 //				weight *= stepEval.getProbability(p.state, observation.step);

code/particles/MyState.h

@@ -65,14 +65,14 @@ struct MyState {
 
 	MyState& operator += (const MyState& o) {
 		pCur += o.pCur;
-		//hPa += o.hPa;
+		hPa += o.hPa;
 		//distanceWalked += o.distanceWalked;
 		return *this;
 	}
 
 	MyState& operator /= (const double d) {
 		pCur /= d;
-		//hPa /= d;
+		hPa /= d;
 		//distanceWalked /= d;
 		return *this;
 	}
@@ -80,7 +80,7 @@ struct MyState {
 	MyState operator * (const double d) const {
 		MyState s = MyState(*this);
 		s.pCur *= d;
-		//s.hPa *= d;
+		s.hPa *= d;
 		//distanceWalked *= d;
 		return s;
 	}
@@ -101,6 +101,10 @@ struct MyState {
 //		return *this;
 //	}
 
+	bool belongsToRegion(const MyState& o) const {
+		return o.pCur.getDistance(pCur) < 700;
+	}
+
 //	/** rejection for the regional estimator. reject after 150cm distance */
 //	bool belongsToRegion(const MyState& o) const {
 

code/particles/MyTransition.h

@@ -48,7 +48,7 @@ public:
 	 */
 	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(0.0, 1.0), distWalk(1.5, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
 
 		distWalkStop.setSeed(1234);
 		distWalk.setSeed(1234);
@@ -101,20 +101,6 @@ public:
 			// 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 );
@@ -129,7 +115,7 @@ public:
 
 
             // --- ATTENTION HORRIBLE CODE INCOMING. ---
-
+			p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
 //            //how many floors are changed? and in what direction (given by the sign)
 //            double numFloorChanged = p.state.z_nr_old - p.state.z_nr;
 
@@ -167,7 +153,7 @@ public:
 //                if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
 //                    p.state.hPa += numFloorChanged * distBaro.draw();
 //                else
-//                    p.state.hPa = numFloorChanged * distBaro.draw();
+ //                   p.state.hPa = numFloorChanged * distBaro.draw();
 //            }
 
 //			// sanity check

code/particles/P3.h

@@ -1,31 +0,0 @@
-#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

code/plan.svg

@@ -43,16 +43,16 @@
      inkscape:pageopacity="0"
      inkscape:pageshadow="2"
      inkscape:window-width="1600"
-     inkscape:window-height="782"
+     inkscape:window-height="845"
      id="namedview3234"
      showgrid="true"
-     inkscape:zoom="2.4027176"
+     inkscape:zoom="0.84948895"
-     inkscape:cx="555.925"
+     inkscape:cx="272.89941"
-     inkscape:cy="388.39069"
+     inkscape:cy="1411.2257"
      inkscape:window-x="0"
      inkscape:window-y="0"
      inkscape:window-maximized="1"
-     inkscape:current-layer="layer16"
+     inkscape:current-layer="layer8"
      inkscape:object-nodes="true"
      units="px"
      showborder="true"
@@ -118,7 +118,7 @@
      inkscape:groupmode="layer"
      id="layer1"
      inkscape:label="roomnames_3"
-     style="display:none">
+     style="display:inline">
     <text
        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"
@@ -1680,7 +1680,7 @@
      inkscape:groupmode="layer"
      id="layer16"
      inkscape:label="floor_0"
-     style="display:inline">
+     style="display:none">
     <path
        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"
@@ -3683,7 +3683,7 @@
      inkscape:groupmode="layer"
      id="layer8"
      inkscape:label="floor_3"
-     style="display:none"
+     style="display:inline"
      transform="translate(0,4.2364502e-5)">
     <path
        style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -4386,7 +4386,7 @@
        sodipodi:nodetypes="cc" />
     <path
        style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
-       d="m 442.91339,336.61418 0,67.32283 389.76378,0 0,-67.32283 -389.76378,0"
+       d="m 442.91339,340.1574 0,60.23622 389.76378,0 0,-60.23622 -389.76378,0"
        id="path132-3"
        inkscape:connector-curvature="0"
        sodipodi:nodetypes="ccccc" />
@@ -4398,7 +4398,7 @@
        sodipodi:nodetypes="ccc" />
     <path
        style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
-       d="m 442.91339,457.08654 0,279.92129"
+       d="m 446.45669,460.62984 0,276.37796"
        id="path138-5"
        inkscape:connector-curvature="0"
        sodipodi:nodetypes="cc" />
@@ -4423,9 +4423,10 @@
        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 506.69291,460.62985 -67.32283,0"
+       d="m 506.69291,460.62985 -60.23622,-10e-6"
        id="path4163"
-       inkscape:connector-curvature="0" />
+       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.42512 0,-63.77953"

code/toni/BarometerEvaluation.h

@@ -13,10 +13,10 @@ public:
 
 	double getProbability(const MyState& state, const BarometerObservation* obs) const {
 
-		return 1;
+		//return 1;
 
 //        //rho_z
-//        double barometerSigma = 0.3;
+		double barometerSigma = 0.09;
 
 //        //The height of the single floor levels.
 //        const static double floor_height[3] = {4.1, 3.4, 3.4};
@@ -39,23 +39,23 @@ public:
 //        }
 //        else {
 //            // constant value for sigma if we assume all floors are same in height
-//            barometerSigma = 0.30 / 1.0; //hPa
+//			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);
+		const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa);
 
 //        //Just for the visualization. i'm a lazy bastard
-//        g_BarometerObservation = obs->hpa;
+		//g_BarometerObservation = obs->hpa;
 
-//        assert(barometerProbability == barometerProbability);
+		assert(barometerProbability == barometerProbability);
-//        assert(state.hPa == state.hPa);
+		assert(state.hPa == state.hPa);
-//        assert(obs->hpa == obs->hpa);
+		assert(obs->hpa == obs->hpa);
 
 //        //std::cout << barometerProbability << std::endl;
 
-//        return  pow(2.0, barometerProbability);
+		//return  pow(2.0, barometerProbability);
-//        //return  barometerProbability;
+		return  barometerProbability;
 
 	}
 

code/toni/BarometerSensorReader.h

@@ -5,25 +5,29 @@
 #include "../reader/SensorReader.h"
 #include <sstream>
 
+#include <Indoor/math/MovingAVG.h>
+
 //circular_buffer<double> measurementHistory(1000);
 
 
 class BarometerSensorReader{
 
 private:
-    circular_buffer<double> measurementHistory;
+//    circular_buffer<double> measurementHistory;
+
+	MovingAVG<float> avg;
 
 public:
 
-    BarometerSensorReader(){
+	BarometerSensorReader(): avg(3) {
-        if(!USE_STATIC_CIRCULAR_BUFFERING){
+//        if(!USE_STATIC_CIRCULAR_BUFFERING){
-            //8.33min
+//            //8.33min
-			measurementHistory.reserve(10000);
+//			measurementHistory.reserve(10000);
-        }
+//        }
-        else{
+//        else{
-            //30 * 500ms = 1,5s
+//            //30 * 500ms = 1,5s
-            measurementHistory.reserve(30);
+//            measurementHistory.reserve(30);
-        }
+//        }
     }
 
     BarometerObservation* readBarometer(const SensorEntry& se) {
@@ -31,52 +35,63 @@ public:
 		std::string tmp = se.data;
 		BarometerObservation* obs = new BarometerObservation();
 
-        //Read the hPa
+		// get the next hPa reading and average it
-        double hPa = stod(tmp);
+		avg.add(stod(tmp));
+		const double hPa = avg.get();
 
-        // load the measurement at current time into the history
+		// everything realtive to the first measurement
-        double currentMeasurement = hPa - measurementHistory[0];
+		static double first_hPa = 0;
+		if (avg.getNumUsed() < avg.getSize()) {first_hPa = avg.get();}
 
-        if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){
+		obs->hpa = hPa - first_hPa;
 
-            //smoothing with alpha value
+		std::cout << obs->hpa << std::endl;
-            if(measurementHistory.size() > 1){
-				double alpha = 0.1;
-                double lastMeasurement = measurementHistory[measurementHistory.size() - 1];
-                currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement);
-
-                obs->hpa = currentMeasurement;
-            }else{
-                obs->hpa = 0;
-            }
-
-            measurementHistory.push_back(currentMeasurement);
-        }
-        else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){
-
-            currentMeasurement = hPa;
-            measurementHistory.push_back(currentMeasurement);
-
-            // calculate the relative air pressure by getting the mean of the first and last three entrys of the history
-            // and subtract them.
-            if (measurementHistory.size() > 5){
-                double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0;
-                double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0;
-
-                obs->hpa = meanHead - meanTail;
-            }
-            else{
-                obs->hpa = 0;
-            }
-        }
-        else //no data smoothing
-        {
-            measurementHistory.push_back(currentMeasurement);
-            obs->hpa = currentMeasurement;
-        }
 
+		// done
 		return obs;
 
+
+
+//        if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){
+
+//            //smoothing with alpha value
+//            if(measurementHistory.size() > 1){
+//				double alpha = 0.1;
+//                double lastMeasurement = measurementHistory[measurementHistory.size() - 1];
+//                currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement);
+
+//                obs->hpa = currentMeasurement;
+//            }else{
+//                obs->hpa = 0;
+//            }
+
+//            measurementHistory.push_back(currentMeasurement);
+//        }
+//        else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){
+
+//            currentMeasurement = hPa;
+//            measurementHistory.push_back(currentMeasurement);
+
+//            // calculate the relative air pressure by getting the mean of the first and last three entrys of the history
+//            // and subtract them.
+//            if (measurementHistory.size() > 5){
+//                double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0;
+//                double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0;
+
+//                obs->hpa = meanHead - meanTail;
+//            }
+//            else{
+//                obs->hpa = 0;
+//            }
+//        }
+//        else //no data smoothing
+//        {
+//            measurementHistory.push_back(currentMeasurement);
+//            obs->hpa = currentMeasurement;
+//        }
+
+//		return obs;
+
     }
 
     //TODO