many changes

added new helper class for 3x3 matrices and vec3
added magnetometer data
added compass detection
refactored pose-estimation (single class)
refactored debug plots (move to own class)
minor changes

math/MovingMedianTS.h

@@ -0,0 +1,74 @@
+#ifndef MOVINGMEDIANTS_H
+#define MOVINGMEDIANTS_H
+
+#include <vector>
+#include <algorithm>
+#include "../data/Timestamp.h"
+
+template <typename T> class MovingMedianTS {
+
+private:
+
+	/** timestamp -> value combination */
+	struct Entry {
+		Timestamp ts;
+		T value;
+		Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;}
+	};
+
+	/** the regional window to use */
+	Timestamp window;
+
+	/** the value history for the window-size */
+	std::vector<Entry> history;
+
+
+
+
+public:
+
+	/** ctor with the window-size to use */
+	MovingMedianTS(const Timestamp window) : window(window) {
+
+	}
+
+	/** add a new value */
+	void add(const Timestamp ts, const T data) {
+
+		// append to history
+		history.push_back(Entry(ts, data));
+
+		// remove too-old history entries
+		const Timestamp oldest = ts - window;
+		while(history.front().ts < oldest) {
+
+			// remove from history
+			history.erase(history.begin());
+
+		}
+
+	}
+
+	/** get the current median */
+	T get() const {
+
+		const auto comp = [] (const Entry& a, const Entry& b) {
+			return a.value < b.value;
+		};
+
+		// create a sorted copy (slow, but works)
+		std::vector<Entry> copy = history;
+		std::sort(copy.begin(), copy.end(), comp);
+
+		// get the median
+		if (copy.size() % 2 != 0) {
+			return copy[(copy.size() + 0) / 2].value;
+		} else {
+			return (copy[copy.size() / 2 - 1].value + copy[copy.size() / 2 + 0].value) / 2;
+		}
+
+	}
+
+};
+
+#endif // MOVINGMEDIANTS_H