worked on synthetic sensors

worked on grid-walker
minor changes/fixes/improvements

synthetic/SyntheticSteps.h

@@ -1,11 +1,16 @@
 #ifndef SYNTHETICSTEPS_H
 #define SYNTHETICSTEPS_H
 
-#include "../sensors/imu/AccelerometerData.h"
 #include "SyntheticWalker.h"
+
+#include "../sensors/imu/AccelerometerData.h"
+
 #include "../math/distribution/Normal.h"
 
-/** fakes accelerometer-data based on synthetic walking data */
+/**
+ * fakes accelerometer-data
+ * based on synthetic walking data
+ */
 class SyntheticSteps : SyntheticWalker::Listener {
 
 public:
@@ -25,9 +30,14 @@ private:
 
 	float lastStepAtDistance = 0;
 
-	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 1);
-	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 1);
-	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 1);
+	Timestamp refStepPattern;
+	Interpolator<Timestamp, AccelerometerData> stepPattern;
+
+	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
+	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
+	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
+
+	int stepPatternPos = -1;
 
 	std::vector<Listener*> listeners;
 
@@ -35,9 +45,33 @@ public:
 
 	/** ctor with the walker to follow */
 	SyntheticSteps(SyntheticWalker* walker) {
+
 		walker->addListener(this);
+		dX.setSeed(1);
+		dY.setSeed(3);
+		dZ.setSeed(5);
+
+		// build the step-pattern (how does a step look-like on the accelerometer)
+		// TODO: switch to MS?! use interpolator?
+//		int duration_ms = 350;
+//		for (int i = 0; i < duration_ms; i += 10) {
+//			float z = std::sin(i*M_PI*2/duration_ms) * 3.0;
+//			if (z < 0) {z *= 0.75;}	// less pronounced in the negative part
+//			float y = std::cos(i*M_PI*2/duration_ms) * 0.5;
+//			const float x = dO.draw();
+//			z += dO.draw()*2;
+//			y += dO.draw();
+//			AccelerometerData acc(x,y,z);
+//			stepPattern.add(Timestamp::fromMS(i), acc);
+//		}
+		stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
+		stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
+		stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
+		stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
+
 	}
 
+	/** attach a listener to this provider */
 	void addListener(Listener* l) {
 		this->listeners.push_back(l);
 	}
@@ -47,15 +81,32 @@ protected:
 
 	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
 
+		(void) curPos;
 		const float nextStepAt = (lastStepAtDistance + stepSize_m);
 
+		// 1st, start with random noise on the accelerometer
 		const float x = dX.draw();
 		const float y = dY.draw();
 		const float z = dZ.draw();
-		AccelerometerData acc(x, y, z);
+		const AccelerometerData base(0, 4, 9.7);
+		const AccelerometerData noise(x, y, z);
+		AccelerometerData acc = base + noise;
 
+		// is it time to inject a "step" into the accelerometer data?
 		if (walkedDistance > nextStepAt) {
 			lastStepAtDistance = walkedDistance;
+			refStepPattern = walkedTime;
+		}
+
+		// overlay the noise with a step-pattern (see ctor)
+		if (refStepPattern.ms() > 0) {
+			Timestamp curPatPos = walkedTime - refStepPattern;
+			if (curPatPos >= stepPattern.getMaxKey()) {
+				refStepPattern = Timestamp::fromMS(0);
+			} else {
+				const AccelerometerData step = stepPattern.get(curPatPos);
+				acc = base + noise*2.5f + step;
+			}
 		}
 
 		for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}

synthetic/SyntheticTurns.h

@@ -1,4 +1,127 @@
-#ifndef SYNTHETICTURNS_H
-#define SYNTHETICTURNS_H
+#ifndef INDOOR_SYNTHETICTURNS_H
+#define INDOOR_SYNTHETICTURNS_H
 
