refactored FIR filters

- real and complex variant
adjusted StepDetection2

math/dsp/fir/ComplexFactory.h

@@ -0,0 +1,31 @@
+#ifndef FIRFACTORY_H
+#define FIRFACTORY_H
+
+#include <vector>
+#include <complex>
+
+
+class FIRFactory {
+
+	float sRate_hz;
+
+public:
+
+	/** ctor */
+	FIRFactory(float sRate_hz) : sRate_hz(sRate_hz) {
+		;
+	}
+
+
+
+//	static std::vector<float> getRealBandbass(float center_hz, float width_hz, float sRate_hz, int size) {
+
+//	}
+
+//	static std::vector<std::complex<float>> getComplexBandbass(float center_hz, float width_hz, float sRate_hz, int size) {
+
+//	}
+
+}
+
+#endif // FIRFACTORY_H

math/dsp/fir/Real.h

@@ -0,0 +1,104 @@
+#ifndef FIRREAL_H
+#define FIRREAL_H
+
+#include <vector>
+#include <cmath>
+#include <string>
+#include "../../../Assertions.h"
+
+namespace FIR {
+
+	namespace Real {
+
+		using Kernel = std::vector<float>;
+		using DataVec = std::vector<float>;
+
+		class Filter {
+
+			Kernel kernel;
+			DataVec data;
+
+		public:
+
+			/** ctor */
+			Filter(const Kernel& kernel) : kernel(kernel) {
+				;
+			}
+
+			/** empty ctor */
+			Filter() : kernel() {
+				;
+			}
+
+			/** set the filter-kernel */
+			void setKernel(const Kernel& kernel) {
+				this->kernel = kernel;
+			}
+
+			/** filter the given incoming real data */
+			DataVec append(const DataVec& newData) {
+				// append to local buffer (as we need some history)
+				data.insert(data.end(), newData.begin(), newData.end());
+				return processLocalBuffer();
+			}
+
+			/** filter the given incoming real value */
+			float append(const float val) {
+				data.push_back(val);
+				auto tmp = processLocalBuffer();
+				if (tmp.size() == 0) {return NAN;}
+				if (tmp.size() == 1) {return tmp[0];}
+				throw Exception("FIR::Real::Filter detected invalid result");
+			}
+
+		private:
+
+			DataVec processLocalBuffer() {
+
+				// sanity check
+				Assert::isNot0(kernel.size(), "FIRComplex:: kernel not yet configured!");
+
+				// number of processable elements (due to filter size)
+				const int processable = data.size() - kernel.size() + 1 - kernel.size()/2;
+
+				// nothing to-do?
+				if (processable <= 0) {return DataVec();}
+
+				// result-vector
+				DataVec res;
+				res.resize(processable);
+
+				// fire
+				convolve(data.data(), res.data(), processable);
+
+				// drop processed elements from the local buffer
+				data.erase(data.begin(), data.begin() + processable);
+
+				// done
+				return res;
+
+			}
+
+			template <typename T> void convolve(const float* src, T* dst, const size_t cnt) {
+
+				const size_t ks = kernel.size();
+
+				for (size_t i = 0; i < cnt; ++i) {
+					T t = T();
+					for (size_t j = 0; j < ks; ++j) {
+						t += src[j+i] * kernel[j];
+					}
+					if (t != t) {throw std::runtime_error("detected NaN");}
+					dst[i] = t;
+				}
+
+			}
+
+
+		};
+
+	}
+
+}
+
+#endif // FIRREAL_H

math/dsp/fir/RealFactory.h

