FrankE
a2c9e575a2
- worked on about everything - grid walker using plugable modules - wifi models - new distributions - worked on geometric data-structures - added typesafe timestamps - worked on grid-building - added sensor-classes - added sensor analysis (step-detection, turn-detection) - offline data reader - many test-cases
113 lines
2.0 KiB
C++
113 lines
2.0 KiB
C++
#ifndef STEPDETECTION_H
|
|
#define STEPDETECTION_H
|
|
|
|
|
|
#include "AccelerometerData.h"
|
|
#include "../../data/Timestamp.h"
|
|
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
#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 "../../Assertions.h"
|
|
|
|
|
|
/**
|
|
* simple step detection based on accelerometer magnitude.
|
|
* magnitude > threshold? -> step!
|
|
* block for several msec until detecting the next one
|
|
*/
|
|
class StepDetection {
|
|
|
|
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.17;
|
|
|
|
/** 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 isStep(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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
#endif // STEPDETECTION_H
|