worked on synthetic sensors

worked on grid-walker
minor changes/fixes/improvements

grid/walk/v3/GridWalker3.h

@@ -37,6 +37,7 @@ public:
 	struct WalkResult {
 		Point3 position;
 		Heading heading = Heading(0);
+		double probability = 1.0;
 	};
 
 
@@ -79,18 +80,26 @@ public:
 		return bbox.contains(pt);
 	}
 
+	/** does one of the given grid-node scontains the provided point-in-question? */
+	const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
+		for (const Node* n : nodes) {
+			if (contains(grid, n, pt)) {return n;}
+		}
+		return nullptr;
+	}
+
 	const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
 
 		const GridPoint gpStart = p3ToGp(params.start);
 		const Node* startNode = grid.getNodePtrFor(gpStart);
 
 		static Distribution::Normal<float> dDist(1, 0.02);
-		static Distribution::Normal<float> dHead(0, 0.02);
+		static Distribution::Normal<float> dHead(0, 0.01);
 
 		// include one additional grid-cell (increased distance)
 		const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
 		const float range_m = params.distance_m + secBuffer_m;
-		const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
+		const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
 
 		WalkResult res;
 		res.heading = params.heading;
@@ -104,33 +113,39 @@ public:
 			const Point2 dst = params.start.xy() + (dir * realDist_m);
 
 			// is dst reachable?
