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/GridWalker.h

@@ -0,0 +1,125 @@
+#ifndef GRIDWALKER_H
+#define GRIDWALKER_H
+
+#include "../../Grid.h"
+#include "../../../math/DrawList.h"
+#include "modules/WalkModule.h"
+
+/**
+ * modular grid-walker that takes various sub-components to determine
+ * p(e) and thus randomly pick edges
+ */
+template <typename Node, typename WalkState> class GridWalker {
+
+private:
+
+	/** all modules to evaluate */
+	std::vector<WalkModule<Node, WalkState>*> modules;
+
+	DrawList<const Node*> drawer;
+
+public:
+
+	/** add the given module */
+	void addModule(WalkModule<Node, WalkState>* module) {
+		modules.push_back(module);
+	}
+
+	/** perform the walk based on the configured setup */
+	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {
+
+		WalkState startState = _startState;
+		updateBefore(startState);
+
+
+
+		// get the node that corresponds to start;
+		const Node* startNode = grid.getNodePtrFor(startState.startPos);
+		Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
+
+		// currently examined node
+		const Node* curNode = startNode;
+
+		// until distance is reached
+		while (dist_m > 0) {
+
+			drawer.reset();
+
+			// evaluate each neighbor
+			for (const Node& neighbor : grid.neighbors(*curNode)) {
+				const double prob = getProbability(startState, *startNode, *curNode, neighbor);
+				drawer.add(&neighbor, prob);
+			}
+
+			// pick a neighbor
+			const Node* nextNode = drawer.get();
+
+			// inform
+			step(startState, *curNode, *nextNode);
+
+			// update
+			dist_m -= nextNode->getDistanceInMeter(*curNode);
+			curNode = nextNode;
+
+		}
+
+		// output state
+		WalkState nextState = startState;
+		nextState.startPos = *curNode;
+
+		// update
+		updateAfter(nextState, *startNode, *curNode);
+
+		// done
+		return nextState;
+
+	}
+
+private:
+
+	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
+	inline void updateBefore(WalkState& state) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateBefore(state);
+		}
+
+	}
+
+	/** update the WalkState after the random walk (if needed) */
+	inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateAfter(state, startNode, endNode);
+		}
+
+	}
+
+	/** one step taken. inform modules */
+	inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->step(state, curNode, nextNode);
+		}
+
+	}
+
+	/** get the probability for the given random walk (one edge) */
+	inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
+
+		//double prob = 1.0;
+		double prob = 0;
+
+		for (const WalkModule<Node, WalkState>* mdl : modules) {
+			//prob *= mdl->getProbability(state, start, cur, next);
+			prob += std::log( mdl->getProbability(state, start, cur, next) );
+		}
+
+		//return prob;
+		return std::exp(prob);
+
+	}
+
+};
+
+#endif // GRIDWALKER_H

grid/walk/v2/GridWalkerMulti.h

