worked on FIR-Convolution and LocalMaxima detection

main.cpp

@@ -10,7 +10,7 @@ class Test : public GridPoint {
 
 #include "tests/Tests.h"
 
-#include "sensors/radio/scan/WiFiScanLinux.h"
+
 #include "sensors/radio/VAPGrouper.h"
 
 #include <KLib/misc/gnuplot/Gnuplot.h>
@@ -18,6 +18,9 @@ class Test : public GridPoint {
 #include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
 #include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
 
+#ifdef WIFI_LINUX
+#include "sensors/radio/scan/WiFiScanLinux.h"
+
 void wifi() {
 
 	K::Gnuplot gp;
@@ -76,10 +79,9 @@ void wifi() {
 
 	}
 
-
-
-
 }
+#endif
+
 
 int main(int argc, char** argv) {
 
@@ -109,7 +111,7 @@ int main(int argc, char** argv) {
 	//::testing::GTEST_FLAG(filter) = "*Matrix4*";
 	//::testing::GTEST_FLAG(filter) = "*Sphere3*";
 
-	::testing::GTEST_FLAG(filter) = "Ray.ModelFac*";
+	::testing::GTEST_FLAG(filter) = "*FIRComplex*";
 	//::testing::GTEST_FLAG(filter) = "Timestamp*";
 
 	//::testing::GTEST_FLAG(filter) = "*RayTrace3*";

math/LocalMaxima.h

@@ -0,0 +1,64 @@
+#ifndef LOCALMAXIMA_H
+#define LOCALMAXIMA_H
+
+class LocalMaxima {
+
+	static constexpr float MAX = 1e40;
+
+	size_t everyNth;
+
+	size_t cnt = 0;
+	float s0;
+	float s1;		// center value
+	float s2;
+
+
+public:
+
+	struct Res {
+		bool isMax;
+		float val;
+		Res(bool isMax, float val) : isMax(isMax), val(val) {;}
+	};
+
+	/** ctor. use only every n-th sample */
+	LocalMaxima(const size_t everyNth) : everyNth(everyNth) {
+		reset();
+	}
+
+	/** is the given value a local maxima? */
+	Res add(const float s) {
+
+		if		(cnt == 0*everyNth) {s0 = s;}	// set, wait some time
+		else if (cnt == 1*everyNth) {s1 = s;}	// set, wait some time
+		else if (cnt == 2*everyNth) {s2 = s;}	// set
+		else if (cnt  > 2*everyNth) {			// now shift values for every time step, until max is found
+			s0 = s1;
+			s1 = s2;
+			s2 = s;
+		}
+
+		++cnt;
+
+		if ((s1 > s0) && (s1 > s2)) {
+			Res res(true, s1);
+			reset();
+			return res;
+		}
+
+		return Res(false, 0);
+
+	}
+
+private:
+
+	void reset() {
+		s0 = MAX;
+		s1 = MAX;
+		s2 = MAX;
+		cnt = 0;
+	}
+
+};
+
+#endif // LOCALMAXIMA_H

math/dsp/Convolution.h

@@ -0,0 +1,10 @@
+#ifndef CONVOLUTION_H
+#define CONVOLUTION_H
+
+class Convolution {
+
+
+
+};
+
+#endif // CONVOLUTION_H

math/dsp/FIRComplex.h

@@ -0,0 +1,214 @@
+#ifndef FIRCOMPLEX_H
+#define FIRCOMPLEX_H
+
+#include <vector>
+#include <complex>
+#include "../../Assertions.h"
+
+/**
+ * FIR filter using complex convolution
+ */
+class FIRComplex {
+
+	/** signal's sample-rate */
+	int sRate_hz;
+
+	/** the created convolution kernel */
+	std::vector<std::complex<float>> kernel;
+
+	/** incoming data */
+	std::vector<std::complex<float>> data;
+
+public:
+
+	/** ctor with signal's sample-rate */
+	FIRComplex(const int sRate_hz) : sRate_hz(sRate_hz) {
+		;
+	}
+
+	/** get the internal kernel */
+	const std::vector<std::complex<float>> getKernel() const {
+		return kernel;
+	}
+
+	/** configure as lowpass with the given cutoff and 2*size+1 */
+	void lowPass(const int cutOff_hz, const int size) {
+		this->kernel = getLowpass(cutOff_hz, sRate_hz, size);
+	}
+
+	/** shift the constructed filter by the given hz-rate */
+	void shiftBy(const int shift_hz) {
+		shiftKernel(shift_hz, sRate_hz);
+	}
+
+	/** filter the given incoming real data */
+	std::vector<std::complex<float>> append(const std::vector<float>& newData) {
+
+		// append to local buffer (as we need some history)
+		//data.insert(data.end(), newData.begin(), newData.end());
+		for (const float f : newData) {
+			data.push_back(std::complex<float>(f, 0));	// real = value, imag = 0;
+		}
+
+		return processLocalBuffer();
+
+	}
+
+	/** filter the given incoming complex data */
+	std::vector<std::complex<float>> append(const std::vector<std::complex<float>>& 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 */
+	std::complex<float> append(const float val) {
+		data.push_back(std::complex<float>(val, 0));
+		auto tmp = processLocalBuffer();
+		if (tmp.size() == 0) {return std::complex<float>(NAN, NAN);}
+		if (tmp.size() == 1) {return tmp[0];}
+		throw Exception("FIRComplex:: detected invalid result");
+	}
+
+	/** filter the given incoming real value */
+	std::complex<float> append(const std::complex<float> c) {
+		data.push_back(c);
+		auto tmp = processLocalBuffer();
+		if (tmp.size() == 0) {return std::complex<float>(NAN, NAN);}
+		if (tmp.size() == 1) {return tmp[0];}
+		throw Exception("FIRComplex:: detected invalid result");
+	}
+
+	void dumpKernel(const std::string& file, const std::string& varName) {
+
+		std::ofstream out(file);
+		out << "# name: " << varName << "\n";
+		out << "# type: complex matrix\n";
+		out << "# rows: " << kernel.size() << "\n";
+		out << "# columns: 1\n";
+
+		for (const std::complex<float> c : kernel) {
+			out << "(" << c.real() << "," << c.imag() << ")" << "\n";
+		}
+		out.close();
+
+	}
+
+
+private:
+
+	std::vector<std::complex<float>> 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 std::vector<std::complex<float>>();}
+
+		// result-vector
+		std::vector<std::complex<float>> 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 std::complex<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;
+		}
+
+	}
+
+
+//	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 += std::complex<float>(src[j+i], 0) * kernel[j];
+//			}
+//			if (t != t) {throw std::runtime_error("detected NaN");}
+//			dst[i] = t;
+//		}
+
+//	}
+
+	/** 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);
+	}
+
+	/** frequency shift the kernel by multiplying with a frequency */
+	void shiftKernel(const int shift_hz, const int sRate_hz) {
+		for (size_t i = 0; i < kernel.size(); ++i) {
+			const float t = (float) i / (float) sRate_hz;
+			const float real = std::cos(t * 2 * M_PI * shift_hz);
+			const float imag = std::sin(t * 2 * M_PI * shift_hz);
+			kernel[i] = kernel[i] * std::complex<float>(real, imag);
+		}
+	}
+
+	// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+	/** normalize using the DC-part of the kernel */
+	static void normalizeDC(std::vector<std::complex<float>>& kernel) {
+		std::complex<float> sum;
+		for (auto f : kernel) {sum += f;}
+		for (auto& f : kernel) {f /= sum;}
+	}
+
+	// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+	static void normalizeAC(std::vector<std::complex<float>>& kernel, const float freq) {
+		throw std::runtime_error("TODO");
+	}
+
+	/** build a lowpass filter kernel */
+	static std::vector<std::complex<float>> getLowpass(const int cutOff, const int sRate, const int n) {
+
+		std::vector<std::complex<float>> kernel;
+
+		for (int i = -n; i <= +n; ++i) {
+
+			const double t = (double) i / (double) sRate;
+			const double tmp = 2 * M_PI * cutOff * t;
+			const double val = (tmp == 0) ? (1) : (std::sin(tmp) / tmp);
+			const double win = winHamming(i+n, n*2);
+			const double res = val * win;// * 0.5f;	// why 0.5?
+			if (res != res) {throw std::runtime_error("detected NaN");}
+			kernel.push_back( std::complex<float>(res, 0) );
+
+		}
+
+		// important!!! normalize so the original frequencies stay at 0dB
+		normalizeDC(kernel);	// dc works for low-pass filter only as this one contains DC!
+
+		return kernel;
+
+	}
+
+};
+
+#endif // FIRCOMPLEX_H

sensors/imu/StepDetection.h

@@ -19,6 +19,7 @@
 
 #include "../../Assertions.h"
 #include "../../math/MovingAverageTS.h"
+#include "../../math/dsp/FIRComplex.h"
 
 
 /**
@@ -145,88 +146,6 @@ public:
 
 	}
 
-
-//private:
-
-//	/** low pass acc-magnitude */
-//	float avg1 = 0;
-
-//	/** even-more low-pass acc-magnitude */
-//	float avg2 = 0;
-
-//private:
-
-//	class Stepper {
-
-//	private:
-
-//		/** block for 300 ms after every step */
-//		const Timestamp blockTime = Timestamp::fromMS(300);
-
-//		/** the threshold for detecting a spike as step */
-//		const float threshold = 0.30;
-
-//		/** block until the given timestamp before detecting additional steps */
-//		Timestamp blockUntil;
-
-
-//	public:
-
-//		/** is the given (relative!) magnitude (mag - ~9.81) a step? */
-//		bool isStep(const Timestamp ts, const float mag) {
-
-//			// still blocking
-//			if (ts < blockUntil) {
-//				return false;
-//			}
-
-//			// threshold reached? -> step!
-//			if (mag > threshold) {
-
-//				// block x milliseconds until detecting the next step
-//				blockUntil = ts + blockTime;
-
-//				// we have a step
-//				return true;
-
-//			}
-
-//			// no step
-//			return false;
-
-//		}
-
-
-//	};
-
-//	Stepper stepper;
-
-//public:
-
-//	/** does the given data indicate a step? */
-//	bool add(const Timestamp ts, const AccelerometerData& acc) {
-
-//		avg1 = avg1 * 0.91 + acc.magnitude() * 0.09;	// short-time average	[filtered steps]
-//		avg2 = avg2 * 0.97 + acc.magnitude() * 0.03;	// long-time average	[gravity]
-
-//		// average maginitude must be > 9.0 to be stable enough to proceed
-//		if (avg2 > 9) {
-
-//			// gravity-free magnitude
-//			const float avg = avg1 - avg2;
-
-//			// detect steps
-//			return stepper.isStep(ts, avg);
-
-//		} else {
-
-//			return false;
-
-//		}
-
-//	}
-
-
 };
 
 