-			for (const Node* n : nodes) {
+			const Node* n = contains(grid, reachableNodes, dst);
-				//const float distToNode = n->inMeter().xy().getDistance(dst);
+			if (n) {
-				//if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) {
+
-				if (contains(grid, n, dst)) {
+				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
-					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
+				const GridPoint gp = p3ToGp(p3);
-					const GridPoint gp = p3ToGp(p3);
+
-					if (grid.hasNodeFor(gp)) {
+				if (grid.hasNodeFor(gp)) {
-						res.position = p3;	// new position
+					res.position = p3;					// update position
-						res.heading;		// keep as-is
+					res.heading;						// keep as-is
-						return res;
+					res.probability;					// keep as-is
-					} else {
+					return res;							// done
-						std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
+				} else {
-					}
+					std::cout << "WARN dst not found" << std::endl;
+					//throw "should not happen";
 				}
+
 			}
 
+			// reduce probability with every new run
+			++cnt;
+			res.probability /= 2;
+
 			// before trying again, modify distance and angle
-			if (1 == 0) {
+			//if (1 == 1) {
-				realDist_m *= dDist.draw();
+				res.heading = params.heading + dHead.draw() * cnt;
-				res.heading += dHead.draw();
+				realDist_m = params.distance_m * dDist.draw();// * cnt;
-			}
+			//}
 
 			// reached max retries?
-			if (++cnt > 10) {
+			if (cnt > 8) {
-				WalkResult res;
+				res.position = params.start;	// reset
-				res.position = params.start;
+				res.heading = params.heading;	// reset
-				res.heading = params.heading;
+				res.probability = 1e-50;			// unlikely
 				return res;
 			}	// did not work out....
 

sensors/imu/AccelerometerData.h

@@ -42,6 +42,14 @@ struct AccelerometerData {
 		return AccelerometerData(x/val, y/val, z/val);
 	}
 
+	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);
+	}
+
 	std::string asString() const {
 		std::stringstream ss;
 		ss << "(" << x << "," << y << "," << z << ")";

sensors/imu/StepDetection.h

@@ -8,12 +8,14 @@
 #include <cmath>
 #include <vector>
 
-#include <KLib/misc/gnuplot/Gnuplot.h>
+#ifdef WITH_DEBUG_PLOT
-#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/Gnuplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
-#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
-
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+#endif
 
 #include "../../Assertions.h"
 #include "../../math/MovingAverageTS.h"
@@ -43,11 +45,12 @@ private:
 		K::Gnuplot gp;
 		K::GnuplotPlot plot;
 		K::GnuplotPlotElementLines lineDet;
+		K::GnuplotPlotElementPoints pointDet;
 		K::GnuplotPlotElementLines lineX;
 		K::GnuplotPlotElementLines lineY;
 		K::GnuplotPlotElementLines lineZ;
 		Timestamp plotRef;
-		int plotCnt = 0;
+		Timestamp lastPlot;
 	#endif
 
 
@@ -57,10 +60,13 @@ public:
 	StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
 
 		#ifdef WITH_DEBUG_PLOT
-			plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000");
+			gp << "set autoscale xfix\n";
-			plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00");
+			plot.setTitle("Step Detection");
-			plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff");
+			plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
+			plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
+			plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
 			plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
+			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
 		#endif
 
 	}
@@ -78,19 +84,25 @@ public:
 		bool step = false;
 
 
-		if (blockUntil > ts) {return false;}
+		if (blockUntil > ts) {
 
-		// wait for a rising edge
+			step = false;
-		if (waitForUp && delta > upperThreshold) {
+
-			blockUntil = ts + blockTime;				// block some time
+		} else {
-			waitForUp = false;
+
-		}
+			// wait for a rising edge
+			if (waitForUp && delta > upperThreshold) {
+				blockUntil = ts + blockTime;				// block some time
+				waitForUp = false;
+			}
+
+			// wait for a falling edge
+			if (!waitForUp && delta < lowerThreshold) {
+				blockUntil = ts + blockTime;				// block some time
+				waitForUp = true;
+				step = true;
+			}
 
-		// wait for a falling edge
-		if (!waitForUp && delta < lowerThreshold) {
-			blockUntil = ts + blockTime;				// block some time
-			waitForUp = true;
-			step = true;
 		}
 
 
@@ -98,6 +110,7 @@ public:
 
 			if (plotRef.isZero()) {plotRef = ts;}
 			const Timestamp tsPlot = (ts-plotRef);
+			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
 
 			//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
 			lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
@@ -105,10 +118,24 @@ public:
 			lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
 			lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
 
-			if (++plotCnt % 25 == 0) {
+			if (step) {
+				pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
+			}
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+				lineX.removeIf(remove);
+				lineY.removeIf(remove);
+				lineZ.removeIf(remove);
+				lineDet.removeIf(remove);
+				pointDet.removeIf(remove);
+
 				gp.draw(plot);
 				gp.flush();
-				//usleep(1000*25);
+				usleep(100);
+
 			}
 
 		#endif

sensors/imu/TurnDetection.h

@@ -5,18 +5,23 @@
 #include "AccelerometerData.h"
 #include "../../data/Timestamp.h"
 #include "../../math/MovingAverageTS.h"
+#include "../../geo/Point3.h"
 
 #include <eigen3/Eigen/Dense>
 
 #include <cmath>
 #include <vector>
 
-#include <KLib/misc/gnuplot/Gnuplot.h>
+#ifdef WITH_DEBUG_PLOT
-#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/Gnuplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
-#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
-
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#endif
 
 #include "../../Assertions.h"
 
@@ -24,6 +29,37 @@ class TurnDetection {
 
 private:
 
+	#ifdef WITH_DEBUG_PLOT
+		Timestamp plotRef;
+		Timestamp lastPlot;
+
+		K::Gnuplot gp1;
+		K::Gnuplot gp2;
+
+		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3);
+
+		K::GnuplotPlot plotGyroRaw;
+		K::GnuplotPlotElementLines lineGyroRawX;
+		K::GnuplotPlotElementLines lineGyroRawY;
+		K::GnuplotPlotElementLines lineGyroRawZ;
+
+		K::GnuplotPlot plotGyroFix;
+		K::GnuplotPlotElementLines lineGyroFixX;
+		K::GnuplotPlotElementLines lineGyroFixY;
+		K::GnuplotPlotElementLines lineGyroFixZ;
+
+		K::GnuplotPlot plotAcc;
+		K::GnuplotPlotElementLines lineAccX;
+		K::GnuplotPlotElementLines lineAccY;
+		K::GnuplotPlotElementLines lineAccZ;
+
+		K::GnuplotSplot plotPose;
+		K::GnuplotSplotElementLines linePose;
+
+		float plotCurHead = 0;
+
+	#endif
+
 
 	//std::vector<GyroscopeData> gyroData;
 	Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -44,7 +80,40 @@ public:
 
 	/** ctor */
 	TurnDetection() {
-		;
+
+		#ifdef WITH_DEBUG_PLOT
+
+			gp1 << "set autoscale xfix\n";
+			gp1 << "set view equal xyz\n";
+
+			multiplot.add(&plotGyroRaw);
+			multiplot.add(&plotGyroFix);
+			multiplot.add(&plotAcc);
+
+			plotGyroRaw.setTitle("Gyroscope (raw)");
+			plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
+			plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
+			plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");
+
+			plotGyroFix.setTitle("Gyroscope (fixed)");
+			plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
+			plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
+			plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");
+
+			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);
+			plotPose.getAxisX().setRange(-5,+5);
+			plotPose.getAxisY().setRange(-5,+5);
+			plotPose.getAxisZ().setRange(-5,+5);
+
+		#endif
+
 	}
 
 
@@ -90,7 +159,6 @@ public:
 		// otherwise we would use a wrong rotation matrix which yields wrong results!
 		if (!orientation.isKnown) {return 0.0f;}
 
-
 		// get the  current gyro-reading as vector
 		Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
 
@@ -117,6 +185,77 @@ public:
 		// rotation = z-axis only!
 		const float delta = area(2);
 
+
+		#ifdef WITH_DEBUG_PLOT
+
+			plotCurHead += delta;
+
+			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(), curGyro(0)) );
+			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) );
+			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) );
+
+			// adjusted gyro
+			lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) );
+			lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) );
+			lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) );
+
+			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+				lastPlot = tsPlot;
+
+				// plot 3D pose
+				std::vector<Point3> pose = {
+					Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1),
+					Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1),
+				};
+				linePose.clear();
+				for (const Point3 p : pose) {
+					Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z;
+					Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1;
+					K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2));
+					linePose.add(gp3);
+				}
+
+				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);
+				lineAccX.removeIf(remove);
+				lineAccY.removeIf(remove);
+				lineAccZ.removeIf(remove);
+
+				const float ax = 0.85 + std::cos(plotCurHead)*0.1;
+				const float ay = 0.85 + std::sin(plotCurHead)*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(multiplot);
+				gp1.flush();
+
+				gp2.draw(plotPose);
+				gp2.flush();
+
+				//usleep(100);
+
+			}
+
+		#endif
+
+
 		// done
 		return delta;
 
