refactoring to add nav mesh

nav/grid/Filter.h

@@ -0,0 +1,253 @@
+#ifndef FILTER_H
+#define FILTER_H
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
+
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
+
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
+
+#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
+
+#include "../Observation.h"
+#include "State.h"
+#include "Node.h"
+#include "NodeResampling.h"
+#include "../Settings.h"
+
+#include <omp.h>
+#include <future>
+
+namespace GridBased {
+
+	class PFInit : public K::ParticleFilterInitializer<MyState> {
+
+	private:
+
+		Grid<MyGridNode>* grid;
+
+	public:
+
+		PFInit(Grid<MyGridNode>* grid) : grid(grid) {
+
+		}
+
+		virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+			std::minstd_rand gen;
+			std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
+			std::uniform_real_distribution<float> distHead(0, 2*M_PI);
+
+
+			for (K::Particle<MyState>& p : particles) {
+				const int idx = distIdx(gen);
+				const MyGridNode& node = (*grid)[idx];
+				p.state.position = node;								// random position
+				p.state.heading.direction = Heading(distHead(gen));		// random heading
+				p.weight = 1.0 / particles.size();						// equal weight
+			}
+
+	//		// fix position + heading
+	//		for (K::Particle<MyState>& p : particles) {
+	////			const int idx = 9000;
+	////			const MyGridNode& node = (*grid)[idx];
+	//			const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0));		// center of the testmap
+	//			p.state.position = node;
+	//			p.state.heading.direction = Heading(0);
+	//		}
+
+		}
+
+	};
+
+	class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+
+	public:
+
+		/** local, static control-data COPY */
+		MyControl ctrl;
+
+		Grid<MyGridNode>* grid;
+		GridWalker<MyGridNode, MyState> walker;
+
+		WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
+		WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
+		WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
+		WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
+		WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
+
+		NodeResampling<MyState, MyGridNode> resampler;
+
+		std::minstd_rand gen;
+
+	public:
+
+		PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
+
+			//walker.addModule(&modFavorZ);
+			walker.addModule(&modHeading);
+			//walker.addModule(&modImportance);
+			walker.addModule(&modActivity);
+
+
+			if (Settings::destination != GridPoint(0,0,0)) {
+				//walker.addModule(&modDestination);
+				modDestination.setDestination(grid->getNodeFor(Settings::destination));
+			}
+
+		}
+
+
+
+		void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
+
+			// local copy!! observation might be changed async outside!! (will really produces crashes!)
+			this->ctrl = *_ctrl;
+			((MyControl*)_ctrl)->resetAfterTransition();
+
+			std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
+
+			// sanity check
+			Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
+
+			//for (K::Particle<MyState>& p : particles) {
+			#pragma omp parallel for num_threads(3)
+			for (int i = 0; i < Settings::numParticles; ++i) {
+
+				//#pragma omp atomic
+				const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
+
+				K::Particle<MyState>& p = particles[i];
+
+				double prob;
+				p.state = walker.getDestination(*grid, p.state, dist_m, prob);
+				//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
+				//p.weight = (prob > 0.01) ? (1.0) : (0.15);
+				//p.weight = prob;
+				//p.weight = 1.0;							// reset
+				//p.weight = std::pow(p.weight, 0.1);		// make all particles a little more equal [less strict]
+				//p.weight *= std::pow(prob, 0.1);		// add grid-walk-probability
+				p.weight = prob;							// grid-walk-probability
+				if (p.weight != p.weight) {throw Exception("nan");}
+
+			}
+
+		}
+
+	};
+
+	class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+		Grid<MyGridNode>* grid;
+
+		WiFiModelLogDistCeiling& wifiModel;
+
+
+		//WiFiObserverFree wiFiProbability;					// free-calculation
+		WiFiObserverGrid<MyGridNode> wiFiProbability;		// grid-calculation
+
+		// smartphone is 1.3 meter above ground
+		const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
+
+	public:
+
+		PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
+			grid(grid), wifiModel(wifiModel),
+			//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) {		// WiFi free
+			wiFiProbability(Settings::WiFiModel::sigma) {					// WiFi grid
+
+
+		}
+
+		double getStairProb(const K::Particle<MyState>& p, const Activity act) {
+
+			const float kappa = 0.75;
+
+			const MyGridNode& gn = grid->getNodeFor(p.state.position);
+			switch (act) {
+
+			case Activity::STANDING:
+			case Activity::WALKING:
+				if (gn.getType() == GridNode::TYPE_FLOOR)		{return kappa;}
+				if (gn.getType() == GridNode::TYPE_DOOR)		{return kappa;}
+																{return 1-kappa;}
+
+			case Activity::WALKING_UP:
+			case Activity::WALKING_DOWN:
+				if (gn.getType() == GridNode::TYPE_STAIR)		{return kappa;}
+				if (gn.getType() == GridNode::TYPE_ELEVATOR)	{return kappa;}
+																{return 1-kappa;}
+
+			}
+
+			return 1.0;
+
+		}
+
+		double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
+
+			double sum = 0;
+
+			// local copy!! observation might be changed async outside!! (will really produces crashes!)
+			const MyObservation observation = _observation;
+
+			// vap-grouping
+			const int numAP1 = observation.wifi.entries.size();
+			const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
+			const int numAP2 = wifiObs.entries.size();
+
+			Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
+
+			// sanity check
+			Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
+
+			#pragma omp parallel for num_threads(3)
+			for (int i = 0; i < Settings::numParticles; ++i) {
+
+				K::Particle<MyState>& p = particles[i];
+
+				// WiFi free
+				//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
+
+				// WiFi grid
+				const MyGridNode& node = grid->getNodeFor(p.state.position);
+				const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
+
+
+				//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
+
+				const double pStair = getStairProb(p, observation.activity);
+				const double pGPS = 1;
+				const double prob = pWiFi * pGPS * pStair;
+
+				p.weight *= prob;		// NOTE: keeps the weight returned by the transition step!
+				//p.weight = prob;		// does NOT keep the weights returned by the transition step
+				if (p.weight != p.weight) {throw Exception("nan");}
+
+				#pragma omp atomic
+				sum += p.weight;
+
+			}
+
+			return sum;
+
+		}
+
+	};
+
+}
+
+
+
+#endif // FILTER_H