@@ -0,0 +1,159 @@
+#ifndef GRIDWALKERMULTI_H
+#define GRIDWALKERMULTI_H
+
+#include "../../Grid.h"
+#include "../../../math/DrawList.h"
+#include "modules/WalkModule.h"
+
+// FOR TESTING
+#include "../../../geo/Heading.h"
+
+/**
+ * modular grid-walker that takes various sub-components to determine
+ * p(e) and thus randomly pick edges
+ */
+template <typename Node, typename WalkState> class GridWalkerMulti {
+
+private:
+
+	struct Path {
+		std::vector<const Node*> nodes;
+		const Node* getSrc() const {return nodes.front();}
+		const Node* getDst() const {return nodes.back();}
+	};
+
+
+private:
+
+	/** all modules to evaluate */
+	std::vector<WalkModule<Node, WalkState>*> modules;
+
+public:
+
+	/** add the given module */
+	void addModule(WalkModule<Node, WalkState>* module) {
+		modules.push_back(module);
+	}
+
+	/** perform the walk based on the configured setup */
+	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float dist_m) {
+
+		// get the node that corresponds to start;
+		const Node* const startNode = grid.getNodePtrFor(_startState.startPos);
+		Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");
+
+		const uint64_t seed = (uint64_t) startNode + rand();
+		DrawList<Path> drawPath;
+		drawPath.setSeed(seed);
+
+		for (int i = 0; i < 1; ++i) {
+
+			float dist = dist_m;
+
+			// currently examined node
+			const Node* curNode = startNode;
+
+			Path path;
+			path.nodes.push_back(curNode);
+
+			// until distance is reached
+			while (dist > 0) {
+
+				DrawList<const Node*> drawEdge;
+				drawEdge.setSeed(seed+1);
+
+				//if (curNode->getNumNeighbors() < 6) {break;}
+
+				// evaluate each neighbor
+				for (const Node& neighbor : grid.neighbors(*curNode)) {
+					const double prob = getProb(*startNode, *curNode, neighbor);
+					drawEdge.add(&neighbor, prob);
+				}
+
+				// pick a neighbor
+				const Node* nextNode = drawEdge.get();
+
+				dist -= nextNode->getDistanceInMeter(*curNode);
+
+				curNode = nextNode;
+
+				path.nodes.push_back(curNode);
+
+			}
+
+			const double prob = getProb(path);
+			drawPath.add(path, prob);
+
+		}
+
+		const Path& bestPath = drawPath.get();
+
+		WalkState nextState = _startState;
+		nextState.startPos = *(bestPath.getDst());
+		return nextState;
+
+
+	}
+
+private:
+
+	inline double getProb(const Node& n0, const Node& n1, const Node& n2) {
+		static float angle = 0; angle += 0.05;
+		angle = std::fmod(angle, 6);
+		if (&n1 == &n2) {return 1;}
+		const Heading head1(angle);
+		const Heading head2(n1.x_cm, n1.y_cm, n2.x_cm, n2.y_cm);
+		const float diff = head1.getDiffHalfRAD(head2);
+		return rand() % 32;
+	}
+
+	inline double getProb(const Path& path) {
+		const Point3 p1 = path.getSrc()->inMeter();
+		const Point3 p2 = path.getDst()->inMeter();
+		const float dist = p1.getDistance(p2);
+		return 1/dist;
+	}
+
+	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
+	inline void updateBefore(WalkState& state) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateBefore(state);
+		}
+
+	}
+
+	/** update the WalkState after the random walk (if needed) */
+	inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->updateAfter(state, startNode, endNode);
+		}
+
+	}
+
+	/** one step taken. inform modules */
+	inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {
+
+		for (WalkModule<Node, WalkState>* mdl : modules) {
+			mdl->step(state, curNode, nextNode);
+		}
+
+	}
+
+	/** get the probability for the given random walk (one edge) */
+	inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {
+
+		double prob = 1.0;
+
+		for (const WalkModule<Node, WalkState>* mdl : modules) {
+			prob *= mdl->getProbability(state, start, cur, next);
+		}
+
+		return prob;
+
+	}
+
+};
+
+#endif // GRIDWALKERMULTI_H

grid/walk/v2/modules/WalkModule.h

@@ -0,0 +1,39 @@
+#ifndef WALKMODULE_H
+#define WALKMODULE_H
+
+#include "../../../Grid.h"
+
+/** base-class for all WalkStates */
+struct WalkState {
+
+	/** position where the walk starts */
+	GridPoint startPos;
+
+	/** ctor */
+	WalkState(const GridPoint& startPos) : startPos(startPos) {;}
+
+};
+
+
+/**
+ * base-class for all walk-modules that influence p(e)
+ */
+template <typename Node, typename WalkState> class WalkModule {
+
+public:
+
+	/** update the given WalkState before starting the walk. e.g. based on sensor readings */
+	virtual void updateBefore(WalkState& state) = 0;
+
+	/** get the probability p(e) from curNode to potentialNode */
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const = 0;
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) = 0;
+
+	/** update the walk state based on the given transition (if any update is necssary) */
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) = 0;
+
+};
+
+#endif // WALKMODULE_H

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -0,0 +1,68 @@
+#ifndef WALKMODULEFOLLOWDESTINATION_H
+#define WALKMODULEFOLLOWDESTINATION_H
+
+#include "WalkModule.h"
+#include "../../../../nav/dijkstra/Dijkstra.h"
+
+/**
+ * favour edges p(e) that approach the destination
+ */
+template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {
+
+private:
+
+	Dijkstra<Node> dijkstra;
+
+	struct DijkstraMapper {
+		const Grid<Node>& grid;
+		DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
+		int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
+		const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
+		float getWeightBetween(const Node& n1, const Node& n2) const {
+			return n1.getDistanceInCM(n2) * n2.navImportance;
+		}
+	};
+
+public:
+
+	/** ctor */
+	WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {
+
+		// shortest path calculation
+		dijkstra.build(&destination, DijkstraMapper(grid));
+
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	virtual double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+
+		const float kappa = 0.8;
+		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
+		const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);
+
+		// probability
+		return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+};
+
+#endif // WALKMODULEFOLLOWDESTINATION_H

