added code from fusion2016

code/particles/MyEvaluation.h

@@ -0,0 +1,89 @@
+#ifndef MYEVALUATION_H
+#define MYEVALUATION_H
+
+#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
+#include "MyObservation.h"
+#include "MyState.h"
+
+#include "../frank/WiFiEvaluation.h"
+#include "../frank/BeaconEvaluation.h"
+#include "../toni/BarometerEvaluation.h"
+#include "../lukas/StepEvaluation.h"
+#include "../lukas/TurnEvaluation.h"
+
+class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+private:
+
+	WiFiEvaluation wifiEval;
+	BeaconEvaluation beaconEval;
+	BarometerEvaluation barometerEval;
+	StepEvaluation stepEval;
+	TurnEvaluation turnEval;
+
+	bool useWifi = true;
+	bool useStep = true;
+	bool useTurn = true;
+	bool useBaro = true;
+	bool useIB = true;
+
+public:
+
+	void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
+		this->useWifi = useWifi;
+		this->useStep = useStep;
+		this->useTurn = useTurn;
+		this->useBaro = useBaro;
+		this->useIB = useIB;
+	}
+
+	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
+
+		//if (observation.wifi) {
+		wifiEval.nextObservation(observation.wifi);
+		//}
+
+		// evalulate each particle
+		double sum = 0;
+		for (K::Particle<MyState>& p : particles) {
+
+			double weight = 1.0;
+
+			if (useWifi) {
+				weight *= wifiEval.getProbability(p.state, observation);
+			}
+
+			if (useBaro && observation.barometer) {
+				weight *= barometerEval.getProbability(p.state, observation.barometer);
+			}
+
+			if (useIB) {
+				weight *= beaconEval.getProbability(p.state, observation);
+			}
+
+			if (useStep) {
+				weight *= stepEval.getProbability(p.state, observation.step);
+			}
+
+			if (useTurn) {
+				weight *= turnEval.getProbability(p.state, observation.turn, true);
+			}
+
+			// set and accumulate
+            p.weight = weight;
+			sum += p.weight;
+
+		}
+
+		// reset
+		observation.step->steps = 0;
+		observation.turn->delta_heading = 0;
+		observation.turn->delta_motion = 0;
+
+		return sum;
+
+	}
+
+};
+
+#endif // MYEVALUATION_H