worked on step-detection

adjusted iir biquad
added biquad-stacking

math/dsp/iir/BiQuad.h

@@ -97,9 +97,9 @@ namespace IIR {
 
 
 		/** configure the filter as low-pass */
-		void setLowPass( const float freq, const float octaves, const float sRate ) {
+		void setLowPass( const float freq, const float Q, const float sRate ) {
 			double freqFact = double(freq) / double(sRate);
-			setLowPass(freqFact, octaves);
+			setLowPass(freqFact, Q);
 		}
 
 
@@ -159,9 +159,9 @@ namespace IIR {
 		}
 
 		/** configure the filter as high-pass */
-		void setHighPass( const float freq, const float octaves, const float sRate ) {
+		void setHighPass( const float freq, const float Q, const float sRate ) {
 			double freqFact = double(freq) / double(sRate);
-			setHighPass(freqFact, octaves);
+			setHighPass(freqFact, Q);
 		}
 
 		/** configure the filter as band-pass. freqFact between ]0;0.5[ */
@@ -189,9 +189,9 @@ namespace IIR {
 		}
 
 		/** configure the filter as band-pass */
-		void setBandPass( const float freq, const float octaves, float sRate ) {
+		void setBandPass( const float freq, const float Q, float sRate ) {
 			double freqFact = double(freq) / double(sRate);
-			setBandPass(freqFact, octaves);
+			setBandPass(freqFact, Q);
 		}
 
 

math/dsp/iir/BiQuadStack.h

@@ -0,0 +1,42 @@
+#ifndef BIQUADSTACK_H
+#define BIQUADSTACK_H
+
+#include <vector>
+#include "BiQuad.h"
+
+namespace IIR {
+
+	template <typename Scalar> class BiQuadStack {
+
+		std::vector<BiQuad<Scalar>> filters;
+
+	public:
+
+		BiQuadStack() {
+			;
+		}
+
+		BiQuadStack(const int num) {
+			filters.resize(num);
+		}
+
+		void resize(const int num) {
+			filters.resize(num);
+		}
+
+		BiQuad<Scalar>& operator [] (const size_t idx) {
+			return filters.at(idx);
+		}
+
+		Scalar filter(Scalar val) {
+			for (BiQuad<Scalar>& bq : filters) {
+				val = bq.filter(val);
+			}
+			return val;
+		}
+
+	};
+
+}
+
+#endif // BIQUADSTACK_H

sensors/imu/StepDetection2.h

@@ -22,8 +22,9 @@
 #endif
 
 #include "../../Assertions.h"
-#include "../../math/dsp/fir/Real.h"
+#include "../../math/dsp/iir/BiQuadStack.h"
-#include "../../math/dsp/fir/RealFactory.h"
+//#include "../../math/dsp/fir/Real.h"
+//#include "../../math/dsp/fir/RealFactory.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/LocalMaxima.h"
 #include "../../math/MovingAverageTS.h"
@@ -36,19 +37,20 @@
  */
 class StepDetection2 {
 
-	static constexpr int sRate_hz = 75;
+	static constexpr int sRate_hz = 100;
 	static constexpr int every_ms = 1000 / sRate_hz;
 
 private:
 
 	FixedFrequencyInterpolator<AccelerometerData> interpol;
-	FIR::Real::Filter fir;
+	//FIR::Real::Filter fir;
+	IIR::BiQuadStack<float> biquad;
 	LocalMaxima locMax;
 
 	// longterm average to center around zero
 	MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
 
-	const float threshold = 0.50;
+	float threshold = 0.50;
 
 	#ifdef WITH_DEBUG_PLOT
 		K::Gnuplot gp;
@@ -69,16 +71,28 @@ private:
 public:
 
 	/** ctor */
-	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
+	StepDetection2(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
 
 		//fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
 		//fir.shiftBy(2.00);			// typical step freq ~2Hz
 
 		//fir.lowPass(3.5, 25);		// everything up to 3 HZ
 
-		FIR::Real::Factory fac(sRate_hz);
+		//FIR::Real::Factory fac(sRate_hz);
-		fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
+		//fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
 
+		if (useBandPass) {
+			biquad.resize(3);
+			biquad[0].setHighPass(1, 0.7, sRate_hz);
+			biquad[1].setLowPass(3.0, 0.7, sRate_hz);
+			biquad[2].setLowPass(3.0, 1.0, sRate_hz);
+			//biquad.setBandPass(2, 3.0, sRate_hz);
+			threshold = 0.6;	// needs a little reduction
+		} else {
+			threshold = 0.8;
+			biquad.resize(1);
+			biquad[0].setLowPass(3, 0.7, sRate_hz);
+		}
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";
@@ -98,7 +112,7 @@ public:
 	/** does the given data indicate a step? */
 	bool add(const Timestamp ts, const AccelerometerData& acc) {
 
-		bool step = false;
+		bool gotStep = false;
 
 		// accel-data incoming on a fixed sampling rate (needed for FIR to work)
 		auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
@@ -109,17 +123,16 @@ public:
 			avg.add(ts, mag);
 			const float mag0 = mag - avg.get();
 
-			//const std::complex<float> c = fir.append(mag0);
+
-			//const float real = c.real();
+//			const float f = fir.append(mag0);
-			//if (real != real) {return;}
+//			if (f != f) {return;}
-			//const float fMag = real;
+			const float f = biquad.filter(mag0);
-			const float f = fir.append(mag0);
-			if (f != f) {return;}
 			const float fMag = f;
 
 			const bool isMax = locMax.add(fMag);
 
-			step = (isMax) && (locMax.get() > threshold);
+			const bool step = (isMax) && (locMax.get() > threshold);
+			if (step) {gotStep = true;}
 
 			#ifdef WITH_DEBUG_OUTPUT
 				if (step) {
@@ -163,7 +176,7 @@ public:
 		// ensure fixed sampling rate for FIR freq filters to work!
 		interpol.add(ts, acc, onResample);
 
-		return step;
+		return gotStep;
 
 
 	}

sensors/imu/StepDetection3.h

@@ -22,7 +22,7 @@
 #endif
 
 #include "../../Assertions.h"
-#include "../../math/dsp/iir/BiQuad.h"
+#include "../../math/dsp/iir/BiQuadStack.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/DelayBuffer.h"
 
@@ -38,9 +38,12 @@ class StepDetection3 {
 
 	static constexpr float gravity = 9.81;
 	static constexpr float stepRate_hz = 2.0;
+	static constexpr float cutOff_hz = 3.0;
+	static constexpr float iirQ = 0.70;
+
 	static constexpr int sRate_hz = 100;
 	static constexpr int every_ms = 1000 / sRate_hz;
-	static constexpr float threshold = 1.0;
+	float threshold = 0.8;
 
 	float max = 0;
 	Timestamp maxTS;
@@ -48,8 +51,8 @@ class StepDetection3 {
 private:
 
 	FixedFrequencyInterpolator<AccelerometerData> interpol;
-	IIR::BiQuad<float> biquad;
+	IIR::BiQuadStack<float> biquad;
-	DelayBuffer<float> delay;
+	DelayBuffer<float> lookBehind;
 
 
 
@@ -72,10 +75,23 @@ private:
 public:
 
 	/** ctor */
-	StepDetection3() : interpol(Timestamp::fromMS(every_ms)), delay(10) {
+	StepDetection3(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), lookBehind(5) {
 
-		biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
+		//biquad.setBandPass(stepRate_hz, 1.5, sRate_hz);
-		biquad.preFill(gravity);
+		//biquad.preFill(gravity);
+
+		if (useBandPass) {
+			biquad.resize(3);
+			biquad[0].setHighPass(1, 0.7, sRate_hz);
+			biquad[1].setLowPass(3.0, 0.7, sRate_hz);
+			biquad[2].setLowPass(3.0, 1.0, sRate_hz);
+			//biquad.setBandPass(2, 3.0, sRate_hz);
+			threshold = 0.6;	// needs a little reduction
+		} else {
+			threshold = 0.8;
+			biquad.resize(1);
+			biquad[0].setLowPass(3, 0.7, sRate_hz);
+		}
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";
@@ -105,10 +121,10 @@ public:
 			const float mag = data.magnitude();
 
 			// apply filter
-			const float fMag = biquad.filter(mag);
+			const float fMag = biquad.filter(mag - 9.81);	// remove gravity
 
 			// history buffer
-			float fMagOld = delay.add(fMag);
+			float fMagOld = lookBehind.add(fMag);
 
 			// zero crossing?
 			float tmp = max;
@@ -117,16 +133,22 @@ public:
 					step = true;
 					gotStep = true;
 				}
-				delay.setAll(0);
+				lookBehind.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;
+
+//					// track delay due to zero crossing
+//					const float tsDelay = ts.ms() - maxTS.ms();
+//					std::cout << "step at " << ts.ms() << ". delay due to zero crossing: " << tsDelay << std::endl;
+
 					outSteps << maxTS.ms() << " " << tmp << "\n";
 					outSteps.flush();
 				}

sensors/imu/StepDetection4.h

@@ -24,7 +24,7 @@
 #endif
 
 #include "../../Assertions.h"
-#include "../../math/dsp/iir/BiQuad.h"
+#include "../../math/dsp/iir/BiQuadStack.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/DelayBuffer.h"
 
@@ -43,7 +43,7 @@ class StepDetection4 {
 	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 threshold = 0.8;
 
 	float max = 0;
 	Timestamp maxTS;
@@ -52,7 +52,7 @@ private:
 
 	PoseDetection* pose;
 	FixedFrequencyInterpolator<Vector3> interpol;
-	IIR::BiQuad<float> biquad;
+	IIR::BiQuadStack<float> biquad;
 	DelayBuffer<float> delay;
 
 
@@ -75,16 +75,30 @@ private:
 public:
 
 	/** ctor */
-	StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
+	StepDetection4(PoseDetection* pose, bool useBandPass) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
 
-		plot.getKey().setVisible(true);
+		//biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
-		lineRaw.setTitle("unrotated Z");
+		//biquad.preFill(gravity);
-		lineFiltered.setTitle("IIR filtered");
 
-		biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
+		if (useBandPass) {
-		biquad.preFill(gravity);
+			biquad.resize(3);
+			biquad[0].setHighPass(1, 0.7, sRate_hz);
+			biquad[1].setLowPass(3.0, 0.7, sRate_hz);
+			biquad[2].setLowPass(3.0, 1.0, sRate_hz);
+			//biquad.setBandPass(2, 3.0, sRate_hz);
+			threshold = 0.6;	// needs a little reduction
+		} else {
+			threshold = 0.8;
+			biquad.resize(1);
+			biquad[0].setLowPass(3, 0.7, sRate_hz);
+		}
 
 		#ifdef WITH_DEBUG_PLOT
+
+		    plot.getKey().setVisible(true);
+			lineRaw.setTitle("unrotated Z");
+			lineFiltered.setTitle("IIR filtered");
+
 			gp << "set autoscale xfix\n";
 			plot.setTitle("Step Detection");
 			plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
@@ -123,7 +137,7 @@ public:
 			const float mag = data.z;
 
 			// apply filter
-			const float fMag = biquad.filter(mag);
+			const float fMag = biquad.filter(mag - gravity);
 
 			// history buffer
 			float fMagOld = delay.add(fMag);