grid/walk/v2/modules/WalkModuleHeading.h

@@ -0,0 +1,76 @@
+#ifndef WALKMODULEHEADING_H
+#define WALKMODULEHEADING_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+public:
+
+	/** ctor */
+	WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
+		;
+	}
+
+	virtual void updateBefore(WalkState& state) override {
+
+		(void) state;
+		state.startHeading += draw.get();
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+
+//		if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
+//			Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
+//			state.startHeading = head;
+//		}
+
+	}
+
+	/** one step (edge) is taken */
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+
+		// TODO
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
+
+		// compare the heading against the state's heading
+		const Heading stateHead = state.startHeading;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
+
+		// determine probability
+		return dist.getProbability(angularDiff);
+
+	}
+
+
+};
+
+#endif // WALKMODULEHEADING_H

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -0,0 +1,78 @@
+#ifndef WALKMODULEHEADINGCONTROL_H
+#define WALKMODULEHEADINGCONTROL_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {
+
+private:
+
+	/** van-Mises distribution */
+	Distribution::LUT<double> dist;
+
+	/** van-Mises draw list */
+	DrawList<double> draw;
+
+	Control* ctrl;
+
+	//std::unordered_map<WalkState*, float> errorTracker;
+
+public:
+
+	/** ctor 3.0 should be OK! */
+	WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 1.5).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+
+		const float var = draw.get() * 0.15;//0.05;
+		//const float var = 0;
+		state.startHeading += ctrl->turnAngle + var;
+
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) startNode;
+
+		// get the heading between curNode and potentialNode
+		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
+
+		// compare the heading against the state's heading - the last error
+		const Heading stateHead = state.startHeading;
+
+		// get the difference
+		const float angularDiff = head.getDiffHalfRAD(stateHead);
+
+		// determine probability
+		const float prob = dist.getProbability(angularDiff);
+		return prob;
+
+	}
+
+
+
+};
+
+#endif // WALKMODULEHEADINGCONTROL_H

grid/walk/v2/modules/WalkModuleNodeImportance.h

@@ -0,0 +1,55 @@
+#ifndef WALKMODULENODEIMPORTANCE_H
+#define WALKMODULENODEIMPORTANCE_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * favor edges based on the importance-factor of the next node.
+ * @see struct GridNodeImportance
+ */
+template <typename Node, typename WalkState> class WalkModuleNodeImportance : public WalkModule<Node, WalkState> {
+
+private:
+
+public:
+
+	/** ctor */
+	WalkModuleNodeImportance() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const double prob = potentialNode.getNavImportance();
+		return std::pow(prob, 10);
+		//return prob;
+
+	}
+
+
+};
+
+#endif // WALKMODULENODEIMPORTANCE_H

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

grid/walk/v2/modules/WalkModuleSpread.h

@@ -0,0 +1,62 @@
+#ifndef WALKMODULESPREAD_H
+#define WALKMODULESPREAD_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+
+/**
+ * simply try to move away from the starting node as much as possible
+ */
+template <typename Node, typename WalkState> class WalkModuleSpread : public WalkModule<Node, WalkState> {
+
+private:
+
+	Point3 avg;
+
+public:
+
+	/** ctor */
+	WalkModuleSpread() {
+		;
+	}
+
+
+	virtual void updateBefore(WalkState& state) override {
+		(void) state;
+		avg = avg * 0.999 + state.startPos.inMeter() * 0.001;
+	}
+
+	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
+		(void) state;
+		(void) startNode;
+		(void) endNode;
+	}
+
+	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
+		(void) state;
+		(void) curNode;
+		(void) nextNode;
+	}
+
+	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
+
+		(void) state;
+		(void) startNode;
+		(void) curNode;
+
+		const float dOld = avg.getDistance(curNode.inMeter());
+		const float dNew = avg.getDistance(potentialNode.inMeter());
+
+		if (dNew > dOld) {return 0.8;}
+		if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
+		if (dNew == dOld) {return 0.2;}
+		return 0;
+
+	}
+
+
+};
+
+
+#endif // WALKMODULESPREAD_H

grid/walk/v2/modules/WalkStateHeading.h

@@ -0,0 +1,16 @@
+#ifndef WALKSTATEHEADING_H
+#define WALKSTATEHEADING_H
+
+#include "../../../../geo/Heading.h"
+
+struct WalkStateHeading {
+
+	Heading startHeading;
+
+	/** ctor */
+	WalkStateHeading(const Heading& curHeading) : startHeading(curHeading) {;}
+
+};
+
+
+#endif // WALKSTATEHEADING_H