worked on FIR-Convolution and LocalMaxima detection
This commit is contained in:
k-a-z-u
@@ -19,6 +19,7 @@
|
||||
|
||||
#include "../../Assertions.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/dsp/FIRComplex.h"
|
||||
|
||||
|
||||
/**
|
||||
@@ -145,88 +146,6 @@ public:
|
||||
|
||||
}
|
||||
|
||||
|
||||
//private:
|
||||
|
||||
// /** low pass acc-magnitude */
|
||||
// float avg1 = 0;
|
||||
|
||||
// /** even-more low-pass acc-magnitude */
|
||||
// float avg2 = 0;
|
||||
|
||||
//private:
|
||||
|
||||
// class Stepper {
|
||||
|
||||
// private:
|
||||
|
||||
// /** block for 300 ms after every step */
|
||||
// const Timestamp blockTime = Timestamp::fromMS(300);
|
||||
|
||||
// /** the threshold for detecting a spike as step */
|
||||
// const float threshold = 0.30;
|
||||
|
||||
// /** block until the given timestamp before detecting additional steps */
|
||||
// Timestamp blockUntil;
|
||||
|
||||
|
||||
// public:
|
||||
|
||||
// /** is the given (relative!) magnitude (mag - ~9.81) a step? */
|
||||
// bool isStep(const Timestamp ts, const float mag) {
|
||||
|
||||
// // still blocking
|
||||
// if (ts < blockUntil) {
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// // threshold reached? -> step!
|
||||
// if (mag > threshold) {
|
||||
|
||||
// // block x milliseconds until detecting the next step
|
||||
// blockUntil = ts + blockTime;
|
||||
|
||||
// // we have a step
|
||||
// return true;
|
||||
|
||||
// }
|
||||
|
||||
// // no step
|
||||
// return false;
|
||||
|
||||
// }
|
||||
|
||||
|
||||
// };
|
||||
|
||||
// Stepper stepper;
|
||||
|
||||
//public:
|
||||
|
||||
// /** does the given data indicate a step? */
|
||||
// bool add(const Timestamp ts, const AccelerometerData& acc) {
|
||||
|
||||
// avg1 = avg1 * 0.91 + acc.magnitude() * 0.09; // short-time average [filtered steps]
|
||||
// avg2 = avg2 * 0.97 + acc.magnitude() * 0.03; // long-time average [gravity]
|
||||
|
||||
// // average maginitude must be > 9.0 to be stable enough to proceed
|
||||
// if (avg2 > 9) {
|
||||
|
||||
// // gravity-free magnitude
|
||||
// const float avg = avg1 - avg2;
|
||||
|
||||
// // detect steps
|
||||
// return stepper.isStep(ts, avg);
|
||||
|
||||
// } else {
|
||||
|
||||
// return false;
|
||||
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
149
sensors/imu/StepDetection2.h
Normal file
149
sensors/imu/StepDetection2.h
Normal file
@@ -0,0 +1,149 @@
|
||||
#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/FIRComplex.h"
|
||||
#include "../../math/FixedFrequencyInterpolator.h"
|
||||
#include "../../math/LocalMaxima.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;
|
||||
FIRComplex fir;
|
||||
LocalMaxima locMax;
|
||||
|
||||
const float threshold = 0.5;
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines lineMag;
|
||||
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)), fir(sRate_hz), locMax(5) {
|
||||
|
||||
fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz
|
||||
fir.shiftBy(2); // typical step freq ~2Hz
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
gp << "set autoscale xfix\n";
|
||||
plot.setTitle("Step Detection");
|
||||
plot.add(&lineMag); lineMag.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;
|
||||
|
||||
auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
|
||||
|
||||
const float mag = data.magnitude();
|
||||
|
||||
const std::complex<float> c = fir.append(mag);
|
||||
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);
|
||||
|
||||
#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);
|
||||
|
||||
lineMag.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();};
|
||||
lineMag.removeIf(remove);
|
||||
pointDet.removeIf(remove);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
usleep(100);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
interpol.add(ts, acc, onResample);
|
||||
|
||||
return step;
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // STEPDETECTION2_H
|
||||
Reference in New Issue
Block a user