OTHER2017/pf/PF.h

#ifndef PF_H
#define PF_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/WiFiQualityAnalyzer.h>

#include <Indoor/sensors/gps/GPSData.h>
#include <Indoor/sensors/gps/GPSProbability.h>
#include <Indoor/sensors/activity/Activity.h>

#include <Indoor/grid/walk/v2/GridWalker.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 <Indoor/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h>

#include "../Settings.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);
	}

};

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;
	}

};

/** observed sensor data */
struct MyObservation {

	/** wifi measurements */
	WiFiMeasurements wifi;

	/** gps measurements */
	GPSData gps;

	/** absolute heading in radians */
	float compassAzimuth_rad = 0;

	// TODO: switch to a general activity enum/detector for barometer + accelerometer + ...?
	/** detected activity */
	//ActivityButterPressure::Activity activity;
	Activity activityNew = Activity::STANDING;

	/** time of evaluation */
	Timestamp currentTime;

};

/** (observed) control data */
struct MyControl {

	/** turn angle (in radians) since the last transition */
	float turnSinceLastTransition_rad = 0;

	/** number of steps since the last transition */
	int numStepsSinceLastTransition = 0;

	/** absolute heading in radians */
	float compassAzimuth_rad = 0;

	// TODO: switch to a general activity enum/detector using barometer + accelerometer?
	/** currently detected activity */
	//ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY;

	Activity activityNew = Activity::STANDING;

	/** reset the control-data after each transition */
	void resetAfterTransition() {
		turnSinceLastTransition_rad = 0;
		numStepsSinceLastTransition = 0;
	}

};

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) {
			again:
				const int idx = distIdx(gen);
				const MyGridNode& node = (*grid)[idx];
			if (node.getNumNeighbors() != 8) {goto again;}
			p.state.position = node;								// random position
			p.state.heading.direction = Heading(distHead(gen));		// random heading
			p.weight = 1.0 / particles.size();						// equal weight
		}

	}

};

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;
	WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
	WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
	//WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;

	WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
	WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;

	std::minstd_rand gen;

public:

	PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma),
		modAbsHead(&ctrl, Settings::IMU::absHeadSigma),
		modDestination(*grid)
		//,modActivity(&ctrl)
		{

		walker.addModule(&modRelHead);
		//walker.addModule(&modAbsHead);
		//walker.addModule(&modActivity);

		//walker.addModule(&modFavorZ);
		//walker.addModule(&modImportance);

//		if (Settings::destination != GridPoint(0,0,0)) {
//			//walker.addModule(&modDestination);
//			modDestination.setDestination(grid->getNodeFor(Settings::destination));
//		}

	}


	int numTrans = 0;



	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {

		++numTrans;

		// 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);


//		std::normal_distribution<float> turnNoise (0, 0.2);
//		std::uniform_real_distribution<float> initTurnNoise(-0.3, +0.3);
//		std::uniform_real_distribution<float> initDistNoise(0.0, 0.5);

		// 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
			float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));

			K::Particle<MyState>& p = particles[i];

			// first transitions: more variation as the state is unknown
			if (numTrans < 70 || numTrans % (10*1) == 0) {

				const MyGridNode* n = grid->getNodePtrFor(p.state.position);

				for (int j = 0; j < 5; ++j) {
					std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
					const int idx = distIdx(gen);
					n = &(grid->getNeighbor(*n, idx));
				}
				p.state.position = *n;

				if (numTrans < 70) {
					std::normal_distribution<float> distTurn(0, 0.3);
					p.state.heading.direction += distTurn(gen);
					dist_m += 0.5;
				}

			}

			//p.state.heading.direction += turnNoise(gen);


			// "blur" the density by adjusting the weights
			//p.weight = std::pow(p.weight, 0.4);

			//p.weight = 1;
			double prob = 1.0;
			p.state = walker.getDestination(*grid, p.state, dist_m, prob);
			p.weight *= prob;							// grid-walk-probability

			//const GridNode gn = grid->getNodeFor(p.state.position);
			//const double probNode = (gn.getNumNeighbors() >= 8) ? (0.7) : (0.3);
			//p.weight *= probNode;


			if (p.weight != p.weight) {
				throw Exception("nan");
			}
			if (p.weight == 0) {
				std::cout << "one particle weight is 0.0!" << std::endl;
				//throw Exception("weight = 0.0");
			}

		}

		// normalize
		double weightSum = 0;
		for (const auto& p : particles) {weightSum += p.weight;}
		for (auto& p : particles) {p.weight /= weightSum;}

	}

};