@@ -0,0 +1,133 @@
+#ifndef FIRREALFACTORY_H
+#define FIRREALFACTORY_H
+
+#include "Real.h"
+
+namespace FIR {
+
+	namespace Real {
+
+		class Factory {
+
+			Kernel kernel;
+
+			float sRate_hz;
+
+		public:
+
+			/** ctor */
+			Factory(float sRate_hz) : sRate_hz(sRate_hz) {
+				;
+			}
+
+			/** frequency shift the kernel by multiplying with a frequency */
+			void shift(const float shift_hz) {
+				for (size_t i = 0; i < kernel.size(); ++i) {
+					const float t = (float) i / (float) sRate_hz;
+					const float real = std::sin(t * 2 * M_PI * shift_hz);
+					kernel[i] = kernel[i] * real;
+				}
+			}
+
+			/** create a lowpass filte kernel */
+			void lowpass(const float cutOff_hz, const int n = 50) {
+
+				kernel.clear();
+
+				for (int i = -n; i <= +n; ++i) {
+					const double t = (double) i / (double) sRate_hz;
+					const double tmp = 2 * M_PI * cutOff_hz * t;
+					const double val = (tmp == 0) ? (1) : (std::sin(tmp) / tmp);
+					const double res = val;// * 0.5f;	// why 0.5?
+					if (res != res) {throw std::runtime_error("detected NaN");}
+					kernel.push_back(res);
+				}
+
+			}
+
+			/** apply hamming window to the filter */
+			void applyWindowHamming() {
+				const int n = (kernel.size()-1)/2;
+				int i = -n;
+				for (float& f : kernel) {
+					f *= winHamming(i+n, n*2);
+					++i;
+				}
+			}
+
+			// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+			/** normalize using the DC-part of the kernel */
+			void normalizeDC() {
+				float sum = 0;
+				for (auto f : kernel) {sum += f;}
+				for (auto& f : kernel) {f /= sum;}
+			}
+
+			// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+			void normalizeAC(const float freq_hz) {
+
+				const int n = (kernel.size()-1)/2;
+				int i = -n;
+				float sum = 0;
+
+				for (float f : kernel) {
+					const double t = (double) i / (double) sRate_hz;
+					const double r = 2 * M_PI * freq_hz * t;
+					const double s = std::sin(r);
+					sum += f * s;
+					++i;
+				}
+
+				for (auto& f : kernel) {f /= sum;}
+
+			}
+
+
+
+
+
+			Kernel getLowpass(const float cutOff_hz, const int n) {
+				lowpass(cutOff_hz, n);
+				applyWindowHamming();
+				normalizeDC();
+				return kernel;
+			}
+
+			Kernel getBandpass(const float width_hz, const float center_hz, const int n) {
+				lowpass(width_hz/2, n);
+				applyWindowHamming();
+				//normalizeDC();
+				shift(center_hz);
+				normalizeAC(center_hz);
+				return kernel;
+			}
+
+			void dumpKernel(const std::string& file, const std::string& varName) {
+
+				std::ofstream out(file);
+				out << "# name: " << varName << "\n";
+				out << "# type: matrix\n";
+				out << "# rows: " << kernel.size() << "\n";
+				out << "# columns: 1\n";
+
+				for (const float f : kernel) {
+					out << f << "\n";
+				}
+				out.close();
+
+			}
+
+		private:
+
+			/** get a value from the hamming window */
+			static double winHamming(const double t, const double size) {
+				return 0.54 - 0.46 * std::cos(2 * M_PI * t / size);
+			}
+
+		};
+
+	}
+
+}
+
+#endif // FIRREALFACTORY_H

sensors/imu/StepDetection.h

@@ -19,7 +19,7 @@
 
 #include "../../Assertions.h"
 #include "../../math/MovingAverageTS.h"
-#include "../../math/dsp/FIRComplex.h"
+//#include "../../math/dsp/fir/RComplex.h"
 
 
 /**

sensors/imu/StepDetection2.h

@@ -22,7 +22,8 @@
 #endif
 
 #include "../../Assertions.h"
-#include "../../math/dsp/FIRComplex.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"
@@ -41,7 +42,7 @@ class StepDetection2 {
 private:
 
 	FixedFrequencyInterpolator<AccelerometerData> interpol;
-	FIRComplex fir;
+	FIR::Real::Filter fir;
 	LocalMaxima locMax;
 
 	// longterm average to center around zero
@@ -68,12 +69,16 @@ private:
 public:
 
 	/** ctor */
-	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(8) {
+	StepDetection2() : 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.lowPass(3.5, 25);		// everything up to 3 HZ
+
+		FIR::Real::Factory fac(sRate_hz);
+		fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
+
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";
@@ -104,10 +109,13 @@ public:
 			avg.add(ts, mag);
 			const float mag0 = mag - avg.get();
 
-			const std::complex<float> c = fir.append(mag0);
+			//const std::complex<float> c = fir.append(mag0);
-			const float real = c.real();
+			//const float real = c.real();
-			if (real != real) {return;}
+			//if (real != real) {return;}
-			const float fMag = real;
+			//const float fMag = real;
+			const float f = fir.append(mag0);
+			if (f != f) {return;}
+			const float fMag = f;
 
 			const bool isMax = locMax.add(fMag);