nav/grid/NavControllerGrid.cpp

@@ -0,0 +1,266 @@
+#include "NavControllerGrid.h"
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+
+#include "Settings.h"
+#include "RegionalResampling.h"
+#include "NodeResampling.h"
+
+#include "../ui/debug/SensorDataWidget.h"
+#include "../ui/map/3D/MapView3D.h"
+#include "../ui/map/2D/MapView2D.h"
+#include "../ui/debug/InfoWidget.h"
+
+#include "../Controller.h"
+
+Q_DECLARE_METATYPE(const void*)
+
+GridBased::NavControllerGrid::NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid) : NavController(mainController, im), grid(grid), wifiModel(im) {
+
+	// filter init
+	std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
+
+	// estimation
+	//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
+	std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
+
+	// resampling
+	std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
+	//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
+	//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
+	//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
+
+	// eval and transition
+	wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
+	std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
+	std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
+
+	// setup the filter
+	pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
+	pf->setTransition(std::move(transition));
+	pf->setEvaluation(std::move(eval));
+	pf->setEstimation(std::move(estimation));
+	pf->setResampling(std::move(resample));
+
+	pf->setNEffThreshold(0.85);         //before 0.75, edit by toni
+	//pf->setNEffThreshold(0.65);		// still too low?
+	//pf->setNEffThreshold(0.25);		// too low
+
+	// attach as listener to all sensors
+	SensorFactory::get().getAccelerometer().addListener(this);
+	SensorFactory::get().getGyroscope().addListener(this);
+	SensorFactory::get().getBarometer().addListener(this);
+	SensorFactory::get().getWiFi().addListener(this);
+	SensorFactory::get().getSteps().addListener(this);
+	SensorFactory::get().getTurns().addListener(this);
+	//SensorFactory::get().getActivity().addListener(this);
+
+}
+
+void GridBased::NavControllerGrid::start() {
+
+	Assert::isFalse(running, "already started!");
+	running = true;
+	curCtrl.resetAfterTransition();	// ensure we start empty ;)
+	tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this);
+	tDisplay = std::thread(&NavControllerGrid::updateMapViewLoop, this);
+
+	// start all sensors
+	SensorFactory::get().getAccelerometer().start();
+	SensorFactory::get().getGyroscope().start();
+	SensorFactory::get().getBarometer().start();
+	SensorFactory::get().getWiFi().start();
+
+#ifndef ANDROID
+	// #include <valgrind/callgrind.h>
+	// run with
+	// valgrind --tool=callgrind --quiet  --instr-atstart=no  ./yasmin
+	// show with
+	// kcachegrind callgrind.out.xxxx
+	CALLGRIND_START_INSTRUMENTATION;
+#endif
+
+}
+
+void GridBased::NavControllerGrid::stop() {
+	Assert::isTrue(running, "not started!");
+	running = false;
+	tFilter.join();
+	tDisplay.join();
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
+	(void) sensor;
+	(void) data;
+	(void) ts;
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
+	(void) sensor;
+	(void) ts;
+	(void) data;
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
+	(void) sensor;
+	(void) ts;
+	(void) data;
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
+	(void) sensor;
+	(void) ts;
+	curObs.wifi = data;
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
+	(void) sensor;
+	(void) ts;
+	curObs.gps = data;
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
+	(void) sensor;
+	(void) ts;
+	curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent;		// set to zero after each transition
+	gotSensorData(ts);
+}
+
+void GridBased::NavControllerGrid::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
+	(void) sensor;
+	(void) ts;
+	curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent;			// set to zero after each transition
+	gotSensorData(ts);
+}
+
+/** called when any sensor has received new data */
+void GridBased::NavControllerGrid::gotSensorData(const Timestamp ts) {
+	curObs.currentTime = ts;
+	if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
+}
+
+//	void debugActivity(const ActivityData& activity)  {
+//		QString act;
+//		switch(activity.curActivity) {
+//			case ActivityButterPressure::Activity::STAY:	act = "STAY"; break;
+//			case ActivityButterPressure::Activity::DOWN:	act = "DOWN"; break;
+//			case ActivityButterPressure::Activity::UP:		act = "UP"; break;
+//			default:										act = "???"; break;
+//		}
+//		Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
+//	}
+
+	/** particle-filter update loop */
+	void GridBased::NavControllerGrid::filterUpdateLoop() {
+
+
+		while(running && !Settings::Filter::useMainThread) {
+
+//			// fixed update rate based on the systems time -> LIVE! even for offline data
+//			const Timestamp ts1 = Timestamp::fromUnixTime();
+//			doUpdate();
+//			const Timestamp ts2 = Timestamp::fromUnixTime();
+//			const Timestamp needed = ts2-ts1;
+//			const Timestamp sleep = Timestamp::fromMS(500) - needed;
+//			std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
+
+			const bool wasUpdated = filterUpdateIfNeeded();
+			if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
+
+		}
+	}
+
+
+
+	/** check whether its time for a filter update, and if so, execute the update and return true */
+	bool GridBased::NavControllerGrid::filterUpdateIfNeeded() {
+
+		static float avgSum = 0;
+		static int avgCount = 0;
+
+		// fixed update rate based on incoming sensor data
+		// allows working with live data and faster for offline data
+		const Timestamp diff = curObs.currentTime - lastTransition;
+		if (diff >= Settings::Filter::updateEvery) {
+
+			// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
+			const Timestamp err = diff - Settings::Filter::updateEvery;
+			lastTransition = curObs.currentTime - err;
+
+			const Timestamp ts1 = Timestamp::fromUnixTime();
+			filterUpdate();
+			const Timestamp ts2 = Timestamp::fromUnixTime();
+			const Timestamp tsDiff = ts2-ts1;
+			const QString filterTime = QString::number(tsDiff.ms());
+			avgSum += tsDiff.ms(); ++avgCount;
+			//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
+			QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
+			return true;
+
+		} else {
+
+			return false;
+
+		}
+
+	}
+
+
+
+	/** perform a filter-update (called from a background-loop) */
+	void GridBased::NavControllerGrid::filterUpdate() {
+
+		//lastEst = curEst;
+		MyState sCurEst = pf->update(&curCtrl, curObs);
+		curEst.pos_m = sCurEst.position.inMeter();
+		curEst.head = sCurEst.heading.direction;
+		//Log::add("Nav", "cur est: " + curEst.position.asString());
+
+		// inform listeners about the new estimation
+		for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}
+
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
+
+		// update estimated path
+		estPath.push_back(curEst.pos_m);
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
+		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
+
+		PFTrans* trans = (PFTrans*)pf->getTransition();
+		const MyGridNode* node = grid->getNodePtrFor(sCurEst.position);
+		if (node) {
+			try {
+				pathToDest = trans->modDestination.getShortestPath(*node);
+				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+				Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathToDestination", Qt::QueuedConnection, Q_ARG(const void*, &pathToDest)), "call failed");
+			} catch (...) {;}
+		}
+		//	mainController->getMapView()->showGridImportance();
+
+	}
+
+
+
+	/** UI update loop */
+	void GridBased::NavControllerGrid::updateMapViewLoop() {
+
+		while(running) {
+			const Timestamp ts1 = Timestamp::fromUnixTime();
+			updateMapView();
+			const Timestamp ts2 = Timestamp::fromUnixTime();
+			const Timestamp tsDiff = ts2-ts1;
+			const QString mapViewTime = QString::number(tsDiff.ms());
+			//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
+			std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
+		}
+	}
+

