began putting everything together

code/particles/MyControl.h

@@ -0,0 +1,8 @@
+#ifndef MYCONTROL_H
+#define MYCONTROL_H
+
+struct MyControl {
+
+};
+
+#endif // MYCONTROL_H

code/particles/MyEvaluation.h

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

code/particles/MyInitializer.h

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

code/particles/MyObservation.h

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

code/particles/MyState.h

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

code/particles/MyTransition.h

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

code/particles/MyTransitionSimple.h

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

code/particles/P3.h

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