sensors/imu/StepDetection2.h

@@ -0,0 +1,149 @@
+#ifndef STEPDETECTION2_H
+#define STEPDETECTION2_H
+
+
+#include "AccelerometerData.h"
+#include "../../data/Timestamp.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/FIRComplex.h"
+#include "../../math/FixedFrequencyInterpolator.h"
+#include "../../math/LocalMaxima.h"
+
+
+/**
+ * simple step detection based on accelerometer magnitude.
+ * magnitude > threshold? -> step!
+ * block for several msec until detecting the next one
+ */
+class StepDetection2 {
+
+	static constexpr int sRate_hz = 75;
+	static constexpr int every_ms = 1000 / sRate_hz;
+
+private:
+
+	FixedFrequencyInterpolator<AccelerometerData> interpol;
+	FIRComplex fir;
+	LocalMaxima locMax;
+
+	const float threshold = 0.5;
+
+	#ifdef WITH_DEBUG_PLOT
+		K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines lineMag;
+		K::GnuplotPlotElementPoints pointDet;
+		Timestamp plotRef;
+		Timestamp lastPlot;
+	#endif
+
+	#ifdef WITH_DEBUG_OUTPUT
+		std::ofstream outFiltered;
+		std::ofstream outSteps;
+	#endif
+
+
+public:
+
+	/** ctor */
+	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) {
+
+		fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
+		fir.shiftBy(2);			// typical step freq ~2Hz
+
+		#ifdef WITH_DEBUG_PLOT
+			gp << "set autoscale xfix\n";
+			plot.setTitle("Step Detection");
+			plot.add(&lineMag); lineMag.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) {
+
+		bool step = false;
+
+		auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
+
+			const float mag = data.magnitude();
+
+			const std::complex<float> c = fir.append(mag);
+			const float real = c.real();
+			if (real != real) {return;}
+			const float fMag = real;
+
+			LocalMaxima::Res res = locMax.add(fMag);
+			step = (res.isMax) && (res.val > threshold);
+
+			#ifdef WITH_DEBUG_OUTPUT
+				if (step) {
+					outSteps << ts.ms() << " " << fMag << "\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);
+
+				lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
+
+				if (step) {
+					pointDet.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
+				}
+
+				if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
+
+					lastPlot = tsPlot;
+					auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
+					lineMag.removeIf(remove);
+					pointDet.removeIf(remove);
+
+					gp.draw(plot);
+					gp.flush();
+					usleep(100);
+
+				}
+
+			#endif
+
+		};
+
+		interpol.add(ts, acc, onResample);
+
+		return step;
+
+
+	}
+
+};
+
+
+#endif // STEPDETECTION2_H

