diff --git a/math/Delay.h b/math/Delay.h new file mode 100644 index 0000000..bd90a94 --- /dev/null +++ b/math/Delay.h @@ -0,0 +1,47 @@ +#ifndef DELAY_H +#define DELAY_H + +#include +#include + +template class Delay { + +private: + + size_t size; + + /** up to "size" elements */ + std::vector values; + +public: + + /** ctor */ + Delay(const int size) : size(size) {;} + + /** add a new value */ + void add(const T val) { + + // add new value + values.push_back(val); + + // remove old ones + while(values.size() > size) { + values.erase(values.begin()); + } + + } + + /** get the delayed value */ + T get() const { + return values.front(); + } + + /** delay output valid? */ + bool isValid() const { + return values.size() == size; + } + + +}; + +#endif // DELAY_H diff --git a/math/LocalMaxima.h b/math/LocalMaxima.h index 8f507e0..5ca45a1 100644 --- a/math/LocalMaxima.h +++ b/math/LocalMaxima.h @@ -1,6 +1,63 @@ #ifndef LOCALMAXIMA_H #define LOCALMAXIMA_H +#include "Delay.h" + +class LocalMaxima { + + int delay; + Delay d0; + Delay d1; + float cur; + int block = 0; + +public: + + LocalMaxima(int delay) : delay(delay), d0(delay*2), d1(delay) { + ; + } + + bool add(const float s) { + + if (block > 0) {--block;} + + d0.add(s); + d1.add(s); + this->cur = s; + + if (isMax()) { + block = delay; + return true; + } + + return false; + + } + + float get() { + return d1.get(); + } + +private: + + bool isMax() { + + if (block > 0) {return false;} + + if (!d0.isValid()) {return false;} + if (!d1.isValid()) {return false;} + + const float s0 = d0.get(); + const float s1 = d1.get(); + const float s2 = cur; + + return (s1 > s0) && (s1 > s2); + + } + +}; + +/* class LocalMaxima { static constexpr float MAX = 1e40; @@ -21,12 +78,12 @@ public: Res(bool isMax, float val) : isMax(isMax), val(val) {;} }; - /** ctor. use only every n-th sample */ + // ctor. use only every n-th sample LocalMaxima(const size_t everyNth) : everyNth(everyNth) { reset(); } - /** is the given value a local maxima? */ + // is the given value a local maxima? Res add(const float s) { if (cnt == 0*everyNth) {s0 = s;} // set, wait some time @@ -60,5 +117,5 @@ private: } }; - +*/ #endif // LOCALMAXIMA_H diff --git a/sensors/imu/PoseDetectionPlot.h b/sensors/imu/PoseDetectionPlot.h index e065ad5..74aae10 100644 --- a/sensors/imu/PoseDetectionPlot.h +++ b/sensors/imu/PoseDetectionPlot.h @@ -8,10 +8,10 @@ #include #include #include - #include + //#include #include #include - #include + //#include #include "../../data/Timestamp.h" #include "../../math/Matrix3.h" diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 8478bba..c19ee7b 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -38,8 +38,8 @@ private: bool waitForUp = false; const Timestamp blockTime; // 150-250 looks good - const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs) - const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing! + const float upperThreshold = +0.4*0.8f; // + is usually smaller than down (look at graphs) + const float lowerThreshold = -1.5*0.8f; // the 0.8 is for testing! #ifdef WITH_DEBUG_PLOT diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h index 36ef37d..eab6e2f 100644 --- a/sensors/imu/StepDetection2.h +++ b/sensors/imu/StepDetection2.h @@ -25,6 +25,7 @@ #include "../../math/dsp/FIRComplex.h" #include "../../math/FixedFrequencyInterpolator.h" #include "../../math/LocalMaxima.h" +#include "../../math/MovingAverageTS.h" /** @@ -43,12 +44,16 @@ private: FIRComplex fir; LocalMaxima locMax; - const float threshold = 0.5; + // longterm average to center around zero + MovingAverageTS avg = MovingAverageTS(Timestamp::fromMS(2000), 0); + + const float threshold = 0.50; #ifdef WITH_DEBUG_PLOT K::Gnuplot gp; K::GnuplotPlot plot; - K::GnuplotPlotElementLines lineMag; + K::GnuplotPlotElementLines lineRaw; + K::GnuplotPlotElementLines lineFiltered; K::GnuplotPlotElementPoints pointDet; Timestamp plotRef; Timestamp lastPlot; @@ -63,15 +68,18 @@ private: public: /** ctor */ - StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) { + StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(8) { - fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz - fir.shiftBy(2); // typical step freq ~2Hz + //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 #ifdef WITH_DEBUG_PLOT gp << "set autoscale xfix\n"; plot.setTitle("Step Detection"); - plot.add(&lineMag); lineMag.getStroke().getColor().setHexStr("#000000"); + 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 @@ -87,17 +95,23 @@ public: bool step = false; + // accel-data incoming on a fixed sampling rate (needed for FIR to work) auto onResample = [&] (const Timestamp ts, const AccelerometerData data) { const float mag = data.magnitude(); - const std::complex c = fir.append(mag); + // use long-term average to center around zero + 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; - LocalMaxima::Res res = locMax.add(fMag); - step = (res.isMax) && (res.val > threshold); + const bool isMax = locMax.add(fMag); + + step = (isMax) && (locMax.get() > threshold); #ifdef WITH_DEBUG_OUTPUT if (step) { @@ -113,7 +127,8 @@ public: const Timestamp tsPlot = (ts-plotRef); const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); - lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) ); + lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), mag) ); + lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) ); if (step) { pointDet.add( K::GnuplotPoint2(tsPlot.ms(), fMag) ); @@ -123,7 +138,8 @@ public: lastPlot = tsPlot; auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; - lineMag.removeIf(remove); + lineRaw.removeIf(remove); + lineFiltered.removeIf(remove); pointDet.removeIf(remove); gp.draw(plot); @@ -136,6 +152,7 @@ public: }; + // ensure fixed sampling rate for FIR freq filters to work! interpol.add(ts, acc, onResample); return step; diff --git a/sensors/imu/TurnDetectionPlot.h b/sensors/imu/TurnDetectionPlot.h index a0a9857..4f76849 100644 --- a/sensors/imu/TurnDetectionPlot.h +++ b/sensors/imu/TurnDetectionPlot.h @@ -104,7 +104,10 @@ gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; - gp1.draw(multiplot); + //gp1.draw(plotGyroRaw); // raw only + gp1.draw(plotGyroFix); // fixed only + //gp1.draw(multiplot); // both + gp1.flush(); }