class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {

	Grid<MyGridNode>* grid;

	WiFiModel& wifiModel;

	EarthMapping& em;


	WiFiObserverFree wiFiProbability;					// free-calculation

	// smartphone is 1.3 meter above ground
	const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);

public:

	PFEval(Grid<MyGridNode>* grid, WiFiModel& wifiModel, EarthMapping& em) :
		grid(grid), wifiModel(wifiModel), em(em),

		wiFiProbability(Settings::WiFiModel::sigma, wifiModel) {		// WiFi free



	}


	double getAbsHead(const MyState& s, const MyObservation& obs) {

		// compare the heading against the state's heading - the last error
		const Heading stateHead = s.heading.direction;
		const Heading obsHead(obs.compassAzimuth_rad);

		// get the difference
		const float angularDiff = obsHead.getDiffHalfRAD(stateHead);

		double res = 0;

		if		(angularDiff > Angle::degToRad(120))	{res = 0.30;}
		else											{res = 0.70;}

		return res;

	}

	double getActivity(const MyState& s, const MyObservation& obs) {

		const MyGridNode& n = grid->getNodeFor(s.position);

		switch(obs.activityNew) {
		case Activity::WALKING:
			return (n.getType() != MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
		case Activity::WALKING_DOWN:
		case Activity::WALKING_UP:
			return (n.getType() == MyGridNode::TYPE_STAIR) ? (0.7) : (0.3);
		default:
			return 1;
		}

		return 1.0;

	}

	WiFiQualityAnalyzer wqa;

	double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {

		// 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();
		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));


		wqa.add(wifiObs);
		float quality = wqa.getQuality();
		//std::cout << "wifi quality: " << quality << std::endl;


		// GPS
		const GPSProbability gpsProb(em);


		// sanity check
		Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");

		// use (0.9 * p) + (0.1 * (1-p)) for error cases
		wiFiProbability.setUseError(true);

		//#pragma omp parallel for num_threads(3)
		for (int i = 0; i < Settings::numParticles; ++i) {

			K::Particle<MyState>& p = particles[i];

			// WiFi free
			double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, wifiObs);
			//double pWiFiMod = Distribution::Exponential<double>::getProbability(0.20, -std::log(pWiFi));
			//double pWiFiMod = std::pow(pWifi, 0.2);
			//double pWiFiVeto = wiFiProbability.getVeto(p.state.position.inMeter()+person, observation.currentTime, wifiObs);


			const bool volatile init = observation.currentTime.sec() < 25;
			//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
			double pWiFiMod = (init) ? (std::pow(pWiFi, 0.5)) : (std::pow(pWiFi, 0.9));


			// WiFi grid
			//const MyGridNode& node = grid->getNodeFor(p.state.position);
			//const MyGridNode& node = grid->getNodeFor(p.state.position);
			//const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);

			//const double pStair = 1;//getStairProb(p, observation.activity);
			double pGPS = gpsProb.getProbability(p.state.position.inMeter(), observation.gps);
			double pAbsHead = getAbsHead(p.state, observation);
			double pActivty = getActivity(p.state, observation);

			// bad wifi? -> we have no idea where we are!
			if (quality < 0.25 && !init) {
				pWiFiMod = 1;
				//p.weight = std::pow(p.weight, 0.5);
			}

			// overall evaluation
			const double prob = pWiFiMod * pAbsHead * pActivty;

			//GPS ERROR?!?!?! does it work without disabling wifi when gps is disabled?

			if (pGPS != 1) {
				int i = 0; (void) i;
			}

			// TESTING
			//p.weight = std::pow(p.weight, 0.5);

//			// do NOT update weights
//			if (quality < 0.25) {
//				p.weight = 1;
//			}

			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");
			}
			if (p.weight == 0) {
				std::cout << "one particle weight is 0.0!" << std::endl;
				// throw Exception("weight = 0.0");
			}

		}

		return 0;

	}

};

#endif // PF_H