huge commit

- worked on about everything
- grid walker using plugable modules
- wifi models
- new distributions
- worked on geometric data-structures
- added typesafe timestamps
- worked on grid-building
- added sensor-classes
- added sensor analysis (step-detection, turn-detection)
- offline data reader
- many test-cases

grid/walk/v2/modules/WalkModuleRelativePressureControl.h

@@ -0,0 +1,162 @@
+#ifndef WALKMODULERELATIVEPRESSURE_H
+#define WALKMODULERELATIVEPRESSURE_H
+
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+#include "../../../../Assertions.h"
+
+
+/**
+ * a walk-state the contains the pressure relative to time t0
+ */
+struct WalkStateRelativePressure {
+
+	/** the pressure level [relative to time t0] one SHOULD messure at this state */
+	float pressureRelToT0;
+
+	int dirLock = 0;
+
+	/** ctor */
+	WalkStateRelativePressure(const float pressureRelToT0) : pressureRelToT0(pressureRelToT0) {;}
+
+};
+
+
+/**
+ * uses the pressure relative to t=0 within the control-data
+ * to estimate the likelyhood for any z-changes during the transition
+ */
+template <typename Node, typename WalkState, typename Control> class WalkModuleRelativePressureControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	Control* ctrl;
+
+	/** pressure-change (hPa) per meter */
+	const float hPaPerMeter = 0.126f;		// given an average hPa of 938
+
+public:
+
+	/** ctor */
+	WalkModuleRelativePressureControl(Control* ctrl, const float hPaPerMeter) : ctrl(ctrl), hPaPerMeter(hPaPerMeter) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		// e.g. walking down from the 3rd to the second floor
+//		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+//		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+//		const int deltaZ_cm = startNode.z_cm - endNode.z_cm;
+//		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+
+//		// update the states pressure
+//		state.pressureRelToT0 += deltaPressure;
+
+//		// sanity checks
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+//		static int xx = 0;
+//		if (++xx % 1024 == 0) {
+//			++xx;
+//		}
+
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - nextNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		// update the states pressure
+		state.pressureRelToT0 = expectedPressure;
+
+		// sanity checks
+		Assert::isNotNaN(deltaPressure, "detected NaN!");
+		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");
+
+		if(std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {--state.dirLock;}
+			if (state.dirLock < 0) {++state.dirLock;}
+		} else {
+			if (deltaZ_cm > 0) {state.dirLock = +25;}
+			if (deltaZ_cm < 0) {state.dirLock = -25;}
+		}
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the values from control-data
+		const float curRelPressure = ctrl->barometer.hPaRelativeToT0;
+		const float pressureSigma = ctrl->barometer.estimatedSigma * 1.50f;
+
+		// not yet available/calibrated/possible -> skip evaluation!
+		if (pressureSigma == 0) {return 1;}
+
+		// e.g. walking down from the 3rd to the second floor
+		// startZ = 10 meter, endZ = 7 meter -> deltaZ = +3 meter;
+		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
+		const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
+		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
+		const float expectedPressure = state.pressureRelToT0 + deltaPressure;
+
+		const float oldErr = std::abs(state.pressureRelToT0 - curRelPressure);
+		const float newErr = std::abs(expectedPressure - curRelPressure);
+
+		if (std::abs(state.dirLock) > 0) {
+			if (state.dirLock > 0) {
+				if (deltaZ_cm < 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			} else {
+				if (deltaZ_cm > 0) {return 0;}
+				if (deltaZ_cm == 0) {return 0.1;}
+				return 0.9;
+			}
+		}
+
+
+		if (oldErr > pressureSigma) {
+			if (deltaZ_cm == 0) {return 0.001;}
+		}
+
+		if (newErr > oldErr) {return 0.002;}
+		if (newErr == oldErr) {return 0.005;}
+		if (newErr < oldErr) {return 0.99;}
+		throw 1;
+
+
+
+//		// compare control-data with potential transition
+//		double prob = Distribution::Normal<double>::getProbability(curRelPressure, pressureSigma, expectedPressure);
+////		prob = std::pow(prob, 20);
+
+//		// sanity checks
+//		Assert::isNotNaN(prob, "detected NaN!");
+//		Assert::isNotNaN(deltaPressure, "detected NaN!");
+//		Assert::isNotNaN(expectedPressure, "detected NaN!");
+
+//		return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULERELATIVEPRESSURE_H