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

sensors/imu/MagnetometerData.h

@@ -0,0 +1,81 @@
+#ifndef INDOOR_IMU_MAGNETOMETERDATA_H
+#define INDOOR_IMU_MAGNETOMETERDATA_H
+
+
+#include <cmath>
+#include <sstream>
+
+/**
+ * data received from a magnetometer sensor
+ */
+struct MagnetometerData {
+
+	float x;
+	float y;
+	float z;
+
+	MagnetometerData() : x(0), y(0), z(0) {;}
+
+	/** ctor from RADIANS */
+	MagnetometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+	std::string asString() const {
+		std::stringstream ss;
+		ss << "(" << x << "," << y << "," << z << ")";
+		return ss.str();
+	}
+
+	bool isValid() const {
+		return (x == x) && (y == y) && (z == z);
+	}
+
+	bool operator == (const GyroscopeData& o ) const {
+		return	EQ_OR_NAN(x, o.x) &&
+				EQ_OR_NAN(y, o.y) &&
+				EQ_OR_NAN(z, o.z);
+	}
+
+	float magnitude() const {
+		return std::sqrt( x*x + y*y + z*z );
+	}
+
+	MagnetometerData& operator += (const MagnetometerData& o) {
+		this->x += o.x;
+		this->y += o.y;
+		this->z += o.z;
+		return *this;
+	}
+
+	MagnetometerData& operator -= (const MagnetometerData& o) {
+		this->x -= o.x;
+		this->y -= o.y;
+		this->z -= o.z;
+		return *this;
+	}
+
+	MagnetometerData operator * (const MagnetometerData& o) const {
+		return MagnetometerData(x*o.x, y*o.y, z*o.z);
+	}
+
+	MagnetometerData operator - (const MagnetometerData& o) const {
+		return MagnetometerData(x-o.x, y-o.y, z-o.z);
+	}
+
+	MagnetometerData operator / (const float val) const {
+		return MagnetometerData(x/val, y/val, z/val);
+	}
+
+
+private:
+
+	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+
+};
+
+namespace std {
+	MagnetometerData sqrt(const MagnetometerData& o) {
+		return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
+	}
+}
+
+#endif // INDOOR_IMU_MAGNETOMETERDATA_H