added code from fusion2016

2016-03-01 15:04:46 +01:00
commit 8d2be0f8a0
97 changed files with 19831 additions and 0 deletions
--- a/code/particles/MyTransitionSimple.h
+++ b/code/particles/MyTransitionSimple.h
@@ -0,0 +1,83 @@
+#ifndef MYTRANSITIONSIMPLE_H
+#define MYTRANSITIONSIMPLE_H
+
+#include <KLib/math/filter/particles/ParticleFilterTransition.h>
+#include <KLib/math/distribution/Normal.h>
+#include "MyState.h"
+#include "MyControl.h"
+
+class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
+
+private:
+
+	/** a simple normal distribution */
+	K::NormalDistribution distX;
+	K::NormalDistribution distY;
+	K::NormalDistribution distZ;
+	K::NormalDistribution distBaro;
+
+public:
+
+	/** ctor */
+	MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
+		distX.setSeed(1234);
+		distY.setSeed(1235);
+		distZ.setSeed(1236);
+		distBaro.setSeed(5678);
+	}
+
+public:
+
+	uint64_t ts = 0;
+	uint64_t deltaMS = 0;
+
+	/** set the current time in millisconds */
+	void setCurrentTime(const uint64_t ts) {
+		if (this->ts == 0) {
+			this->ts = ts;
+			deltaMS = 0;
+		} else {
+			deltaMS = ts - this->ts;
+			this->ts = ts;
+		}
+	}
+
+	virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		for (K::Particle<MyState>& p : particles) {
+
+            p.state.heading_old = p.state.heading;
+            p.state.x_cm_old = p.state.x_cm;
+            p.state.y_cm_old = p.state.y_cm;
+            p.state.z_nr_old = p.state.z_nr;
+
+			p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
+			p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
+			p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
+			p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old,  p.state.x_cm, p.state.y_cm);
+
+//            if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
+//            if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
+//            if (p.state.x_cm < 0) {p.state.x_cm = 0;}
+//            if (p.state.y_cm < 0) {p.state.y_cm = 0;}
+
+
+
+            //update barometer
+			p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
+
+			// update walked distance (2D)
+			const double dx = p.state.x_cm_old - p.state.x_cm;
+			const double dy = p.state.y_cm_old - p.state.y_cm;
+			p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
+
+
+		}
+
+	}
+
+
+};
+
+
+#endif // MYTRANSITIONSIMPLE_H