175 lines
4.2 KiB
C++
175 lines
4.2 KiB
C++
#ifndef STEPDETECTION2_H
|
|
#define STEPDETECTION2_H
|
|
|
|
|
|
#include "AccelerometerData.h"
|
|
#include "../../data/Timestamp.h"
|
|
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
#ifdef WITH_DEBUG_PLOT
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
|
|
#endif
|
|
|
|
#ifdef WITH_DEBUG_OUTPUT
|
|
#include <fstream>
|
|
#endif
|
|
|
|
#include "../../Assertions.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"
|
|
|
|
|
|
/**
|
|
* simple step detection based on accelerometer magnitude.
|
|
* magnitude > threshold? -> step!
|
|
* block for several msec until detecting the next one
|
|
*/
|
|
class StepDetection2 {
|
|
|
|
static constexpr int sRate_hz = 75;
|
|
static constexpr int every_ms = 1000 / sRate_hz;
|
|
|
|
private:
|
|
|
|
FixedFrequencyInterpolator<AccelerometerData> interpol;
|
|
FIR::Real::Filter fir;
|
|
LocalMaxima locMax;
|
|
|
|
// longterm average to center around zero
|
|
MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
|
|
|
|
const float threshold = 0.50;
|
|
|
|
#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 */
|
|
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::Real::Factory fac(sRate_hz);
|
|
fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
|
|
|
|
|
|
#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 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();
|
|
|
|
// use long-term average to center around zero
|
|
avg.add(ts, mag);
|
|
const float mag0 = mag - avg.get();
|
|
|
|
//const std::complex<float> 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);
|
|
|
|
step = (isMax) && (locMax.get() > threshold);
|
|
|
|
#ifdef WITH_DEBUG_OUTPUT
|
|
if (step) {
|
|
outSteps << ts.ms() << " " << fMag << "\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(tsPlot.ms(), fMag) );
|
|
}
|
|
|
|
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 step;
|
|
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
#endif // STEPDETECTION2_H
|