nav/grid/NavControllerGrid.h

@@ -0,0 +1,89 @@
+#ifndef NAVCONTROLLERGRID_H
+#define NAVCONTROLLERGRID_H
+
+#include "../sensors/AccelerometerSensor.h"
+#include "../sensors/GyroscopeSensor.h"
+#include "../sensors/BarometerSensor.h"
+#include "../sensors/WiFiSensor.h"
+#include "../sensors/SensorFactory.h"
+#include "../sensors/StepSensor.h"
+#include "../sensors/TurnSensor.h"
+
+#include <Indoor/Assertions.h>
+#include <thread>
+
+#include "State.h"
+#include "Filter.h"
+#include "../Controller.h"
+
+#include "../NavController.h"
+
+
+namespace GridBased {
+
+	class NavControllerGrid : public NavController {
+
+	private:
+
+		Grid<MyGridNode>* grid;
+		WiFiModelLogDistCeiling wifiModel;
+
+		std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
+
+		DijkstraPath<MyGridNode> pathToDest;
+
+		MyObservation curObs;
+		MyControl curCtrl;
+
+
+	public:
+
+		NavControllerGrid(Controller* mainController, Floorplan::IndoorMap* im, Grid<MyGridNode>* grid);
+
+
+		void start() override;
+
+		void stop() override;
+
+
+		void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override;
+
+		void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override;
+
+		void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override;
+
+		void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override;
+
+		void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override;
+
+		void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override ;
+
+		void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
+
+		//	void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
+
+
+	private:
+
+		/** called when any sensor has received new data */
+		void gotSensorData(const Timestamp ts);
+
+	//	void debugActivity(const ActivityData& activity);
+
+		/** particle-filter update loop */
+		void filterUpdateLoop();
+
+		/** check whether its time for a filter update, and if so, execute the update and return true */
+		bool filterUpdateIfNeeded();
+
+		/** perform a filter-update (called from a background-loop) */
+		void filterUpdate();
+
+		/** UI update loop */
+		void updateMapViewLoop();
+
+	};
+
+}
+
+#endif // NAVCONTROLLERGRID_H

