worked on step-detection

sensors/imu/StepDetection3.h

@@ -29,8 +29,10 @@
 
 /**
  * simple step detection based on accelerometer magnitude.
- * magnitude > threshold? -> step!
- * block for several msec until detecting the next one
+ * interpolated to a fixed frequency
+ * passed through IIR filter
+ * searching for zero crossings that follow a peak value
+ * that is above a certain threshold
  */
 class StepDetection3 {
 

sensors/imu/StepDetection4.h

@@ -0,0 +1,197 @@
+#ifndef STEPDETECTION4_H
+#define STEPDETECTION4_H
+
+
+#include "AccelerometerData.h"
+#include "../../data/Timestamp.h"
+
+#include "PoseDetection.h"
+
+#include <cmath>
+#include <vector>
+
+#ifdef WITH_DEBUG_PLOT
+	#include <KLib/misc/gnuplot/Gnuplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplot.h>
+	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlot.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
+#endif
+
+#ifdef WITH_DEBUG_OUTPUT
+	#include <fstream>
+#endif
+
+#include "../../Assertions.h"
+#include "../../math/dsp/iir/BiQuad.h"
+#include "../../math/FixedFrequencyInterpolator.h"
+#include "../../math/DelayBuffer.h"
+
+
+/**
+ * step detection based on accelerometer data,
+ * un-rotated using pose-detection
+ * interpolated to a fixed frequency
+ * passed through IIR filter
+ * searching for zero crossings that follow a peak value
+ * that is above a certain threshold
+ */
+class StepDetection4 {
+
+	static constexpr float gravity = 9.81;
+	static constexpr float stepRate_hz = 2.0;
+	static constexpr int sRate_hz = 100;
+	static constexpr int every_ms = 1000 / sRate_hz;
+	static constexpr float threshold = 1.0;
+
+	float max = 0;
+	Timestamp maxTS;
+
+private:
+
+	PoseDetection* pose;
+	FixedFrequencyInterpolator<Vector3> interpol;
+	IIR::BiQuad<float> biquad;
+	DelayBuffer<float> delay;
+
+
+	#ifdef WITH_DEBUG_PLOT
+		K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines lineRaw;
+		K::GnuplotPlotElementLines lineFiltered;
+		K::GnuplotPlotElementPoints pointDet;
+		Timestamp plotRef;
+		Timestamp lastPlot;
+	#endif
+
+	#ifdef WITH_DEBUG_OUTPUT
+		std::ofstream outFiltered;
+		std::ofstream outSteps;
+	#endif
+
+
+public:
+
+	/** ctor */
+	StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
+
+		plot.getKey().setVisible(true);
+		lineRaw.setTitle("unrotated Z");
+		lineFiltered.setTitle("IIR filtered");
+
+		biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
+		biquad.preFill(gravity);
+
+		#ifdef WITH_DEBUG_PLOT
+			gp << "set autoscale xfix\n";
+			plot.setTitle("Step Detection");
+			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
+
+		#ifdef WITH_DEBUG_OUTPUT
+			outFiltered = std::ofstream("/tmp/sd2_filtered.dat");
+			outSteps = std::ofstream("/tmp/sd2_steps.dat");
+		#endif
+
+	}
+
+	/** does the given data indicate a step? */
+	bool add(const Timestamp ts, const AccelerometerData& _acc) {
+
+		// 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 false;}
+
+		// get the current accs-reading as vector
+		const Vector3 vec(_acc.x, _acc.y, _acc.z);
+
+		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
+		const Vector3 acc = pose->getMatrix() * vec;
+
+		// will be set when a step was detected
+		bool gotStep = false;
+
+		// accel-data incoming on a fixed sampling rate (needed for FIR to work)
+		// NOTE!!!! MIGHT TRIGGER MORE THAN ONCE PER add() !!!
+		auto onResample = [&] (const Timestamp ts, const Vector3 data) {
+
+			bool step = false;
+			const float mag = data.z;
+
+			// apply filter
+			const float fMag = biquad.filter(mag);
+
+			// history buffer
+			float fMagOld = delay.add(fMag);
+
+			// zero crossing?
+			float tmp = max;
+			if (fMagOld > 0 && fMag < 0) {
+				if (max > threshold) {
+					step = true;
+					gotStep = true;
+				}
+				delay.setAll(0);
+				max = 0;
+			}
+
+			// track maximum value
+			if (fMag > max) {max = fMag; maxTS = ts;}
+
+			#ifdef WITH_DEBUG_OUTPUT
+				if (step) {
+					std::cout << ts.ms() << std::endl;
+					outSteps << maxTS.ms() << " " << tmp << "\n";
+					outSteps.flush();
+				}
+				outFiltered << ts.ms() << " " << fMag << "\n";
+			#endif
+
+			#ifdef WITH_DEBUG_PLOT
+
+				if (plotRef.isZero()) {plotRef = ts;}
+				const Timestamp tsPlot = (ts-plotRef);
+				const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
+
+				lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), data.z) );
+				lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
+
+				if (step) {
+					pointDet.add( K::GnuplotPoint2((maxTS-plotRef).ms(), tmp) );
+				}
+
+				if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+					lastPlot = tsPlot;
+					auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+					lineRaw.removeIf(remove);
+					lineFiltered.removeIf(remove);
+					pointDet.removeIf(remove);
+
+					gp.draw(plot);
+					gp.flush();
+					usleep(100);
+
+				}
+
+			#endif
+
+		};
+
+		// ensure fixed sampling rate for FIR freq filters to work!
+		interpol.add(ts, acc, onResample);
+
+		return gotStep;
+
+
+	}
+
+};
+
+
+
+#endif // STEPDETECTION4_H