fixed some issues

added new pose/turn detections new helper classes define-flags for libEigen
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
--- a/sensors/gps/GPSData.h
+++ b/sensors/gps/GPSData.h
@@ -3,18 +3,19 @@

 #include "../../data/Timestamp.h"
 #include "../../geo/EarthPos.h"
+#include "../../math/Floatingpoint.h"

 struct GPSData {

 	/** time this measurement was received (NOT the GPS-time) */
 	Timestamp tsReceived;

-	float lat;			// deg
-	float lon;			// deg
-	float alt;			// m above sea-level
+	FPDefault lat;			// deg
+	FPDefault lon;			// deg
+	FPDefault alt;			// m above sea-level

-	float accuracy;		// m [might be NAN]
-	float speed;		// m/s [might be NAN]
+	FPDefault accuracy;		// m [might be NAN]
+	FPDefault speed;		// m/s [might be NAN]

 	/** ctor for invalid/unknown data */
 	GPSData() : tsReceived(), lat(NAN), lon(NAN), alt(NAN), accuracy(NAN), speed(NAN) {;}
@@ -49,7 +50,7 @@ struct GPSData {

 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/AccelerometerData.h
+++ b/sensors/imu/AccelerometerData.h
@@ -3,20 +3,23 @@

 #include <cmath>
 #include <sstream>
+#include "../../math/Floatingpoint.h"


 /** data received from an accelerometer sensor */
 struct AccelerometerData {

-	float x;
-	float y;
-	float z;
+	FPDefault x;
+	FPDefault y;
+	FPDefault z;

 	AccelerometerData() : x(0), y(0), z(0) {;}

-	AccelerometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+	AccelerometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;}

-	float magnitude() const {
+	//AccelerometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;}
+
+	FPDefault magnitude() const {
 		return std::sqrt( x*x + y*y + z*z );
 	}

@@ -34,22 +37,24 @@ struct AccelerometerData {
 		return *this;
 	}

-	AccelerometerData operator - (const AccelerometerData& o) const {
-		return AccelerometerData(x-o.x, y-o.y, z-o.z);
+
+	AccelerometerData operator + (const AccelerometerData o) const {
+		return AccelerometerData(x+o.x, y+o.y, z+o.z);
 	}

-	AccelerometerData operator / (const float val) const {
-		return AccelerometerData(x/val, y/val, z/val);
+	AccelerometerData operator - (const AccelerometerData& o) const {
+		return AccelerometerData(x-o.x, y-o.y, z-o.z);
 	}

 	AccelerometerData operator * (const float val) const {
 		return AccelerometerData(x*val, y*val, z*val);
 	}

-	AccelerometerData operator + (const AccelerometerData o) const {
-		return AccelerometerData(x+o.x, y+o.y, z+o.z);
+	AccelerometerData operator / (const float val) const {
+		return AccelerometerData(x/val, y/val, z/val);
 	}

+
 	std::string asString() const {
 		std::stringstream ss;
 		ss << "(" << x << "," << y << "," << z << ")";
@@ -68,7 +73,7 @@ struct AccelerometerData {

 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/CompassData.h
+++ b/sensors/imu/CompassData.h
@@ -4,16 +4,16 @@

 #include <cmath>
 #include <sstream>
-
+#include "../../math/Floatingpoint.h"

 /** data received from a compass sensor */
 struct CompassData {

 	/** azimuth angle. NAN if not available */
-	float azimuth = NAN;
+	FPDefault azimuth = NAN;

 	/** describes the sensor's quality */
-	float quality01 = 0;
+	FPDefault quality01 = 0;


 	/** empty ctor */
@@ -49,7 +49,7 @@ struct CompassData {

 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/CompassDetection.h
+++ b/sensors/imu/CompassDetection.h
@@ -2,18 +2,21 @@
 #define INDOOR_IMU_COMPASSDETECTION_H

 #include "MagnetometerData.h"
-#include "PoseDetection.h"
+#include "PoseProvider.h"
+#include "TurnProvider.h"

 #include "../../data/Timestamp.h"
 #include "../../math/MovingAverageTS.h"
 #include "../../math/MovingStdDevTS.h"
 #include "../../geo/Point3.h"
 #include "../../Assertions.h"
+#include "../../geo/Angle.h"

 #include "CompassDetectionPlot.h"

 #include <cmath>
 #include <vector>
+#include "../../math/dsp/iir/BiQuad.h"


 /**
@@ -33,21 +36,37 @@ private:
 //		Timestamp lastEstimation;
 //	} orientation;

-	MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
+	MovingAverageTS<MagnetometerData> avgIn;

 	//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
 	MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);

-	PoseDetection* pose = nullptr;
+
+
+	const PoseProvider* pose = nullptr;
+	const TurnProvider* turn = nullptr;
+
 	int numMagReadings = 0;

+	bool stable;
+
+	MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
+	float lastHeading = 0;
+	float curSigma = 0;
+
 public:

 	/** ctor */
-	CompassDetection(PoseDetection* pose) : pose(pose) {
+	CompassDetection(const PoseProvider* pose, const TurnProvider* turn, const Timestamp avgFrame = Timestamp::fromMS(500)) :
+	    pose(pose), turn(turn), avgIn(avgFrame, MagnetometerData(0,0,0)) {
 		;
 	}

+	/** get the current uncertainty estimation */
+	float getSigma() {
+		return curSigma + pose->getSigma() + turn->getSigma();
+	}
+
 	/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
 	float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {

@@ -73,30 +92,38 @@ public:

 		// calculate angle
 		// https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-		const float mx = curMag.x;
-		const float my = curMag.y;
+		const float mx = (curMag.x);
+		const float my = (curMag.y);
 		const float tmp = std::atan2(my, mx);
 		//const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my));

 		// http://www.magnetic-declination.com/
 		// http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html
 		const float declination = 3.0f / 180.0f * M_PI;		// GERMANY!
-		const float curHeading = - tmp + declination;
+		//const float declination = 0;		// GERMANY!
+		float curHeading = Angle::makeSafe_2PI(- tmp + declination);
+
 		float resHeading;
-		bool stable = true;
+		stable = true;

 		// calculate standard-deviation within a certain timeframe
-		stdDev.add(ts, curHeading);
+		const float curDiff = Angle::getSignedDiffRAD_2PI(curHeading, lastHeading);
+		stdDev.add(ts, curDiff);
+		stdDevForSigma.add(ts, curDiff);
+		curSigma = (5.0f/180.0f*(float)M_PI) + stdDevForSigma.get()*10;
+		lastHeading = curHeading;
+
+

 		// if the standard-deviation is too high,
 		// the compass is considered unstable
-		if (numMagReadings < 250 || stdDev.get() > 0.30) {
-			resHeading = NAN;
-			stable = false;
-		} else {
+//		if (numMagReadings < 250 || stdDev.get() > 0.30) {
+//			resHeading = NAN;
+//			stable = false;
+//		} else {
 			resHeading = curHeading;
 			stable = true;
-		}
+//		}

 		#ifdef WITH_DEBUG_PLOT
 			plot.add(ts, curHeading, stable, mag, curMag);
@@ -107,6 +134,7 @@ public:

 	}

+
 };

 #endif // INDOOR_IMU_COMPASSDETECTION_H
--- a/sensors/imu/CompassDetectionPlot.h
+++ b/sensors/imu/CompassDetectionPlot.h
@@ -22,7 +22,7 @@

 		K::Gnuplot gp1;
 		K::Gnuplot gp2;
-		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
+		//K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);

 		K::GnuplotPlot plotMagRaw;
 		K::GnuplotPlotElementLines lineMagRawX;
@@ -47,8 +47,8 @@
 			gp1 << "set autoscale xfix\n";
 			gp2 << "set size ratio -1\n";

-			multiplot.add(&plotMagRaw);
-			multiplot.add(&plotMagFix);
+			//multiplot.add(&plotMagRaw);
+			//multiplot.add(&plotMagFix);

 			plotMagRaw.setTitle("Magnetometer (raw)");
 			plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x");
@@ -106,7 +106,7 @@
 				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
 				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";

-				gp1.draw(multiplot);
+				gp1.draw(plotMagFix);
 				gp1.flush();

 				gp2.draw(plotMagScatter);
--- a/sensors/imu/GravityData.h
+++ b/sensors/imu/GravityData.h
@@ -3,20 +3,20 @@

 #include <cmath>
 #include <sstream>
-
+#include "../../math/Floatingpoint.h"

 /** data received from an accelerometer sensor */
 struct GravityData {

-	float x;
-	float y;
-	float z;
+	FPDefault x;
+	FPDefault y;
+	FPDefault z;

    GravityData() : x(0), y(0), z(0) {;}

    GravityData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}

-	float magnitude() const {
+	FPDefault magnitude() const {
 		return std::sqrt( x*x + y*y + z*z );
 	}

@@ -38,7 +38,7 @@ struct GravityData {
        return GravityData(x-o.x, y-o.y, z-o.z);
 	}

-    GravityData operator / (const float val) const {
+	GravityData operator / (const FPDefault val) const {
        return GravityData(x/val, y/val, z/val);
 	}

@@ -60,7 +60,7 @@ struct GravityData {

 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/GyroscopeData.h
+++ b/sensors/imu/GyroscopeData.h
@@ -3,6 +3,7 @@

 #include <cmath>
 #include <sstream>
+#include "../../math/Floatingpoint.h"

 /**
 * data received from a gyroscope sensor
@@ -10,14 +11,14 @@
 */
 struct GyroscopeData {

-	float x;
-	float y;
-	float z;
+	FPDefault x;
+	FPDefault y;
+	FPDefault z;

 	GyroscopeData() : x(0), y(0), z(0) {;}

 	/** ctor from RADIANS */
-	GyroscopeData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+	GyroscopeData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;}

 	float magnitude() const {
 		return std::sqrt( x*x + y*y + z*z );
@@ -41,7 +42,7 @@ struct GyroscopeData {

 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/MagnetometerData.h
+++ b/sensors/imu/MagnetometerData.h
@@ -4,20 +4,21 @@

 #include <cmath>
 #include <sstream>
+#include "../../math/Floatingpoint.h"

 /**
 * data received from a magnetometer sensor
 */
 struct MagnetometerData {

-	float x;
-	float y;
-	float z;
+	FPDefault x;
+	FPDefault y;
+	FPDefault 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) {;}
+	MagnetometerData(const FPDefault x, const FPDefault y, const FPDefault z) : x(x), y(y), z(z) {;}

 	std::string asString() const {
 		std::stringstream ss;
@@ -29,7 +30,7 @@ struct MagnetometerData {
 		return (x == x) && (y == y) && (z == z);
 	}

-	bool operator == (const GyroscopeData& o ) const {
+	bool operator == (const MagnetometerData& o ) const {
 		return	EQ_OR_NAN(x, o.x) &&
 				EQ_OR_NAN(y, o.y) &&
 				EQ_OR_NAN(z, o.z);
@@ -53,22 +54,27 @@ struct MagnetometerData {
 		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 MagnetometerData& o) const {
 		return MagnetometerData(x-o.x, y-o.y, z-o.z);
 	}

-	MagnetometerData operator / (const float val) const {
+	MagnetometerData operator * (const MagnetometerData& o) const {
+		return MagnetometerData(x*o.x, y*o.y, z*o.z);
+	}
+
+	MagnetometerData operator / (const FPDefault 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) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };

--- a/sensors/imu/PoseDetection.h
+++ b/sensors/imu/PoseDetection.h
@@ -5,6 +5,7 @@

 #include "../../data/Timestamp.h"

+#include "../../math/MovingStdDevTS.h"
 #include "../../math/MovingAverageTS.h"
 #include "../../math/MovingMedianTS.h"
 #include "../../math/Matrix3.h"
@@ -14,12 +15,13 @@
 //#include <eigen3/Eigen/Dense>

 #include "PoseDetectionPlot.h"
+#include "PoseProvider.h"

 /**
 * estimate the smartphones world-pose,
 * based on the accelerometer's data
 */
-class PoseDetection {
+class PoseDetection : public PoseProvider {


 	/** live-pose-estimation using moving average of the smartphone's accelerometer */
@@ -29,7 +31,7 @@ class PoseDetection {
 		MovingAverageTS<AccelerometerData> avg;

 		EstMovingAverage(const Timestamp window) :
-			avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {
+		    avg(window, AccelerometerData()) {

 			// start approximately
 			addAcc(Timestamp(), AccelerometerData(0,0,9.81));
@@ -45,26 +47,82 @@ class PoseDetection {
 			return avg.get();
 		}

+//		/** get the current rotation matrix estimation */
+//		//Eigen::Matrix3f get() const {
+//		Matrix3 get() const {
+
+//			// get the current acceleromter average
+//			const AccelerometerData avgAcc = avg.get();
+//			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
+//			const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);
+
+//			// rotate average-accelerometer into (0,0,1)
+//			//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+//			const Vector3 zAxis(0,0,1);
+//			const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
+//			//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
+//			//const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+
+//			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
+//			//Eigen::Vector3f aligned = (rotMat * avg).normalized();
+//			const Vector3 aligned = (rotMat * avg).normalized();
+//			Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
+//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+//			return rotMat;
+
+//		}
+
+		// FOR TESTING
 		/** get the current rotation matrix estimation */
 		//Eigen::Matrix3f get() const {
 		Matrix3 get() const {

+			// https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
 			// get the current acceleromter average
 			const AccelerometerData avgAcc = avg.get();
 			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
-			const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);
+			const Point3 vZ = Point3(avgAcc.x, avgAcc.y, avgAcc.z).normalized();
+			const Point3 vX = cross(Point3(0,1,0), vZ).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
+			//const Point3 v2 = cross(v3, vx).normalized();
+			const Point3 vY = cross(vZ, vX).normalized();

-			// rotate average-accelerometer into (0,0,1)
-			//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
-			const Vector3 zAxis(0,0,1);
-			const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
-			//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
-			//const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+			Matrix3 rotMat({
+			    vX.x, vY.x, vZ.x,
+			    vX.y, vY.y, vZ.y,
+			    vX.z, vY.z, vZ.z,
+			});
+
+			// above transposed = inverse matrix = undo rotation
+			rotMat = rotMat.transposed();
+
+//			// https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
+//			// get the current acceleromter average
+//			const AccelerometerData avgAcc = avg.get();
+//			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
+//			const Point3 vZ = Point3(-avgAcc.x, -avgAcc.y, -avgAcc.z).normalized();
+//			const Point3 vX = cross(vZ, Point3(0,1,0)).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
+//			//const Point3 v2 = cross(v3, vx).normalized();
+//			const Point3 vY = cross(vX, vZ).normalized();
+
+//			Matrix3 rotMat({
+//			    vX.x, vY.x, vZ.x,
+//			    vX.y, vY.y, vZ.y,
+//			    vX.z, vY.z, vZ.z,
+//			});
+
+//			//rotMat = Matrix3::getRotationDeg(180, 0, 0) * rotMat;
+//			//rotMat = Matrix3::getRotationDeg(0, 0, 180) * rotMat;
+
+//			// above transposed = inverse matrix = undo rotation
+//			//rotMat = rotMat.transposed();

 			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
-			//Eigen::Vector3f aligned = (rotMat * avg).normalized();
-			const Vector3 aligned = (rotMat * avg).normalized();
-			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+			const Vector3 zAxis(0,0,1);
+			const Vector3 inp(avgAcc.x, avgAcc.y, avgAcc.z);
+			const Vector3 aligned = (rotMat * inp).normalized();
+			Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
+			//Assert::isTrue((aligned-zAxis).norm() < 0.10f, "deviation too high");

 			return rotMat;

@@ -120,22 +178,24 @@ class PoseDetection {
 //	};


-
 private:

 	struct {
-		//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
 		Matrix3 rotationMatrix = Matrix3::identity();
+		float curSigma = 0;
 		bool isKnown = false;
 		Timestamp lastEstimation;
 	} orientation;

 	/** how the pose is estimated */
 	//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
-	EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450));
+	EstMovingAverage est;
 	//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));

+	MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
+
 	#ifdef WITH_DEBUG_PLOT
+	    int plotLimit = 0;
 		PoseDetectionPlot plot;
 	#endif

@@ -143,22 +203,46 @@ private:
 public:

 	/** ctor */
-	PoseDetection() {
-		;
+	PoseDetection(const Timestamp delay = Timestamp::fromMS(450)) : est(delay) {
+        #ifdef WITH_DEBUG_PLOT
+		    plot.setName("PoseDetection1");
+        #endif
 	}

 	/** get the smartphone's rotation matrix */
-	const Matrix3& getMatrix() const {
+	const Matrix3& getMatrix() const override {
 		return orientation.rotationMatrix;
 	}

 	/** is the pose known and stable? */
-	bool isKnown() const {
+	bool isKnown() const override {
 		return orientation.isKnown;
 	}

+	Matrix3 getMatrixGyro() const {
+		return Matrix3::identity();
+	}
+
+	Matrix3 getMatrixAcc() const {
+		return orientation.rotationMatrix;
+	}
+
+	/** current uncertainty */
+	float getSigma() const {
+		return orientation.curSigma;
+	}
+
+
 	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {

+		// uncertainty
+		const Vector3 curAcc = Vector3(acc.x, acc.y, acc.z);
+		float angleDiff = std::acos(curAcc.normalized().dot(Vector3(0,0,1)));
+		if (!std::isnan(angleDiff)) {
+			stdDevForSigma.add(ts, angleDiff);
+			orientation.curSigma = stdDevForSigma.get()*1;
+		}
+
 		// add accelerometer data
 		est.addAcc(ts, acc);

@@ -169,7 +253,10 @@ public:

 		// debug-plot (if configured)
 		#ifdef WITH_DEBUG_PLOT
-			plot.add(ts, est.getBase(), orientation.rotationMatrix);
+		    if (++plotLimit > 5) {
+				plot.add(ts, est.getBase(), orientation.rotationMatrix);
+				plotLimit = 0;
+			}
 		#endif

 	}
--- a/sensors/imu/PoseDetection2.h
+++ b/sensors/imu/PoseDetection2.h
@@ -0,0 +1,301 @@
+#ifndef POSEDETECTION2_H
+#define POSEDETECTION2_H
+
+
+#include "AccelerometerData.h"
+#include "GyroscopeData.h"
+
+#include "../../data/Timestamp.h"
+
+//#include "../../math/MovingAverageTS.h"
+//#include "../../math/MovingMedianTS.h"
+#include "../../math/Matrix3.h"
+
+#include "../../math/filter/Complementary.h"
+
+#include "../../geo/Point3.h"
+
+//#include "../../math/FixedFrequencyInterpolator.h"
+
+#include "PoseDetectionPlot.h"
+#include "PoseProvider.h"
+
+/**
+ * estimate the smartphones world-pose,
+ * based on the accelerometer's data
+ *
+ * https://robotics.stackexchange.com/questions/6953/how-to-calculate-euler-angles-from-gyroscope-output
+ */
+class PoseDetection2 : public PoseProvider {
+
+private:
+
+	struct {
+		//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
+		Matrix3 rotationMatrix = Matrix3::identity();
+		bool isKnown = false;
+		Timestamp lastEstimation;
+	} orientation;
+
+	/** how the pose is estimated */
+	//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
+	//EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450));
+	//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));
+
+	#ifdef WITH_DEBUG_PLOT
+		PoseDetectionPlot plot;
+		PoseDetectionPlotAngles plotAngles;
+	#endif
+
+	Vector3 lastGyroReading;
+	Timestamp lastGyroReadingTS;
+
+	//FixedFrequencyInterpolator<Vector3> interpolAcc;
+	//FixedFrequencyInterpolator<Vector3> interpolGyro;
+
+	Filter::Complementary<Vector3> filter;
+
+	//std::vector<Vector3> accBuf;
+	//std::vector<Vector3> gyroBuf;
+
+
+
+	Vector3 curAccel;
+
+	bool firstAccel = true;
+	Vector3 curAccel_rad;
+	Vector3 curGyroSum_rad;
+
+	//Matrix3 curGyroMatrix = Matrix3::identity();
+
+	static constexpr float splitFreq = 2.5;
+	static constexpr float sRate = 50;
+	static constexpr int sRateTS = 1000/sRate;
+
+
+public:
+
+	/** ctor */
+	//PoseDetection2() : interpolAcc(Timestamp::fromMS(sRateTS)), interpolGyro(Timestamp::fromMS(sRateTS)), filter(splitFreq, sRate) {
+	PoseDetection2() : filter(1, 2.5) {
+        #ifdef WITH_DEBUG_PLOT
+		    plot.setName("PoseDetection2");
+        #endif
+	}
+
+	/** get the smartphone's rotation matrix */
+	const Matrix3& getMatrix() const override {
+		return orientation.rotationMatrix;
+	}
+
+	/** is the pose known and stable? */
+	bool isKnown() const override {
+		return orientation.isKnown;
+	}
+
+
+	float angleX_old_rad = 0;
+	float angleY_old_rad = 0;
+
+
+	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
+
+		// https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/#docs
+
+		float angleX_rad = std::atan2( acc.y, acc.z);
+		float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
+		float angleZ_rad = 0;
+
+		if (std::abs(angleX_rad-angleX_old_rad) > M_PI) {angleX_rad += 2*M_PI;}
+		if (std::abs(angleY_rad-angleY_old_rad) > M_PI) {angleY_rad += 2*M_PI;}
+
+		angleX_old_rad = angleX_rad;
+		angleY_old_rad = angleY_rad;
+
+//		float angleX_rad = std::atan2(acc.y, std::sqrt(acc.x*acc.x + acc.z*acc.z));
+//		float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
+//		float angleZ_rad = 0;
+
+//		Point2 ref(0,1);
+//		Point2 xz( acc.x, acc.z);
+//		Point2 yz(-acc.y, acc.z);
+//		float angleY_rad = std::atan2( determinant(ref,xz), dot(ref,xz) );
+//		float angleX_rad = std::atan2( determinant(ref,yz), dot(ref,yz) );
+//		float angleZ_rad = 0.0;
+
+//		float angleX_rad =  std::atan2(acc.y, acc.z);
+//		float angleY_rad = -std::atan2(acc.x, acc.z);
+//		float angleZ_rad = 0;//-std::atan2(acc.y, acc.x);
+
+
+		//		float angleX2_rad = (angleX1_rad > 0) ? (angleX1_rad - 2*M_PI) : (angleX1_rad + 2*M_PI);
+//		float angleY2_rad = (angleY1_rad > 0) ? (angleY1_rad - 2*M_PI) : (angleY1_rad + 2*M_PI);
+//		float angleX_rad = (std::abs(curAngle_rad.x-angleX1_rad) < std::abs(curAccel_rad.x-angleX2_rad)) ? (angleX1_rad) : (angleX2_rad);
+//		float angleY_rad = (std::abs(curAngle_rad.y-angleY1_rad) < std::abs(curAccel_rad.y-angleY2_rad)) ? (angleY1_rad) : (angleY2_rad);
+
+		curAccel = Vector3(acc.x, acc.y, acc.z);
+		curAccel_rad = Vector3(angleX_rad, angleY_rad, angleZ_rad);
+
+		//curAccel_rad = Matrix3::getRotationRad(0,0,-angleZ_rad) * Vector3(angleX_rad, angleY_rad, angleZ_rad);
+
+
+		if (firstAccel) {
+			curGyroSum_rad = curAccel_rad;
+			firstAccel = false;
+		}
+
+
+
+	}
+
+
+
+
+	void addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
+
+		Vector3 vec(gyro.x, gyro.y, gyro.z);
+
+		// ignore the first reading completely, just remember its timestamp
+		if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return;}
+
+		// time-difference between previous and current reading
+		const Timestamp curDiff = ts - lastGyroReadingTS;
+		lastGyroReadingTS = ts;
+
+		// fast sensors might lead to delay = 0 ms. filter those values
+		if (curDiff.isZero()) {return;}
+
+		// current area
+		const Vector3 curArea =	(lastGyroReading * curDiff.sec());
+
+//		// update sum
+//		curAngle_rad += curArea;
+//		curGyroSum_rad += curArea;
+
+//		// PAPER
+		const float dx = 1 * lastGyroReading.x +	std::sin(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.y +	std::cos(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
+		const float dy =							std::cos(curGyroSum_rad.x)*lastGyroReading.y +															-std::sin(curGyroSum_rad.x)*lastGyroReading.z;
+		const float dz =							std::sin(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.y +								std::cos(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
+		curGyroSum_rad.x += dx*curDiff.sec();
+		curGyroSum_rad.y += dy*curDiff.sec();
+		curGyroSum_rad.z += dz*curDiff.sec();
+
+
+//		// PAPER
+//		const Vector3 n = lastGyroReading / lastGyroReading.norm();
+//		const float mag = lastGyroReading.norm() * curDiff.sec();
+//		if (mag != 0) {
+//			curGyroMatrix = Matrix3::getRotationVec(n.x, n.y, n.z, mag).transposed() * curGyroMatrix;
+//		}
+
+		// DEBUG PLOT
+
+
+		// update old reading
+		lastGyroReading = vec;
+
+		update(ts);
+
+
+
+	}
+
+	Matrix3 getMatrixGyro() const {
+		return Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
+	}
+
+	Matrix3 getMatrixAcc() const {
+		return Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z);
+	}
+
+private:
+
+	Vector3 curAngle_rad;
+	int cnt = 0;
+
+	void update(const Timestamp ts) {
+
+
+		// complementary filter		x = alpha * x + (1-alpha) * y;
+		const float alpha = 0.98f;
+		//curAngle_rad = curAngle_rad*alpha + curAccel_rad*(1-alpha);
+		//curAngle_rad = curAccel_rad;
+		//curAngle_rad = curGyroSum_rad;
+
+		//std::cout << curGyroSum_rad.x <<" " << curGyroSum_rad.y << " " << curGyroSum_rad.z << std::endl;
+
+		//curAngle_rad = filter.get();
+
+		filter.addSlow(ts, curAccel_rad);
+		filter.addFast(ts, curGyroSum_rad);
+
+		static Vector3 fused;
+		static Vector3 pref;
+		Vector3 diff = curGyroSum_rad - pref;
+		fused = (fused+diff) * alpha + curAccel_rad * (1-alpha);
+		pref = curGyroSum_rad;
+
+		// update
+		//orientation.rotationMatrix = Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z); //getMatrixFor(cur);
+		//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); //getMatrixFor(cur);
+		//orientation.rotationMatrix = curGyroMatrix;
+		//orientation.rotationMatrix = Matrix3::getRotationRadZ(curGyroSum_rad.z) * Matrix3::getRotationRadX(curGyroSum_rad.x) * Matrix3::getRotationRadY(curGyroSum_rad.y);
+		//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
+		orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, 0); //getMatrixFor(cur);
+
+
+		orientation.isKnown = true;
+		orientation.lastEstimation = ts;
+
+		// debug-plot (if configured)
+        #ifdef WITH_DEBUG_PLOT
+		    plot.add(ts, curAccel, orientation.rotationMatrix);
+			if (++cnt % 2 == 0) {
+				plotAngles.addAcc(ts, curAccel_rad.x, curAccel_rad.y);
+				plotAngles.addGyro(ts, curGyroSum_rad.x, curGyroSum_rad.y);
+				plotAngles.addFused(ts, fused.x, fused.y);
+			}
+        #endif
+
+	}
+
+//	/** get the current rotation matrix estimation */
+//	Matrix3 getMatrixFor(const Vector3 cur) const {
+
+//		// rotate average-accelerometer into (0,0,1)
+//		//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+//		const Vector3 zAxis(0,0,1);
+//		const Matrix3 rotMat = getRotationMatrix(cur.normalized(), zAxis);
+//		//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
+//		//const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
+
+//		// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
+//		//Eigen::Vector3f aligned = (rotMat * avg).normalized();
+//		const Vector3 aligned = (rotMat * cur).normalized();
+//		Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+//		return rotMat;
+
+//	}
+
+//	/** get a matrix that rotates the vector "from" into the vector "to" */
+//	static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {
+
+//		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
+
+//		const Vector3 v = from.cross(to) / from.cross(to).norm();
+//		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
+
+//		Matrix3 A({
+//			0.0f,	-v.z,	v.y,
+//			v.z,	0.0f,	-v.x,
+//			-v.y,	v.x,	0.0f
+//		});
+
+//		return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));
+
+//	}
+
+};
+
+#endif // POSEDETECTION2_H
--- a/sensors/imu/PoseDetection3.h
+++ b/sensors/imu/PoseDetection3.h
@@ -0,0 +1,179 @@
+#ifndef POSEDETECTION3_H
+#define POSEDETECTION3_H
+
+#include "AccelerometerData.h"
+#include "GyroscopeData.h"
+
+#include "../../data/Timestamp.h"
+#include "../../math/Matrix3.h"
+#include "../../math/Quaternion.h"
+
+#include "../../geo/Point3.h"
+
+#include "PoseDetectionPlot.h"
+#include "PoseProvider.h"
+
+
+class PoseDetection3 : public PoseProvider {
+
+private:
+
+	struct {
+		//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
+		Matrix3 rotationMatrix = Matrix3::identity();
+		bool isKnown = false;
+		Timestamp lastEstimation;
+	} orientation;
+
+	#ifdef WITH_DEBUG_PLOT
+		PoseDetectionPlot plot;
+		PoseDetectionPlotAngles plotAngles;
+	#endif
+
+	Vector3 lastGyroReading;
+	Timestamp lastGyroReadingTS;
+
+public:
+
+	/** ctor */
+	PoseDetection3() {
+		#ifdef WITH_DEBUG_PLOT
+			plot.setName("PoseDetection3");
+		#endif
+	}
+
+	/** get the smartphone's rotation matrix */
+	const Matrix3& getMatrix() const override {
+		return orientation.rotationMatrix;
+	}
+
+	/** is the pose known and stable? */
+	bool isKnown() const override {
+		return orientation.isKnown;
+	}
+
+
+	bool first = true;
+
+	Quaternion qGyro = Quaternion(1, 0,0,0);
+	Quaternion qGyroUpdate;
+	Quaternion qAccel;
+	Quaternion qFiltered;
+
+	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
+
+		const Vector3 vec(-acc.x, -acc.y, acc.z);
+		const Vector3 v = vec.normalized();
+
+
+		const float mag = -std::acos(v.dot(Vector3(0,0,1)));//-std::acos(v.z); << same
+		const Vector3 n = Vector3(v.y, -v.x, 0).normalized();
+		qAccel = Quaternion::fromAxisAngle(mag, n.x, n.y, n.z);
+
+		//const float magY = std::atan2(acc.x, acc.z);
+		//const float magX = std::atan2(acc.y, acc.z);
+		//qAccel = Quaternion::fromAxisAngle(magX, 1, 0, 0) * Quaternion::fromAxisAngle(-magY, 0, 1, 0);
+
+
+
+		//Vector3 dv = qAccel.toEuler();
+		//dv.z = 0;
+		//std::cout << dv.x << " " << dv.y << " " << dv.z << std::endl;
+
+		// for plotting
+		if (first) {
+			qGyro = qAccel;
+			first = false;
+		}
+
+		int i = 0; (void) i;
+
+	}
+
+
+
+
+	void addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
+
+		Vector3 vec(gyro.x, gyro.y, gyro.z);
+
+		// ignore the first reading completely, just remember its timestamp
+		if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return;}
+
+		// time-difference between previous and current reading
+		const Timestamp curDiff = ts - lastGyroReadingTS;
+		lastGyroReadingTS = ts;
+
+		// fast sensors might lead to delay = 0 ms. filter those values
+		if (curDiff.isZero()) {return;}
+
+		// current area
+		//const Vector3 curArea =	(lastGyroReading * curDiff.sec());
+
+
+		// length of the rotation vector
+		const float theta =  curDiff.sec() * lastGyroReading.norm();
+		const Vector3 v = lastGyroReading.normalized();
+		lastGyroReading = vec;
+
+		if (theta == 0) {return;}
+		qGyroUpdate = Quaternion::fromAxisAngle(theta, v.x, v.y, v.z);// qUpdate(cos(theta/2),		v.x * sin(theta/2), v.y * sin(theta/2), v.z * sin(theta/2));
+
+		// update
+		qGyro = qGyroUpdate * qGyro;
+
+
+
+		update(ts);
+
+	}
+
+	Matrix3 getMatrixAcc() const {
+		Vector3 r = qAccel.toEuler();
+		return Matrix3::getRotationRad(r.x, r.y, r.z);
+	}
+	Matrix3 getMatrixGyro() const {
+		Vector3 r = qGyro.toEuler();
+		return Matrix3::getRotationRad(r.x, r.y, r.z);
+	}
+
+private:
+
+	Vector3 curAngle_rad;
+	int cnt = 0;
+
+	void update(const Timestamp ts) {
+
+
+		//Quaternion qFused = qGyro;
+		//qFiltered = qAccel;
+		//Quaternion qFused = (qGyro * alpha) + (qAccel * (1-alpha));
+		qFiltered = Quaternion::lerp(qFiltered*qGyroUpdate, qAccel, 0.05);
+		//qFiltered = Quaternion::fromAxisAngle(qAccel.w*0.02, qAccel.x, qAccel.y, qAccel.z) * (qFiltered * qGyroUpdate) * 0.98;
+
+		Vector3 fused = qFiltered.toEuler();
+		//std::cout << fused.x <<" " << fused.y << " " << fused.z << std::endl;
+
+		// update
+		orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, fused.z); //getMatrixFor(cur);
+
+
+		orientation.isKnown = true;
+		orientation.lastEstimation = ts;
+
+		// debug-plot (if configured)
+		#ifdef WITH_DEBUG_PLOT
+		    plot.add(ts, Vector3(0,0,0), orientation.rotationMatrix);
+			if (++cnt % 2 == 0) {
+				plotAngles.addAcc(ts, qAccel.toEuler().x, qAccel.toEuler().y);
+				plotAngles.addGyro(ts, qGyro.toEuler().x, qGyro.toEuler().y);
+				plotAngles.addFused(ts, fused.x, fused.y);
+			}
+		#endif
+
+	}
+
+
+};
+
+#endif // POSEDETECTION3_H
--- a/sensors/imu/PoseDetectionPlot.h
+++ b/sensors/imu/PoseDetectionPlot.h
@@ -17,19 +17,103 @@
 	#include "../../math/Matrix3.h"
 	#include "AccelerometerData.h"

+    class PoseDetectionPlotAngles {
+
+		Timestamp plotRef;
+		Timestamp lastPlot;
+
+		K::Gnuplot gp;
+
+		K::GnuplotPlot plotAcc;
+
+		K::GnuplotPlotElementLines lineAccX;
+		K::GnuplotPlotElementLines lineAccY;
+
+		K::GnuplotPlotElementLines lineGyroX;
+		K::GnuplotPlotElementLines lineGyroY;
+
+		K::GnuplotPlotElementLines lineFusedX;
+		K::GnuplotPlotElementLines lineFusedY;
+
+	public:
+
+		PoseDetectionPlotAngles() {
+
+			gp << "set autoscale xfix\n";
+
+			plotAcc.setTitle("Accelerometer");
+
+			plotAcc.add(&lineFusedX); lineFusedX.getStroke().getColor().setHexStr("#990000"); lineFusedX.setTitle("FusedX");lineFusedX.getStroke().setWidth(3);
+			plotAcc.add(&lineFusedY); lineFusedY.getStroke().getColor().setHexStr("#09900"); lineFusedY.setTitle("FusedY");	lineFusedY.getStroke().setWidth(3);
+
+			plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff8888"); lineAccX.setTitle("AccX");		lineAccX.getStroke().setType(K::GnuplotDashtype::DOTTED);		lineAccX.getStroke().setWidth(2);
+			plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#88ff88"); lineAccY.setTitle("AccY");		lineAccY.getStroke().setType(K::GnuplotDashtype::DOTTED);		lineAccY.getStroke().setWidth(2);
+
+			plotAcc.add(&lineGyroX); lineGyroX.getStroke().getColor().setHexStr("#ff8888"); lineGyroX.setTitle("GyroX");	lineGyroX.getStroke().setType(K::GnuplotDashtype::DASHED);		lineGyroX.getStroke().setWidth(2);
+			plotAcc.add(&lineGyroY); lineGyroY.getStroke().getColor().setHexStr("#88ff88"); lineGyroY.setTitle("GyroY");	lineGyroY.getStroke().setType(K::GnuplotDashtype::DASHED);		lineGyroY.getStroke().setWidth(2);
+
+			plotAcc.getKey().setVisible(true);
+
+		}
+
+		void addAcc(Timestamp ts, float x, float y) {
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
+			lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
+		}
+
+		void addGyro(Timestamp ts, float x, float y) {
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			lineGyroX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
+			lineGyroY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
+		}
+
+		void addFused(Timestamp ts, float x, float y) {
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			lineFusedX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
+			lineFusedY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
+			if (++cnt % 40 == 0) {flush(ts);}
+		}
+
+
+
+	private:
+
+		int cnt = 0;
+
+		void flush(Timestamp ts) {
+			cleanup(ts);
+			gp.draw(plotAcc);
+			gp.flush();
+		}
+
+		// remove old  entries
+		void cleanup(Timestamp ts) {
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(3000);
+			auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+			lineAccX.removeIf(remove);
+			lineAccY.removeIf(remove);
+			lineGyroX.removeIf(remove);
+			lineGyroY.removeIf(remove);
+			lineFusedX.removeIf(remove);
+			lineFusedY.removeIf(remove);
+		}
+
+
+
+	};
+
 	class PoseDetectionPlot {

 		Timestamp plotRef;
 		Timestamp lastPlot;

-		K::Gnuplot gp1;
 		K::Gnuplot gp2;

-		K::GnuplotPlot plotAcc;
-		K::GnuplotPlotElementLines lineAccX;
-		K::GnuplotPlotElementLines lineAccY;
-		K::GnuplotPlotElementLines lineAccZ;
-
 		K::GnuplotSplot plotPose;
 		K::GnuplotSplotElementLines linePose;
 		//K::GnuplotSplotElementEmpty emptyPose;
@@ -41,14 +125,9 @@
 		/** ctor */
 		PoseDetectionPlot() {

-			gp1 << "set autoscale xfix\n";
+			//gp1 << "set autoscale xfix\n";
 			gp2 << "set view equal xyz\n";

-			plotAcc.setTitle("Accelerometer");
-			plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
-			plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
-			plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");
-
 			plotPose.setTitle("Pose");
 			plotPose.getView().setEnabled(false);
 			plotPose.add(&linePose);
@@ -58,6 +137,10 @@
 			plotPose.getAxisY().setRange(-8,+8);
 			plotPose.getAxisZ().setRange(-8,+8);

+			plotPose.getAxisX().setLabel("x");
+			plotPose.getAxisY().setLabel("y");
+			plotPose.getAxisZ().setLabel("z");
+
 			const float a = 0.05; const float b = 0.95;
 			pose = {
 				{{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden
@@ -85,17 +168,20 @@

 		}

+		void setName(const std::string& name) {
+			plotPose.setTitle(name);
+		}
+
+		void add(Timestamp ts, const Vector3& avg, const Matrix3& rotation) {
+			add(ts, AccelerometerData(avg.x, avg.y, avg.z), rotation);
+		}
+
 		void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) {

 			if (plotRef.isZero()) {plotRef = ts;}
 			const Timestamp tsPlot = (ts-plotRef);
 			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);

-			// acc
-			lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) );
-			lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) );
-			lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) );
-
 			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
 				lastPlot = tsPlot;

@@ -128,20 +214,18 @@
 				const Vector3 vx = rotation * Vector3(2,0,0);
 				const Vector3 vy = rotation * Vector3(0,3,0);
 				const Vector3 vz = rotation * Vector3(0,0,5);
+				const Vector3 vA = Vector3(avg.x, avg.y, -avg.z) * 1;
 				linePose.clear();
 				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z));
 				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z));
 				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z));
+				linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vA.x, vA.y, vA.z));

-				// remove old accelerometer entries
-				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
-				lineAccX.removeIf(remove);
-				lineAccY.removeIf(remove);
-				lineAccZ.removeIf(remove);
+				gp2 << "set label 91 at " << vx.x << "," << vx.y << "," << vx.z << " 'x' \n";
+				gp2 << "set label 92 at " << vy.x << "," << vy.y << "," << vy.z << " 'y' \n";
+				gp2 << "set label 93 at " << vz.x << "," << vz.y << "," << vz.z << " 'z' \n";
+				gp2 << "set label 99 at " << vA.x << "," << vA.y << "," << vA.z << " 'accel' \n";

-				// raw accelerometer
-				gp1.draw(plotAcc);
-				gp1.flush();

 				// 3D pose
 				gp2.draw(plotPose);
--- a/sensors/imu/PoseProvider.h
+++ b/sensors/imu/PoseProvider.h
@@ -0,0 +1,43 @@
+#ifndef POSEPROVIDER_H
+#define POSEPROVIDER_H
+
+#include "../../math/Matrix3.h"
+
+class PoseProvider {
+
+public:
+
+	virtual const Matrix3& getMatrix() const = 0;
+
+	virtual float getSigma() const = 0;
+
+	virtual bool isKnown() const = 0;
+
+};
+
+
+class PoseProviderDummy : public PoseProvider {
+
+	Matrix3 mat = Matrix3::identity();
+
+public:
+
+	virtual const Matrix3& getMatrix() const override {
+		return mat;
+	}
+
+	Matrix3 getMatrixGyro() const {
+		return mat;
+	}
+
+	Matrix3 getMatrixAcc() const {
+		return mat;
+	}
+
+	virtual bool isKnown() const override {
+		return true;
+	}
+
+};
+
+#endif // POSEPROVIDER_H
--- a/sensors/imu/StepDetection2.h
+++ b/sensors/imu/StepDetection2.h
@@ -1,7 +1,6 @@
 #ifndef STEPDETECTION2_H
 #define STEPDETECTION2_H

-
 #include "AccelerometerData.h"
 #include "../../data/Timestamp.h"

@@ -52,6 +51,8 @@ private:

 	float threshold = 0.50;

+	float curFiltered = 0;
+
 	#ifdef WITH_DEBUG_PLOT
 		K::Gnuplot gp;
 		K::GnuplotPlot plot;
@@ -109,6 +110,10 @@ public:

 	}

+	float getCurFiltered() const {
+		return curFiltered;
+	}
+
 	/** does the given data indicate a step? */
 	bool add(const Timestamp ts, const AccelerometerData& acc) {

@@ -118,6 +123,7 @@ public:
 		auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {

 			const float mag = data.magnitude();
+			Assert::isNotNaN(mag, "detected NaN magnitude");

 			// use long-term average to center around zero
 			avg.add(ts, mag);
@@ -128,6 +134,8 @@ public:
 //			if (f != f) {return;}
 			const float f = biquad.filter(mag0);
 			const float fMag = f;
+			curFiltered = fMag;
+			Assert::isNotNaN(fMag, "detected NaN filtered magnitude");

 			const bool isMax = locMax.add(fMag);

@@ -173,6 +181,8 @@ public:

 		};

+		//qDebug() << ts.ms() << " ---" << acc.x << " " << acc.y << " " << acc.z;
+
 		// ensure fixed sampling rate for FIR freq filters to work!
 		interpol.add(ts, acc, onResample);

--- a/sensors/imu/TurnDetection.h
+++ b/sensors/imu/TurnDetection.h
@@ -4,11 +4,11 @@
 #include "GyroscopeData.h"
 #include "AccelerometerData.h"
 #include "../../data/Timestamp.h"
-#include "../../math/MovingAverageTS.h"
+#include "../../math/MovingStdDevTS.h"
 #include "../../math/Matrix3.h"

 #include "../../geo/Point3.h"
-#include "PoseDetection.h"
+#include "PoseProvider.h"

 //#include <eigen3/Eigen/Dense>

@@ -19,12 +19,13 @@


 #include "../../Assertions.h"
+#include "TurnProvider.h"

-class TurnDetection {
+class TurnDetection : public TurnProvider {

 private:

-	PoseDetection* pose = nullptr;
+	PoseProvider* pose = nullptr;

 	//std::vector<GyroscopeData> gyroData;
 	//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -32,6 +33,10 @@ private:

 	Timestamp lastGyroReading;

+	float curSigma = 0;
+
+	MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
+
 	#ifdef WITH_DEBUG_PLOT
 		TurnDetectionPlot plot;
 	#endif
@@ -39,7 +44,7 @@ private:
 public:

 	/** ctor */
-	TurnDetection(PoseDetection* pose) : pose(pose) {
+	TurnDetection(PoseProvider* pose) : pose(pose) {
 		;
 	}

@@ -65,6 +70,11 @@ public:
 //	} driftEst;


+	/** get the current uncertainty estimation */
+	float getSigma() const override {
+		return curSigma;
+	}
+
 	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {

 		// ignore the first reading completely, just remember its timestamp
@@ -90,17 +100,20 @@ public:
 		const Vector3 curGyro = pose->getMatrix() * vec;
 		//driftEst.removeDrift(ts, curGyro);

+
 		// area
 		//const Eigen::Vector3f area =
 		const Vector3 area =

-				// Trapezoid rule (should be more accurate but does not always help?!)
-				//(prevGyro * curDiff.sec()) +					// squared region
-				//((curGyro - prevGyro) * 0.5 * curDiff.sec());	// triangle region to the next (enhances the quality)
+		    // Trapezoid rule (should be more accurate but does not always help?!)
+		    //(prevGyro * curDiff.sec()) +					// squared region
+		    //((curGyro - prevGyro) * 0.5 * curDiff.sec());	// triangle region to the next (enhances the quality)

-				// just the rectangular region
-				(prevGyro * curDiff.sec());		// BEST?!
+		    // average (is the same as above)
+		    //((curGyro+prevGyro)/2 * curDiff.sec());

+		    // just the rectangular region
+		    (prevGyro * curDiff.sec());		// BEST?!

 		//}

@@ -112,9 +125,14 @@ public:
 		const float delta = area.z;

 		#ifdef WITH_DEBUG_PLOT
-			plot.add(ts, delta, gyro, curGyro);
+		    plot.addRelative(ts, delta, gyro, curGyro);
 		#endif

+		//stdDevForSigma.add(ts, prevGyro.z);	// ignore delta T. directly us radians-per-sec as sigma
+		//curSigma = stdDevForSigma.get();
+		const float radPerSec = 1.0f / 180.0f * M_PI;
+		curSigma = radPerSec + std::abs(prevGyro.z * 0.05);	// constant of 1deg/sec + 5% of current turn rate
+
 		// done
 		return delta;

--- a/sensors/imu/TurnDetection2.h
+++ b/sensors/imu/TurnDetection2.h
@@ -0,0 +1,117 @@
+#ifndef TURNDETECTION2_H
+#define TURNDETECTION2_H
+
+
+#include "GyroscopeData.h"
+#include "AccelerometerData.h"
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../math/Matrix3.h"
+
+#include "../../geo/Point3.h"
+#include "PoseProvider.h"
+
+//#include <eigen3/Eigen/Dense>
+
+#include <cmath>
+#include <vector>
+
+#include "TurnDetectionPlot.h"
+
+
+#include "../../Assertions.h"
+
+/** THIS WILL NOT WORK AS EXPECTED! */
+class TurnDetection2 {
+
+private:
+
+	PoseProvider* pose = nullptr;
+
+	Vector3 sumSinceStart = Vector3(0,0,0);
+
+	Timestamp lastGyroReadingTS;
+	Vector3 lastGyroReading = Vector3(0,0,0);
+
+	bool first = true;
+	Matrix3 poseMat = Matrix3::identity();
+
+	float curRes = 0;
+
+	#ifdef WITH_DEBUG_PLOT
+		TurnDetectionPlot plot;
+	#endif
+
+public:
+
+	/** ctor */
+	TurnDetection2(PoseProvider* pose) : pose(pose) {
+		;
+	}
+
+
+	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
+
+
+
+		// ignore the first reading completely, just remember its timestamp
+		if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return curRes;}
+
+		// time-difference between previous and current reading
+		const Timestamp curDiff = ts - lastGyroReadingTS;
+		lastGyroReadingTS = ts;
+
+		// fast sensors might lead to delay = 0 ms. filter those values
+		if (curDiff.isZero()) {return curRes;}
+
+		// current area
+		//const Eigen::Vector3f area =
+		const Vector3 curArea =
+
+		        // Trapezoid rule (should be more accurate but does not always help?!)
+		        //(prevGyro * curDiff.sec()) +					// squared region
+		        //((curGyro - prevGyro) * 0.5 * curDiff.sec());	// triangle region to the next (enhances the quality)
+
+		        // just the rectangular region
+		        (lastGyroReading * curDiff.sec());		// BEST?!
+
+		//}
+
+		// adjust sum
+		sumSinceStart = sumSinceStart + curArea;
+
+		// update the previous value
+		lastGyroReading = Vector3(gyro.x, gyro.y, gyro.z);
+
+
+		// TESTING
+		const float sign = (curArea.z < 0) ? (-1) : (+1);
+		const float mag = curArea.norm();
+		return (mag * sign);
+
+
+//		// ignore readings until the first orientation-estimation is available
+//		// otherwise we would use a wrong rotation matrix which yields wrong results!
+//		if (!pose->isKnown()) {return curRes;}
+//		if (first) {poseMat = pose->getMatrix(); first = false;}
+
+//		// rotate the sum since start into our desired coordinate system, where the smartphone lies flat on the ground
+//		const Vector3 angles = pose->getMatrix() * sumSinceStart;
+//		//const Vector3 angles = poseMat * sumSinceStart;
+
+//		// rotation = z-axis only!
+//		//const float delta = area(2);
+//		curRes = angles.z;
+
+//		#ifdef WITH_DEBUG_PLOT
+//		    plot.addAbsolute(ts, curRes, gyro, sumSinceStart);
+//		#endif
+
+//		// done
+//		return curRes;
+
+	}
+
+};
+
+#endif // TURNDETECTION2_H
--- a/sensors/imu/TurnDetectionPlot.h
+++ b/sensors/imu/TurnDetectionPlot.h
@@ -69,7 +69,7 @@

 		}

-		void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
+		void addRelative(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {

 			plotCurHead += delta;

@@ -114,6 +114,49 @@

 		}

+		void addAbsolute(Timestamp ts, const float curHead, const GyroscopeData& gyro, const Vector3& gyroFixed) {
+
+			if (plotRef.isZero()) {plotRef = ts;}
+			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
+
+			// raw gyro
+			lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
+			lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
+			lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );
+
+			// adjusted gyro
+			lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) );
+			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
+			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineGyroRawX.removeIf(remove);
+				lineGyroRawY.removeIf(remove);
+				lineGyroRawZ.removeIf(remove);
+				lineGyroFixX.removeIf(remove);
+				lineGyroFixY.removeIf(remove);
+				lineGyroFixZ.removeIf(remove);
+
+				const float ax = 0.85 + std::cos(curHead)*0.1;
+				const float ay = 0.85 + std::sin(curHead)*0.1;
+				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
+				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
+
+				//gp1.draw(plotGyroRaw);	// raw only
+				gp1.draw(plotGyroFix);	// fixed only
+				//gp1.draw(multiplot);	// both
+
+				gp1.flush();
+
+			}
+
+		}
+
 	};

 #endif
--- a/sensors/imu/TurnProvider.h
+++ b/sensors/imu/TurnProvider.h
@@ -0,0 +1,12 @@
+#ifndef TURN_PROVIDER_H
+#define TURN_PROVIDER_H
+
+class TurnProvider {
+
+public:
+
+	virtual float getSigma() const = 0;
+
+};
+
+#endif // TURN_PROVIDER_H
--- a/sensors/pressure/BarometerData.h
+++ b/sensors/pressure/BarometerData.h
@@ -3,28 +3,67 @@


 #include <cmath>
+#include "../../math/Floatingpoint.h"

 /** data received from a barometer sensor */
 struct BarometerData {

-	float hPa;
+	FPDefault hPa;

 	explicit BarometerData() : hPa(0) {;}

-	explicit BarometerData(const float hPa) : hPa(hPa) {;}
+	explicit BarometerData(const FPDefault hPa) : hPa(hPa) {;}

 	/** valid data? */
 	bool isValid() const {
-		return hPa == hPa;
+		return !std::isnan(hPa);
 	}

 	bool operator == (const BarometerData& o ) const {
 		return EQ_OR_NAN(hPa, o.hPa);
 	}

+public:
+
+	static constexpr double R = 8.31447;	// universal gas constant for air,		newton meter / mol kelvin
+	static constexpr double L = 0.0065;	// standard temperature lapse rate,			degree kelven per meter
+	static constexpr double g = 9.80665;	// gravity constant,					meter per square second
+	static constexpr double M = 0.0289644;	// molar mass for dry air,				kg / mol
+	static constexpr double P0 = 1013.25;	// pressure at mean sea level,			hPa
+	static constexpr double T0 = 288.15;		// temperature at mean sea level,		kelvin
+
+	/** hPa->meter assuming a constant 1013.25 at 0m as reference */
+	static FPDefault hpaToMeter(const FPDefault pressure) {
+		return static_cast<FPDefault>(
+		    (T0/L) * (std::pow(static_cast<FPDouble>(pressure)/P0, -(R*L)/(g*M))-1.0)
+		);
+	}
+
+	/** hPa->meter by using the given pressure pRef at hRef as reference */
+	static FPDefault hpaToMeter(const FPDefault pressure, const FPDefault PRef, const FPDefault hRef) {
+		return static_cast<FPDefault>(
+		    hRef + (T0/L) * (std::pow(static_cast<FPDouble>(pressure)/PRef, -(R*L)/(g*M))-1.0)
+		);
+	}
+
+	/** meter->hPa assuming a constant 1013.25 at 0m as reference */
+	static FPDefault meterTohPa(const FPDefault altitude) {
+		return static_cast<FPDefault>(
+		    P0 * std::pow(T0 / (T0+L*altitude), (g*M)/(R*L))
+		);
+	}
+
+	/**  meter->hPa by using the given pressure pRef at hRef as reference */
+	static FPDefault meterTohPa(const FPDefault altitude, const FPDefault PRef, const FPDefault hRef) {
+		return static_cast<FPDefault>(
+		    PRef * std::pow(T0 / (T0+L*(altitude-hRef)), (g*M)/(R*L))
+		);
+	}
+
+
 private:

-	static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
+	static inline bool EQ_OR_NAN(const FPDefault a, const FPDefault b) {return (a==b) || ( (a!=a) && (b!=b) );}

 };