-#endif // SYNTHETICTURNS_H
+#include "SyntheticWalker.h"
+
+#include "../sensors/imu/AccelerometerData.h"
+#include "../sensors/imu/GyroscopeData.h"
+#include "../geo/Heading.h"
+
+#include "../math/distribution/Normal.h"
+
+/**
+ * simulates acceleromter and gyroscope data
+ * based on synthetic walking data
+ *
+ * @brief The SyntheticTurns class
+ */
+class SyntheticTurns : public SyntheticWalker::Listener {
+
+public:
+
+	class Listener {
+	public:
+		virtual void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) = 0;
+	};
+
+private:
+
+	/** the walker to listen to */
+	SyntheticWalker* walker;
+
+	Distribution::Normal<float> dAccX = Distribution::Normal<float>(0, 2.5);
+	Distribution::Normal<float> dAccY = Distribution::Normal<float>(0, 1.5);
+	Distribution::Normal<float> dAccZ = Distribution::Normal<float>(0, 1);
+
+	Distribution::Normal<float> dGyroX = Distribution::Normal<float>(0, 0.02);
+	Distribution::Normal<float> dGyroY = Distribution::Normal<float>(0, 0.02);
+	Distribution::Normal<float> dGyroZ = Distribution::Normal<float>(0, 0.02);
+
+	Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
+	Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
+	Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10);	// heading error, slightly biased
+
+
+	std::vector<Listener*> listeners;
+
+public:
+
+	/** ctor with the walker to follow */
+	SyntheticTurns(SyntheticWalker* walker) {
+		walker->addListener(this);
+		dAccX.setSeed(1);
+		dAccY.setSeed(3);
+		dAccZ.setSeed(5);
+		dGyroX.setSeed(7);
+		dGyroY.setSeed(9);
+		dGyroZ.setSeed(11);
+	}
+
+	/** attach a listener to this provider */
+	void addListener(Listener* l) {
+		this->listeners.push_back(l);
+	}
+
+protected:
+
+	Timestamp lastTs;
+	Heading desiredHead = Heading(0);
+	Heading curHead = Heading(0);
+	Point3 lastPos = Point3(NAN, NAN, NAN);
+	float change;
+
+	inline float clamp(const float val, const float min, const float max) {
+		if (val < min) {return min;}
+		if (val > max) {return max;}
+		return val;
+	}
+
+	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
+
+		// time sine last onWalk();
+		if (lastTs.isZero()) {lastTs = walkedTime; return;}
+		const Timestamp deltaTs = walkedTime - lastTs;
+		lastTs = walkedTime;
+
+		if (lastPos.x != lastPos.x) {
+			lastPos = curPos;
+		} else {
+			desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
+			lastPos = curPos;
+		}
+
+		// difference between current-heading and desired-heading
+		const float diffRad = Heading::getSignedDiff(curHead, desiredHead);
+
+		// slowly change the current heading to match the desired one
+		const float maxChange = dMaxChange.draw();
+		const float toChange = clamp(diffRad, -maxChange, +maxChange);
+		//if (change < toChange) {change += toChange*0.01;}
+		if (change > toChange) {change *= 0.93;}
+		if (change < toChange) {change += dChange.draw()/10000;}
+		//if (change > toChange) {change -= dChange.draw();}
+
+
+
+		curHead += change;
+
+		// convert to gyro's radians-per-second
+		const float radPerSec = change * 1000 / deltaTs.ms();;
+
+		const float accX = 0.00 + dAccX.draw();
+		const float accY = 0.00 + dAccY.draw();
+		const float accZ = 9.81 + dAccZ.draw();
+		AccelerometerData acc(accX, accY, accZ);
+
+		const float gyroX = dGyroX.draw();
+		const float gyroY = dGyroY.draw();
+		const float gyroZ = dGyroZ.draw() + radPerSec;
+		GyroscopeData gyro(gyroX, gyroY, gyroZ);
+
+		for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}
+
+	}
+
+};
+
+#endif // INDOOR_SYNTHETICTURNS_H

synthetic/SyntheticWalker.h

@@ -45,7 +45,7 @@ public:
 	}
 
 	/** increment the walk */
-	void tick(const Timestamp timePassed) {
+	Point3 tick(const Timestamp timePassed) {
 
 		// update time
 		this->walkedTime += timePassed;
@@ -61,6 +61,8 @@ public:
 		// inform listener
 		for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
 
+		return curPosOnPath;
+
 	}