tests/math/dsp/TestFIRComplex.cpp

@@ -0,0 +1,45 @@
+#ifdef WITH_TESTS
+
+#include <fstream>
+#include "../../Tests.h"
+#include "../../../math/dsp/FIRComplex.h"
+#include <random>
+
+
+TEST(FIRComplex, filter1) {
+
+	const float sRate = 200;
+	const float freq = 10;
+
+	FIRComplex f(sRate);
+	f.lowPass(5, 50);	f.dumpKernel("/tmp/k1.m", "k1");
+	f.shiftBy(freq);	f.dumpKernel("/tmp/k2.m", "k2");
+
+	std::minstd_rand gen;
+	std::normal_distribution<float> noise(0.0, 0.3);
+
+	std::vector<std::complex<float>> out;
+
+	std::ofstream fileO("/tmp/orig.dat");
+	std::ofstream fileF("/tmp/filtered.dat");
+
+	for (int i = 0; i < 1000; ++i) {
+		const float t = i / sRate;
+		const float n = noise(gen);
+		const float s = std::sin(2*M_PI*freq*t);
+		const float v = s+n;
+		std::vector<float> values;
+		values.push_back(s+n);
+		fileO << v << "\n";
+		std::vector<std::complex<float>> res = f.append(values);
+		out.insert(out.end(), res.begin(), res.end());
+	}
+
+
+	for (const std::complex<float> c : out) {
+		fileF << c.real() << "\n";
+	}
+
+}
+
+#endif