huge commit
- 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
This commit is contained in:
112
sensors/imu/StepDetection.h
Normal file
112
sensors/imu/StepDetection.h
Normal file
@@ -0,0 +1,112 @@
|
||||
#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
|
||||
Reference in New Issue
Block a user