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/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index c0fec12..5d75082 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -131,7 +131,8 @@ namespace NM { const float requestedDistance_m = walk.requested.getToBeWalkedDistance(); const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos); const float diff = walkedDistance_m - requestedDistance_m; - return dist.getProbability(diff); + const double res = dist.getProbability(diff); + return res; //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); } diff --git a/navMesh/walk/NavMeshWalkSinkOrSwim.h b/navMesh/walk/NavMeshWalkSinkOrSwim.h new file mode 100644 index 0000000..6860134 --- /dev/null +++ b/navMesh/walk/NavMeshWalkSinkOrSwim.h @@ -0,0 +1,156 @@ +#ifndef NAVMESHWALKSINKORSWIM_H +#define NAVMESHWALKSINKORSWIM_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" +#include "../../math/distribution/Normal.h" +#include "../../math/distribution/Uniform.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + +namespace NM { + + /** + * try to move to the requested location + * and, if not, return null + */ + template class NavMeshWalkSinkOrSwim { + + public: + + struct Config { + Distribution::Uniform* distanceVariation = nullptr; + Distribution::Uniform* headingVariation = nullptr; + void check() { + Assert::isNotNull(distanceVariation, "distanceVariation must not be null"); + Assert::isNotNull(headingVariation, "headingVariation must not be null"); + } + }; + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + Config cfg; + + int hits = 0; + int misses = 0; + + public: + + + /** single result */ + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + ResultEntry lastRes; + + /** list of results */ + using ResultList = std::vector; + + public: + + /** ctor without config */ + NavMeshWalkSinkOrSwim(const NavMesh& mesh) : mesh(mesh), cfg() { + + } + + /** ctor with config */ + NavMeshWalkSinkOrSwim(const NavMesh& mesh, Config cfg) : mesh(mesh), cfg(cfg) { + cfg.check(); + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + ResultEntry re; + + // variation? + const float distVar = (cfg.distanceVariation) ? (cfg.distanceVariation->draw()) : (0); + const float headingVar = (cfg.headingVariation) ? (cfg.headingVariation->draw()) : (0); + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance() + distVar; + const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; + + // construct reachable region + NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + // get the to-be-reached destination's position (using start+distance+heading) + const Heading heading = params.heading + headingVar; + const Point2 dir = heading.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + re.heading = params.heading; // heading was OK -> keep + re.location.pos = dstTria->toPoint3(dst); // new destination position + re.location.tria = dstTria; // new destination triangle + re.probability = 1; + ++hits; + + // calculate probability + const NavMeshPotentialWalk pwalk(params, re.location); + re.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + re.probability *= p1; + } + + lastRes = re; + + } else { + +// re.heading = params.heading; // keep +// re.location = params.start; // keep +// re.probability = 0; // kill + re = lastRes; + //re.probability *= 0.1; + ++misses; + + } + + const int total = (hits + misses); + if (total % 10000 == 0) { + //std::cout << "hits: " << (hits*100/total) << "%" << std::endl; + } + + // done + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) { + + // sanity checks + params.check(); + + return {getOne(params)}; + + } + + + }; + +} + +#endif // NAVMESHWALKSINKORSWIM_H diff --git a/sensors/imu/StepDetection3.h b/sensors/imu/StepDetection3.h new file mode 100644 index 0000000..8dab918 --- /dev/null +++ b/sensors/imu/StepDetection3.h @@ -0,0 +1,178 @@ +#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. + * interpolated to a fixed frequency + * passed through IIR filter + * searching for zero crossings that follow a peak value + * that is above a certain threshold + */ +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 diff --git a/sensors/imu/StepDetection4.h b/sensors/imu/StepDetection4.h new file mode 100644 index 0000000..da088ed --- /dev/null +++ b/sensors/imu/StepDetection4.h @@ -0,0 +1,197 @@ +#ifndef STEPDETECTION4_H +#define STEPDETECTION4_H + + +#include "AccelerometerData.h" +#include "../../data/Timestamp.h" + +#include "PoseDetection.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" + + +/** + * step detection based on accelerometer data, + * un-rotated using pose-detection + * interpolated to a fixed frequency + * passed through IIR filter + * searching for zero crossings that follow a peak value + * that is above a certain threshold + */ +class StepDetection4 { + + 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: + + PoseDetection* pose; + 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 */ + StepDetection4(PoseDetection* pose) : 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); + + #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) { + + // ignore readings until the first orientation-estimation is available + // otherwise we would use a wrong rotation matrix which yields wrong results! + if (!pose->isKnown()) {return false;} + + // get the current accs-reading as vector + const Vector3 vec(_acc.x, _acc.y, _acc.z); + + // rotate it into our desired coordinate system, where the smartphone lies flat on the ground + const Vector3 acc = pose->getMatrix() * vec; + + // will be set when a step was detected + 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 Vector3 data) { + + bool step = false; + const float mag = data.z; + + // 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(), data.z) ); + 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 // STEPDETECTION4_H