nav/grid/Node.h

@@ -0,0 +1,33 @@
+#ifndef NODE_H
+#define NODE_H
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/sensors/radio/WiFiGridNode.h>
+
+struct MyGridNode : public GridNode, public GridPoint, public WiFiGridNode<30> {
+
+	float navImportance;
+	float getNavImportance() const { return navImportance; }
+
+	float walkImportance;
+	float getWalkImportance() const { return walkImportance; }
+
+
+	/** empty ctor */
+	MyGridNode() : GridPoint(-1, -1, -1) {;}
+
+	/** ctor */
+	MyGridNode(const int x_cm, const int y_cm, const int z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
+
+
+	static void staticDeserialize(std::istream& inp) {
+		WiFiGridNode::staticDeserialize(inp);
+	}
+
+	static void staticSerialize(std::ostream& out) {
+		WiFiGridNode::staticSerialize(out);
+	}
+
+};
+
+#endif // NODE_H

nav/grid/NodeResampling.h

@@ -0,0 +1,123 @@
+#ifndef NODERESAMPLING_H
+#define NODERESAMPLING_H
+
+
+#include <algorithm>
+#include <random>
+
+#include <Indoor/grid/Grid.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h>
+
+
+	/**
+	 * uses simple probability resampling by drawing particles according
+	 * to their current weight.
+	 * HOWEVER: after drawing them, do NOT use them directly, but replace them with a neighbor
+	 * O(log(n)) per particle
+	 */
+	template <typename State, typename Node>
+	class NodeResampling : public K::ParticleFilterResampling<State> {
+
+	private:
+
+		/** this is a copy of the particle-set to draw from it */
+		std::vector<K::Particle<State>> particlesCopy;
+
+		/** random number generator */
+		std::minstd_rand gen;
+
+		Grid<Node>& grid;
+
+	public:
+
+		/** ctor */
+		NodeResampling(Grid<Node>& grid) : grid(grid) {
+			gen.seed(1234);
+		}
+
+		void resample(std::vector<K::Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			// TODO: this solution requires EXPLICIT overloading which is bad...
+			//static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
+
+			const uint32_t cnt = (uint32_t) particles.size();
+
+			// equal weight for all particles. sums up to 1.0
+			const double equalWeight = 1.0 / (double) cnt;
+
+			// ensure the copy vector has the same size as the real particle vector
+			particlesCopy.resize(cnt);
+
+			// swap both vectors
+			particlesCopy.swap(particles);
+
+			// calculate cumulative weight
+			double cumWeight = 0;
+			for (uint32_t i = 0; i < cnt; ++i) {
+				cumWeight += particlesCopy[i].weight;
+				particlesCopy[i].weight = cumWeight;
+			}
+
+			std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
+			std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
+			std::normal_distribution<float> distTurn(0.0, +0.03);
+
+			// now draw from the copy vector and fill the original one
+			// with the resampled particle-set
+			for (uint32_t i = 0; i < cnt; ++i) {
+
+				// slight chance to get a truely random node as particle
+				// mainly for testing
+				if (distNewOne(gen) < 0.005) {
+					particles[i].state.position = grid[distRndNode(gen)];
+					particles[i].weight = equalWeight;
+					continue;
+				}
+
+				// normal redraw procedure
+				particles[i] = draw(cumWeight);
+				particles[i].weight = equalWeight;
+
+				const Node* n = grid.getNodePtrFor(particles[i].state.position);
+				if (n == nullptr) {continue;}	// should not happen!
+
+				for (int j = 0; j < 2; ++j) {
+					std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
+					const int idx = distIdx(gen);
+					n = &grid.getNeighbor(*n, idx);
+				}
+
+
+				particles[i].state.position = *n;
+				particles[i].state.heading.direction += distTurn(gen);
+
+			}
+
+		}
+
+	private:
+
+		/** draw one particle according to its weight from the copy vector */
+		const K::Particle<State>& draw(const double cumWeight) {
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> dist(0, cumWeight);
+
+			// draw a random value between [0:cumWeight]
+			const float rand = dist(gen);
+
+			// search comparator (cumWeight is ordered -> use binary search)
+			auto comp = [] (const K::Particle<State>& s, const float d) {return s.weight < d;};
+			auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
+			return *it;
+
+		}
+
+
+
+	};
+
+
+
+#endif // NODERESAMPLING_H

nav/grid/RegionalResampling.h

@@ -0,0 +1,72 @@
+#ifndef REGIONALRESAMPLING_H
+#define REGIONALRESAMPLING_H
+
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include "State.h"
+
+namespace GridBased {
+
+	class RegionalResampling : public K::ParticleFilterResampling<MyState> {
+
+	public:
+
+		float maxDist = 12.5;
+
+		RegionalResampling() {;}
+
+		void resample(std::vector<K::Particle<MyState>>& particles) override {
+
+			Point3 sum;
+			for (const K::Particle<MyState>& p : particles) {
+				sum += p.state.position.inMeter();
+			}
+			const Point3 avg = sum / particles.size();
+
+			std::vector<K::Particle<MyState>> next;
+			for (const K::Particle<MyState>& p : particles) {
+				const float dist = p.state.position.inMeter().getDistance(avg);
+				if (rand() % 6 != 0) {continue;}
+				if (dist < maxDist) {next.push_back(p);}
+			}
+
+			// cumulate
+			std::vector<K::Particle<MyState>> copy = particles;
+			double cumWeight = 0;
+			for ( K::Particle<MyState>& p : copy) {
+				cumWeight += p.weight;
+				p.weight = cumWeight;
+			}
+
+			// draw missing particles
+			const int missing = particles.size() - next.size();
+			for (int i = 0; i < missing; ++i) {
+				next.push_back(draw(copy, cumWeight));
+			}
+
+			std::swap(next, particles);
+
+		}
+
+		std::minstd_rand gen;
+
+		/** draw one particle according to its weight from the copy vector */
+		const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> dist(0, cumWeight);
+
+			// draw a random value between [0:cumWeight]
+			const float rand = dist(gen);
+
+			// search comparator (cumWeight is ordered -> use binary search)
+			auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;};
+			auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
+			return *it;
+
+		}
+
+	};
+
+}
+
+#endif // REGIONALRESAMPLING_H

nav/grid/State.h

@@ -0,0 +1,44 @@
+#ifndef GRID_STATE_H
+#define GRID_STATE_H
+
+#include <Indoor/grid/walk/v2/GridWalker.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
+
+namespace GridBased {
+
+	struct MyState : public WalkState, public WalkStateFavorZ, public WalkStateHeading {
+
+
+		/** ctor */
+		MyState(const int x_cm, const int y_cm, const int z_cm) : WalkState(GridPoint(x_cm, y_cm, z_cm)), WalkStateHeading(Heading(0), 0) {
+			;
+		}
+
+		MyState() : WalkState(GridPoint()), WalkStateHeading(Heading(0), 0) {
+			;
+		}
+
+		MyState& operator += (const MyState& o) {
+			position += o.position;
+			return *this;
+		}
+
+		MyState& operator /= (const float val) {
+			position /= val;
+			return *this;
+		}
+
+		MyState operator * (const float val) const {
+			MyState copy = *this;
+			copy.position = copy.position * val;
+			return copy;
+		}
+
+	};
+
+}
+
+#endif // GRID_STATE_H