From 039f9c4ceefeba03ac72259a58683383e7ed1e0e Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Tue, 3 Jul 2018 10:56:30 +0200 Subject: [PATCH] refactored FIR filters - real and complex variant adjusted StepDetection2 --- math/dsp/{FIRComplex.h => fir/Complex.h} | 0 math/dsp/fir/ComplexFactory.h | 31 ++++++ math/dsp/fir/Real.h | 104 ++++++++++++++++++ math/dsp/fir/RealFactory.h | 133 +++++++++++++++++++++++ sensors/imu/StepDetection.h | 2 +- sensors/imu/StepDetection2.h | 24 ++-- 6 files changed, 285 insertions(+), 9 deletions(-) rename math/dsp/{FIRComplex.h => fir/Complex.h} (100%) create mode 100644 math/dsp/fir/ComplexFactory.h create mode 100644 math/dsp/fir/Real.h create mode 100644 math/dsp/fir/RealFactory.h diff --git a/math/dsp/FIRComplex.h b/math/dsp/fir/Complex.h similarity index 100% rename from math/dsp/FIRComplex.h rename to math/dsp/fir/Complex.h diff --git a/math/dsp/fir/ComplexFactory.h b/math/dsp/fir/ComplexFactory.h new file mode 100644 index 0000000..1532ebd --- /dev/null +++ b/math/dsp/fir/ComplexFactory.h @@ -0,0 +1,31 @@ +#ifndef FIRFACTORY_H +#define FIRFACTORY_H + +#include +#include + + +class FIRFactory { + + float sRate_hz; + +public: + + /** ctor */ + FIRFactory(float sRate_hz) : sRate_hz(sRate_hz) { + ; + } + + + +// static std::vector getRealBandbass(float center_hz, float width_hz, float sRate_hz, int size) { + +// } + +// static std::vector> getComplexBandbass(float center_hz, float width_hz, float sRate_hz, int size) { + +// } + +} + +#endif // FIRFACTORY_H diff --git a/math/dsp/fir/Real.h b/math/dsp/fir/Real.h new file mode 100644 index 0000000..cae949c --- /dev/null +++ b/math/dsp/fir/Real.h @@ -0,0 +1,104 @@ +#ifndef FIRREAL_H +#define FIRREAL_H + +#include +#include +#include +#include "../../../Assertions.h" + +namespace FIR { + + namespace Real { + + using Kernel = std::vector; + using DataVec = std::vector; + + 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 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 diff --git a/math/dsp/fir/RealFactory.h b/math/dsp/fir/RealFactory.h new file mode 100644 index 0000000..95d92a8 --- /dev/null +++ b/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 diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index c19ee7b..99d067c 100644 --- a/sensors/imu/StepDetection.h +++ b/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" /** diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h index eab6e2f..7cee7e6 100644 --- a/sensors/imu/StepDetection2.h +++ b/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 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 c = fir.append(mag0); - const float real = c.real(); - if (real != real) {return;} - const float fMag = real; + //const std::complex c = fir.append(mag0); + //const float real = c.real(); + //if (real != real) {return;} + //const float fMag = real; + const float f = fir.append(mag0); + if (f != f) {return;} + const float fMag = f; const bool isMax = locMax.add(fMag);