From 2dee085131c5efc93107fd764d779d95c9061fa9 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 17 Jul 2018 09:53:30 +0200 Subject: [PATCH] added IIR stuff worked on StepDetection --- math/DelayBuffer.h | 33 ++++ math/dsp/fir/Complex.h | 2 +- math/dsp/iir/BiQuad.h | 322 +++++++++++++++++++++++++++++++++++ sensors/imu/StepDetection3.h | 176 +++++++++++++++++++ 4 files changed, 532 insertions(+), 1 deletion(-) create mode 100644 math/DelayBuffer.h create mode 100644 math/dsp/iir/BiQuad.h create mode 100644 sensors/imu/StepDetection3.h diff --git a/math/DelayBuffer.h b/math/DelayBuffer.h new file mode 100644 index 0000000..dbe44cf --- /dev/null +++ b/math/DelayBuffer.h @@ -0,0 +1,33 @@ +#ifndef DELAYBUFFER_H +#define DELAYBUFFER_H + +#include + +/** efficient delay using a ring-buffer */ +template class DelayBuffer { + + size_t head = 0; + std::vector vec; + +public: + + /** ctor */ + DelayBuffer(int size) { + vec.resize(size); + } + + /** set all elements to the same value */ + void setAll(const Scalar s) { + std::fill(vec.begin(), vec.end(), s); + } + + /** append a new element, get the delayed output */ + Scalar add(Scalar s) { + vec[head] = s; + head = (head + 1) % vec.size(); // next to-be-overwritten element = oldest element = tail + return vec[head]; + } + +}; + +#endif // DELAYBUFFER_H diff --git a/math/dsp/fir/Complex.h b/math/dsp/fir/Complex.h index 0399608..513ef6a 100644 --- a/math/dsp/fir/Complex.h +++ b/math/dsp/fir/Complex.h @@ -3,7 +3,7 @@ #include #include -#include "../../Assertions.h" +#include "../../../Assertions.h" /** * FIR filter using complex convolution diff --git a/math/dsp/iir/BiQuad.h b/math/dsp/iir/BiQuad.h new file mode 100644 index 0000000..39bd679 --- /dev/null +++ b/math/dsp/iir/BiQuad.h @@ -0,0 +1,322 @@ +#ifndef IIR_BIQUAD +#define IIR_BIQUAD + +#include +#include "../../../Assertions.h" + +namespace IIR { + + + /** frequency limits */ + #define BFG_MIN 0.0001 + #define BFG_MAX 0.4999 + + + /** + * a simple biquad filter that can be used + * for low- or high-pass filtering + * http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt + */ + template class BiQuad { + + public: + + /** ctor */ + BiQuad() : in(), out() { + reset(); + } + + /** filter the given amplitude of the given channel (history) */ + Scalar filter( const Scalar aIn ) { + + Scalar aOut = 0; + aOut += aIn *(b0a0); + aOut += in[0] *(b1a0); + aOut += in[1] *(b2a0); + aOut -= out[0]*(a1a0); + aOut -= out[1]*(a2a0); + + in[1] = in[0]; + in[0] = aIn; + + out[1] = out[0]; + out[0] = aOut; + + return aOut; + + } + + void preFill(const Scalar s) { + for (int i = 0; i < 100; ++i) { + filter(s); + } + } + + /** reset (disable) the filter */ + void reset() { + + b0a0 = 1.0; + b1a0 = 0.0; + b2a0 = 0.0; + a1a0 = 0.0; + a2a0 = 0.0; + + memset(in, 0, sizeof(in)); + memset(out, 0, sizeof(out)); + + } + + /** configure the filter as low-pass. freqFact between ]0;0.5[ */ + void setLowPass( double freqFact, const float octaves ) { + + sanityCheck(freqFact); + + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = (1.0 - cos(w0))/2.0; + double b1 = 1.0 - cos(w0); + double b2 = (1.0 - cos(w0))/2.0; + double a0 = 1.0 + alpha; + double a1 = -2.0*cos(w0); + double a2 = 1.0 - alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + + /** configure the filter as low-pass */ + void setLowPass( const float freq, const float octaves, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setLowPass(freqFact, octaves); + } + + + + //http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter + //http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp + /** + * configure as low-pass filter with resonance + * @param freqFact the frequency factor between ]0;0.5[ + * @param res + */ + void setLowPassResonance( double freqFact, float res ) { + + sanityCheck(freqFact); + + res *= 10; + + double g = pow(10.0, 0.05 * res); + double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2); + + double theta = M_PI * freqFact; + double sn = 0.5 * d * sin(theta); + double beta = 0.5 * (1 - sn) / (1 + sn); + double gamma = (0.5 + beta) * cos(theta); + double alpha = 0.25 * (0.5 + beta - gamma); + + double a0 = 1.0; + double b0 = 2.0 * alpha; + double b1 = 2.0 * 2.0 * alpha; + double b2 = 2.0 * alpha; + double a1 = 2.0 * -gamma; + double a2 = 2.0 * beta; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure the filter as high-pass. freqFact between ]0;0.5[ */ + void setHighPass( double freqFact, const float octaves ) { + + sanityCheck(freqFact); + + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = (1.0 + cos(w0))/2.0; + double b1 = -(1.0 + cos(w0)); + double b2 = (1.0 + cos(w0))/2.0; + double a0 = 1.0 + alpha; + double a1 = -2.0*cos(w0); + double a2 = 1.0 - alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure the filter as high-pass */ + void setHighPass( const float freq, const float octaves, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setHighPass(freqFact, octaves); + } + + /** configure the filter as band-pass. freqFact between ]0;0.5[ */ + void setBandPass( double freqFact, const float octaves ) { + + sanityCheck(freqFact); + + //double w0 = 2 * K_PI * / 2 / freqFact; + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = sin(w0)/2.0; + double b1 = 0.0; + double b2 = -sin(w0)/2.0; + double a0 = 1.0 + alpha; + double a1 = -2.0*cos(w0); + double a2 = 1.0 - alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure the filter as band-pass */ + void setBandPass( const float freq, const float octaves, float sRate ) { + double freqFact = double(freq) / double(sRate); + setBandPass(freqFact, octaves); + } + + + /** configure the filter as all-pass. freqFact between ]0;0.5[ */ + void setAllPass( double freqFact, const float octaves ) { + + sanityCheck(freqFact); + + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = 1 - alpha; + double b1 = -2*cos(w0); + double b2 = 1 + alpha; + double a0 = 1 + alpha; + double a1 = -2*cos(w0); + double a2 = 1 - alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure the filter as all-pass */ + void setAllPass( const float freq, const float octaves, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setAllPass(freqFact, octaves); + } + + /** configure as notch filter. freqFact between ]0;0.5[ */ + void setNotch( double freqFact, const float octaves ) { + + sanityCheck(freqFact); + + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = 1.0; + double b1 = -2.0*cos(w0); + double b2 = 1.0; + double a0 = 1.0 + alpha; + double a1 = -2.0*cos(w0); + double a2 = 1.0 - alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure as notch filter */ + void setNotch( const float freq, const float octaves, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setNotch(freqFact, octaves); + } + + /** configure the filter as low-shelf. increase all aplitudes below freq? freqFact between ]0;0.5[ */ + void setLowShelf( double freqFact, const float octaves, const float gain ) { + + sanityCheck(freqFact); + + double A = sqrt( pow(10, (gain/20.0)) ); + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = A*( (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); + double b1 = 2.0*A*( (A-1.0) - (A+1.0)*cos(w0) ); + double b2 = A*( (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); + double a0 = (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; + double a1 = -2.0*( (A-1.0) + (A+1.0)*cos(w0) ); + double a2 = (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + /** configure the filter as low-shelf. increase all aplitudes below freq? */ + void setLowShelf( const float freq, const float octaves, const float gain, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setLowShelf(freqFact, octaves, gain); + } + + /** configure the filter as high-shelf. increase all amplitues above freq? freqFact between ]0;0.5[ */ + void setHighShelf( double freqFact, const float octaves, const float gain ) { + + sanityCheck(freqFact); + + double A = sqrt( pow(10, (gain/20.0)) ); + double w0 = 2.0 * M_PI * freqFact; + double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) ); + + double b0 = A*( (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha ); + double b1 = -2.0*A*( (A-1.0) + (A+1.0)*cos(w0) ); + double b2 = A*( (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha ); + double a0 = (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha; + double a1 = 2.0*( (A-1.0) - (A+1.0)*cos(w0) ); + double a2 = (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha; + + setValues(a0, a1, a2, b0, b1, b2); + + } + + + /** configure the filter as high-shelf. increase all amplitues above freq? */ + void setHighShelf( const float freq, const float octaves, const float gain, const float sRate ) { + double freqFact = double(freq) / double(sRate); + setHighShelf(freqFact, octaves, gain); + } + + + protected: + + /** pre-calculate the quotients for the filtering */ + void setValues(double a0, double a1, double a2, double b0, double b1, double b2) { + b0a0 = float(b0/a0); + b1a0 = float(b1/a0); + b2a0 = float(b2/a0); + a2a0 = float(a2/a0); + a1a0 = float(a1/a0); + } + + /** the bi-quad filter params */ + float b0a0; + float b1a0; + float b2a0; + + float a1a0; + float a2a0; + + /** history for input values, per channel */ + Scalar in[2]; + + /** history for ouput values, per channel */ + Scalar out[2]; + + void sanityCheck(const float freqFact) const { + Assert::isTrue(freqFact >= BFG_MIN, "frequency out of bounds"); + Assert::isTrue(freqFact <= BFG_MAX, "frequency out of bounds"); + } + + }; + + +} + + +#endif // IIR_BIQUAD diff --git a/sensors/imu/StepDetection3.h b/sensors/imu/StepDetection3.h new file mode 100644 index 0000000..fe32814 --- /dev/null +++ b/sensors/imu/StepDetection3.h @@ -0,0 +1,176 @@ +#ifndef STEPDETECTION3_H +#define STEPDETECTION3_H + + +#include "AccelerometerData.h" +#include "../../data/Timestamp.h" + +#include +#include + +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include +#endif + +#ifdef WITH_DEBUG_OUTPUT + #include +#endif + +#include "../../Assertions.h" +#include "../../math/dsp/iir/BiQuad.h" +#include "../../math/FixedFrequencyInterpolator.h" +#include "../../math/DelayBuffer.h" + + +/** + * simple step detection based on accelerometer magnitude. + * magnitude > threshold? -> step! + * block for several msec until detecting the next one + */ +class StepDetection3 { + + 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: + + FixedFrequencyInterpolator interpol; + IIR::BiQuad biquad; + DelayBuffer 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 */ + StepDetection3() : interpol(Timestamp::fromMS(every_ms)), delay(10) { + + 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) { + + 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 AccelerometerData data) { + + bool step = false; + const float mag = data.magnitude(); + + // 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(), mag) ); + 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 // STEPDETECTION3_H