Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

math/Delay.h

@@ -0,0 +1,47 @@
+#ifndef DELAY_H
+#define DELAY_H
+
+#include <cstdint>
+#include <vector>
+
+template <typename T> class Delay {
+
+private:
+
+	size_t size;
+
+	/** up to "size" elements */
+	std::vector<T> values;
+
+public:
+
+	/** ctor */
+	Delay(const int size) : size(size) {;}
+
+	/** add a new value */
+	void add(const T val) {
+
+		// add new value
+		values.push_back(val);
+
+		// remove old ones
+		while(values.size() > size) {
+			values.erase(values.begin());
+		}
+
+	}
+
+	/** get the delayed value */
+	T get() const {
+		return values.front();
+	}
+
+	/** delay output valid? */
+	bool isValid() const {
+		return values.size() == size;
+	}
+
+
+};
+
+#endif // DELAY_H

math/LocalMaxima.h

@@ -1,6 +1,63 @@
 #ifndef LOCALMAXIMA_H
 #define LOCALMAXIMA_H
 
+#include "Delay.h"
+
+class LocalMaxima {
+
+	int delay;
+	Delay<float> d0;
+	Delay<float> d1;
+	float cur;
+	int block = 0;
+
+public:
+
+	LocalMaxima(int delay) : delay(delay), d0(delay*2), d1(delay) {
+		;
+	}
+
+	bool add(const float s) {
+
+		if (block > 0) {--block;}
+
+		d0.add(s);
+		d1.add(s);
+		this->cur = s;
+
+		if (isMax()) {
+			block = delay;
+			return true;
+		}
+
+		return false;
+
+	}
+
+	float get() {
+		return d1.get();
+	}
+
+private:
+
+	bool isMax() {
+
+		if (block > 0) {return false;}
+
+		if (!d0.isValid()) {return false;}
+		if (!d1.isValid()) {return false;}
+
+		const float s0 = d0.get();
+		const float s1 = d1.get();
+		const float s2 = cur;
+
+		return (s1 > s0) && (s1 > s2);
+
+	}
+
+};
+
+/*
 class LocalMaxima {
 
 	static constexpr float MAX = 1e40;
@@ -21,12 +78,12 @@ public:
 		Res(bool isMax, float val) : isMax(isMax), val(val) {;}
 	};
 
-	/** ctor. use only every n-th sample */
+	// ctor. use only every n-th sample
 	LocalMaxima(const size_t everyNth) : everyNth(everyNth) {
 		reset();
 	}
 
-	/** is the given value a local maxima? */
+	// is the given value a local maxima?
 	Res add(const float s) {
 
 		if		(cnt == 0*everyNth) {s0 = s;}	// set, wait some time
@@ -60,5 +117,5 @@ private:
 	}
 
 };
-
+*/
 #endif // LOCALMAXIMA_H

sensors/imu/PoseDetectionPlot.h

@@ -8,10 +8,10 @@
 	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
 	#include <KLib/misc/gnuplot/GnuplotPlot.h>
 	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
-	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
+	//#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
 	#include <KLib/misc/gnuplot/GnuplotSplot.h>
 	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
-	#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
+	//#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
 
 	#include "../../data/Timestamp.h"
 	#include "../../math/Matrix3.h"

sensors/imu/StepDetection.h

@@ -38,8 +38,8 @@ private:
 	bool waitForUp = false;
 
 	const Timestamp blockTime;								// 150-250 looks good
-    const float upperThreshold = +0.4*0.6f;					// + is usually smaller than down (look at graphs)
+	const float upperThreshold = +0.4*0.8f;					// + is usually smaller than down (look at graphs)
-    const float lowerThreshold = -1.5*0.6f;					// the 0.8 is for testing!
+	const float lowerThreshold = -1.5*0.8f;					// the 0.8 is for testing!
 
 
 	#ifdef WITH_DEBUG_PLOT

sensors/imu/StepDetection2.h

@@ -25,6 +25,7 @@
 #include "../../math/dsp/FIRComplex.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/LocalMaxima.h"
+#include "../../math/MovingAverageTS.h"
 
 
 /**
@@ -43,12 +44,16 @@ private:
 	FIRComplex fir;
 	LocalMaxima locMax;
 
-	const float threshold = 0.5;
+	// longterm average to center around zero
+	MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
+
+	const float threshold = 0.50;
 
 	#ifdef WITH_DEBUG_PLOT
 		K::Gnuplot gp;
 		K::GnuplotPlot plot;
-		K::GnuplotPlotElementLines lineMag;
+		K::GnuplotPlotElementLines lineRaw;
+		K::GnuplotPlotElementLines lineFiltered;
 		K::GnuplotPlotElementPoints pointDet;
 		Timestamp plotRef;
 		Timestamp lastPlot;
@@ -63,15 +68,18 @@ private:
 public:
 
 	/** ctor */
-	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) {
+	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(8) {
 
-		fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
+		//fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
-		fir.shiftBy(2);			// typical step freq ~2Hz
+		//fir.shiftBy(2.00);			// typical step freq ~2Hz
+
+		fir.lowPass(3.5, 25);		// everything up to 3 HZ
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";
 			plot.setTitle("Step Detection");
-			plot.add(&lineMag); lineMag.getStroke().getColor().setHexStr("#000000");
+			plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
+			plot.add(&lineFiltered); lineFiltered.getStroke().getColor().setHexStr("#000000");
 			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
 		#endif
 
@@ -87,17 +95,23 @@ public:
 
 		bool step = false;
 
+		// accel-data incoming on a fixed sampling rate (needed for FIR to work)
 		auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
 
 			const float mag = data.magnitude();
 
-			const std::complex<float> c = fir.append(mag);
+			// use long-term average to center around zero
+			avg.add(ts, mag);
+			const float mag0 = mag - avg.get();
+
+			const std::complex<float> c = fir.append(mag0);
 			const float real = c.real();
 			if (real != real) {return;}
 			const float fMag = real;
 
-			LocalMaxima::Res res = locMax.add(fMag);
+			const bool isMax = locMax.add(fMag);
-			step = (res.isMax) && (res.val > threshold);
+
+			step = (isMax) && (locMax.get() > threshold);
 
 			#ifdef WITH_DEBUG_OUTPUT
 				if (step) {
@@ -113,7 +127,8 @@ public:
 				const Timestamp tsPlot = (ts-plotRef);
 				const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
 
-				lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
+				lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), mag) );
+				lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
 
 				if (step) {
 					pointDet.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
@@ -123,7 +138,8 @@ public:
 
 					lastPlot = tsPlot;
 					auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
-					lineMag.removeIf(remove);
+					lineRaw.removeIf(remove);
+					lineFiltered.removeIf(remove);
 					pointDet.removeIf(remove);
 
 					gp.draw(plot);
@@ -136,6 +152,7 @@ public:
 
 		};
 
+		// ensure fixed sampling rate for FIR freq filters to work!
 		interpol.add(ts, acc, onResample);
 
 		return step;

sensors/imu/TurnDetectionPlot.h

@@ -104,7 +104,10 @@
 				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(plotGyroRaw);	// raw only
+				gp1.draw(plotGyroFix);	// fixed only
+				//gp1.draw(multiplot);	// both
+
 				gp1.flush();
 
 			}