RussenJournal/filter/Logic.h

#ifndef FLOGIC_H
#define FLOGIC_H

#include <Indoor/grid/Grid.h>

#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>

#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>

#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>

#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>

#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>

#include "Structs.h"
#include <omp.h>
#include "../Settings.h"


/** particle-filter init */
struct PFInit : public K::ParticleFilterInitializer<MyState> {

	Grid<MyNode>& grid;

	PFInit(Grid<MyNode>& grid) : grid(grid) {;}

	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
		for (K::Particle<MyState>& p : particles) {

			int idx = rand() % grid.getNumNodes();
			p.state.position = grid[idx];									// random position
			p.state.heading.direction = (rand() % 360) / 180.0 * M_PI;		// random heading
			p.state.heading.error = 0;
            p.state.relativePressure = 0;                                   // start with a relative pressure of 0

		}
	}

};

/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {

	Grid<MyNode>& grid;

	GridWalker<MyNode, MyState> walker;

	WalkModuleHeading<MyNode, MyState> modHeadUgly;							// stupid
	WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
	WalkModuleNodeImportance<MyNode, MyState> modImportance;
	WalkModuleSpread<MyNode, MyState> modSpread;
	WalkModuleFavorZ<MyNode, MyState> modFavorZ;
    //WalkModulePreventVisited<MyNode, MyState> modPreventVisited;

    std::minstd_rand gen;


    PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {

        walker.addModule(&modHead);
        //walker.addModule(&modSpread);				// might help in some situations! keep in mind!

        //walker.addModule(&modHeadUgly);
        //walker.addModule(&modImportance);
        //walker.addModule(&modFavorZ);
        //walker.addModule(&modButterAct);
        //walker.addModule(&modWifi);
        //walker.addModule(&modPreventVisited);
	}

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

        std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);

		for (K::Particle<MyState>& p : particles) {

            // save old position
            p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);

            // update steps
            const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));

			// update the particle in-place
            p.state = walker.getDestination(grid, p.state, dist_m);

            // update the baromter
            float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
            p.state.relativePressure += deltaZ_cm * 0.105f;

		}
	}

};


struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {

    WiFiModelLogDistCeiling& wifiModel;
    //WiFiObserverFree wiFiProbability;             // free-calculation
    WiFiObserverGrid<MyNode> wiFiProbability;		// grid-calculation

    BeaconModelLogDistCeiling& beaconModel;
    BeaconObserverFree beaconProbability;

    Grid<MyNode>& grid;

    PFEval(WiFiModelLogDistCeiling& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
        wifiModel(wifiModel),
        beaconModel(beaconModel),
        grid(grid),
        wiFiProbability(Settings::WiFiModel::sigma),
        beaconProbability(Settings::BeaconModel::sigma, beaconModel){

	}

	/** probability for WIFI */
    inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {

       const MyNode& node = grid.getNodeFor(point);
       return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);

    }

    /** probability for BEACONS */
    inline double getBEACON(const MyObs& observation, const GridPoint& point){
        return beaconProbability.getProbability(point.inMeter(), observation.currentTime, observation.beacons);
    }

    /** probability for Barometer */
    inline double getBaroPressure(const MyObs& observation, const float hPa) const{
        return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
    }

	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {

		double sum = 0;
        const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);

		for (K::Particle<MyState>& p : particles) {

            // Point3 pos_m = p.state.position.inMeter();
            // Point3 posOld_m = p.state.positionOld.inMeter();

            const double pWifi = getWIFI(observation, wifiObs, p.state.position);
            const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);

            //small checks
            _assertNotNAN(pWifi, "pups");
            _assertNot0(pBaroPressure,"pBaroPressure is null");

            const double prob = pWifi*pBaroPressure;

            p.weight = prob;
            sum += (prob);

		}

        if(sum == 0.0){
            return 1.0;
        }

		return sum;

	}

};

#endif // FLOGIC_H