@@ -128,17 +267,31 @@ public:
 
 		// add accelerometer data
 		//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
-		est.addAcc(acc);
+		est.addAcc(ts, acc);
 
-		// start with the first available timestamp
+		if (1 == 0) {
-		if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
+
+			// FASTER
+
+			// start with the first available timestamp
+			if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
+
+			// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
+			if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
+				orientation.rotationMatrix = est.get();
+				orientation.isKnown = true;
+				orientation.lastEstimation = ts;
+				est.reset();
+			}
+
+		} else {
+
+			// MORE ACCURATE
 
-		// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
-		if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
 			orientation.rotationMatrix = est.get();
 			orientation.isKnown = true;
 			orientation.lastEstimation = ts;
-			est.reset();
+
 		}
 
 	}
@@ -207,7 +360,7 @@ public:
 		}
 
 		/** add the given accelerometer reading */
-		void addAcc(const AccelerometerData& acc) {
+		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
 
 			// did NOT improve the result for every smartphone (only some)
 			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
@@ -218,6 +371,11 @@ public:
 			sum += vec;
 			++cnt;
 
+
+		}
+
+		AccelerometerData getAvg() const {
+			return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
 		}
 
 		/** get the current rotation matrix estimation */
@@ -245,9 +403,64 @@ public:
 		}
 
 
+	};
+
+
+	struct XYZ2 {
+
+		// average the accelerometer
+		MovingAverageTS<AccelerometerData> avg = MovingAverageTS<AccelerometerData>(Timestamp::fromMS(1250), AccelerometerData());
+
+		XYZ2() {
+
+			// start approximately
+			addAcc(Timestamp(), AccelerometerData(0,0,9.81));
+
+		}
+
+		/** add the given accelerometer reading */
+		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
+
+			// did NOT improve the result for every smartphone (only some)
+			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
+			//if (deltaMag > 5.0) {return;}
+
+			avg.add(ts, acc);
+
+		}
+
+		AccelerometerData getAvg() const {
+			return avg.get();
+		}
+
+		/** get the current rotation matrix estimation */
+		Eigen::Matrix3f get() const {
+
+			// get the current acceleromter average
+			AccelerometerData avgAcc = getAvg();
+			const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
+
+			// rotate average-accelerometer into (0,0,1)
+			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
+			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();
+			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
+
+			return rotMat;
+
+		}
+
+		/** reset the current sum etc. */
+		void reset() {
+			;
+		}
+
 	} est;
 
 
+
 //	struct RotationMatrixEstimationUsingAccAngle {
 
 //		Eigen::Vector3f lastAvg;

sensors/offline/FilePlayer.h

@@ -83,6 +83,10 @@ namespace Offline {
 			// get all sensor events from the offline file
 			const std::vector<Entry> events = reader->getEntries();
 
+			// reference time (system vs. first-event)
+			Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
+			Timestamp tsRef2 = Timestamp::fromRunningTime();
+
 			// process every event
 			for (const Entry& e : events) {
 
@@ -92,6 +96,14 @@ namespace Offline {
 				// timestamp
 				const Timestamp ts = Timestamp::fromMS(e.ts);
 
+				// ensure events happen occur fast as they did during recording
+				if (realtime) {
+					const Timestamp ts1 = ts-tsRef1;
+					const Timestamp ts2 = Timestamp::fromRunningTime() - tsRef2;
+					const Timestamp diff = ts1-ts2;
+					if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
+				}
+
 				// event index
 				const size_t idx = e.idx;
 

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);
+	Timestamp refStepPattern;
-	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 1);
+	Interpolator<Timestamp, AccelerometerData> stepPattern;
-	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 1);
+
+	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
+#ifndef INDOOR_SYNTHETICTURNS_H
-#define 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;
+
 	}