From f990485d44a34ace7e57ac1b1a0f1f7c0830e33a Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 7 Aug 2018 19:36:07 +0200 Subject: [PATCH] worked on step-detection adjusted iir biquad added biquad-stacking --- math/dsp/iir/BiQuad.h | 12 ++++----- math/dsp/iir/BiQuadStack.h | 42 ++++++++++++++++++++++++++++++++ sensors/imu/StepDetection2.h | 47 +++++++++++++++++++++++------------- sensors/imu/StepDetection3.h | 44 ++++++++++++++++++++++++--------- sensors/imu/StepDetection4.h | 34 ++++++++++++++++++-------- 5 files changed, 135 insertions(+), 44 deletions(-) create mode 100644 math/dsp/iir/BiQuadStack.h diff --git a/math/dsp/iir/BiQuad.h b/math/dsp/iir/BiQuad.h index 67aee40..ad15eb8 100644 --- a/math/dsp/iir/BiQuad.h +++ b/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); } diff --git a/math/dsp/iir/BiQuadStack.h b/math/dsp/iir/BiQuadStack.h new file mode 100644 index 0000000..6c92203 --- /dev/null +++ b/math/dsp/iir/BiQuadStack.h @@ -0,0 +1,42 @@ +#ifndef BIQUADSTACK_H +#define BIQUADSTACK_H + +#include +#include "BiQuad.h" + +namespace IIR { + + template class BiQuadStack { + + std::vector> filters; + + public: + + BiQuadStack() { + ; + } + + BiQuadStack(const int num) { + filters.resize(num); + } + + void resize(const int num) { + filters.resize(num); + } + + BiQuad& operator [] (const size_t idx) { + return filters.at(idx); + } + + Scalar filter(Scalar val) { + for (BiQuad& bq : filters) { + val = bq.filter(val); + } + return val; + } + + }; + +} + +#endif // BIQUADSTACK_H diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h index 7cee7e6..0bb354b 100644 --- a/sensors/imu/StepDetection2.h +++ b/sensors/imu/StepDetection2.h @@ -22,8 +22,9 @@ #endif #include "../../Assertions.h" -#include "../../math/dsp/fir/Real.h" -#include "../../math/dsp/fir/RealFactory.h" +#include "../../math/dsp/iir/BiQuadStack.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 interpol; - FIR::Real::Filter fir; + //FIR::Real::Filter fir; + IIR::BiQuadStack biquad; LocalMaxima locMax; // longterm average to center around zero MovingAverageTS avg = MovingAverageTS(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.setKernel(fac.getBandpass(0.66, 2.0, 40)); + //FIR::Real::Factory fac(sRate_hz); + //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 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 f = fir.append(mag0); +// if (f != f) {return;} + const float f = biquad.filter(mag0); 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; } diff --git a/sensors/imu/StepDetection3.h b/sensors/imu/StepDetection3.h index 8dab918..f4fffaf 100644 --- a/sensors/imu/StepDetection3.h +++ b/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 interpol; - IIR::BiQuad biquad; - DelayBuffer delay; + IIR::BiQuadStack biquad; + DelayBuffer 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.preFill(gravity); + //biquad.setBandPass(stepRate_hz, 1.5, sRate_hz); + //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(); } diff --git a/sensors/imu/StepDetection4.h b/sensors/imu/StepDetection4.h index da088ed..4aa2174 100644 --- a/sensors/imu/StepDetection4.h +++ b/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 interpol; - IIR::BiQuad biquad; + IIR::BiQuadStack biquad; DelayBuffer 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); - lineRaw.setTitle("unrotated Z"); - lineFiltered.setTitle("IIR filtered"); + //biquad.setBandPass(stepRate_hz, 1.0, sRate_hz); + //biquad.preFill(gravity); - biquad.setBandPass(stepRate_hz, 1.0, sRate_hz); - 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 + + 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);