a lot!!! of changes

added main menu added debug display many debug widgets for plotting live data worked on android live sensors added offline-data sensor feeding some dummy data sensors worked on the map display added ui debug for grid-points, particles and weights added a cool dude to display the estimation added real filtering based on the Indoor components c++11 fixes for android compilation online and offline filtering support new resampling technique for testing map loading via dialog
2016-09-16 19:30:04 +02:00
parent d910e88220
commit 075d8bb633
90 changed files with 4735 additions and 624 deletions
--- a/nav/Filter.h
+++ b/nav/Filter.h
@@ -0,0 +1,149 @@
+#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/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/WalkModuleButterActivity.h>
+#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
+
+#include "State.h"
+#include "Node.h"
+
+#include "../Settings.h"
+
+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
+		}
+
+//		// fix position + heading
+//		for (K::Particle<MyState>& p : particles) {
+//			const int idx = 9000;
+//			const MyGridNode& node = (*grid)[idx];
+//			p.state.position = node;
+//			p.state.heading.direction = Heading(0);
+//		}
+
+	}
+
+};
+
+class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+
+public:
+
+	Grid<MyGridNode>* grid;
+	GridWalker<MyGridNode, MyState> walker;
+
+	WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
+	WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
+	WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
+	WalkModuleButterActivity<MyGridNode, MyState> modBarometer;
+	WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
+
+	std::minstd_rand gen;
+
+public:
+
+	PFTrans(Grid<MyGridNode>* grid, MyControl* ctrl) : grid(grid), modHeading(ctrl, Settings::turnSigma), modDestination(*grid) {
+
+		walker.addModule(&modFavorZ);
+		walker.addModule(&modHeading);
+		walker.addModule(&modImportance);
+		walker.addModule(&modBarometer);
+		walker.addModule(&modDestination);
+
+		if (Settings::destination != GridPoint(0,0,0)) {
+			modDestination.setDestination(grid->getNodeFor(Settings::destination));
+		}
+
+	}
+
+
+
+	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		std::normal_distribution<float> noise(0, Settings::stepSigma);
+
+		for (K::Particle<MyState>& p : particles) {
+			const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::stepLength + noise(gen));
+			p.state = walker.getDestination(*grid, p.state, dist_m);
+		}
+
+		((MyControl*)control)->resetAfterTransition();
+
+	}
+
+};
+
+class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+	WiFiModelLogDistCeiling& wifiModel;
+	WiFiObserverFree wiFiProbability;
+
+public:
+
+	PFEval(WiFiModelLogDistCeiling& wifiModel) : wifiModel(wifiModel), wiFiProbability(Settings::wifiSigma, wifiModel) {
+
+	}
+
+	double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
+
+		double sum = 0;
+
+		// smartphone is 1.3 meter above ground
+		const Point3 person(0,0,Settings::smartphoneAboveGround);
+
+		// local copy!! observation might be changed async outside!! (will really produces crashes!)
+		const MyObservation observation = _observation;
+
+		for (K::Particle<MyState>& p : particles) {
+			const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, observation.wifi);
+			const double pGPS = 1;
+			const double prob = pWiFi * pGPS;
+			p.weight = prob;
+			sum += prob;
+		}
+
+		return sum;
+
+	}
+
+};
+
+
+
+#endif // FILTER_H