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
2017-10-31 19:38:08 +01:00
parent d97b325650
commit 284c6b11a6
14 changed files with 1377 additions and 579 deletions
--- a/sensors/imu/CompassDetection.h
+++ b/sensors/imu/CompassDetection.h
@@ -0,0 +1,112 @@
+#ifndef INDOOR_IMU_COMPASSDETECTION_H
+#define INDOOR_IMU_COMPASSDETECTION_H
+
+#include "MagnetometerData.h"
+#include "PoseDetection.h"
+
+#include "../../data/Timestamp.h"
+#include "../../math/MovingAverageTS.h"
+#include "../../math/MovingStdDevTS.h"
+#include "../../geo/Point3.h"
+#include "../../Assertions.h"
+
+#include "CompassDetectionPlot.h"
+
+#include <cmath>
+#include <vector>
+
+
+/**
+ * custom compass implementation, similar to turn-detection
+ */
+class CompassDetection {
+
+private:
+
+	#ifdef WITH_DEBUG_PLOT
+		CompassDetectionPlot plot;
+	#endif
+
+//	struct {
+//		Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
+//		bool isKnown = false;
+//		Timestamp lastEstimation;
+//	} orientation;
+
+	MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
+
+	//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
+	MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
+
+	PoseDetection* pose = nullptr;
+	int numMagReadings = 0;
+
+public:
+
+	/** ctor */
+	CompassDetection(PoseDetection* pose) : pose(pose) {
+		;
+	}
+
+	/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
+	float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {
+
+		++numMagReadings;
+
+		// get the current magnetometer-reading as vector
+		//Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z;
+		const Vector3 vec(mag.x, mag.y, mag.z);
+
+		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
+		//Eigen::Vector3f _curMag = pose->getMatrix() * vec;
+		const Vector3 _curMag = pose->getMatrix() * vec;
+
+		//avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2)));
+		avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z));
+		const MagnetometerData curMag = avgIn.get();
+		//const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z);
+
+		// calculate standard-deviation
+		//stdDev.add(ts, curMag);
+		//const float dev = std::max(stdDev.get().x, stdDev.get().y);
+
+
+		// 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 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;
+		float resHeading;
+		bool stable = true;
+
+		// calculate standard-deviation within a certain timeframe
+		stdDev.add(ts, 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 {
+			resHeading = curHeading;
+			stable = true;
+		}
+
+		#ifdef WITH_DEBUG_PLOT
+			plot.add(ts, curHeading, stable, mag, curMag);
+		#endif
+
+		// done
+		return resHeading;
+
+	}
+
+};
+
+#endif // INDOOR_IMU_COMPASSDETECTION_H