FrankE
d283d9b326
new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
184 lines
3.8 KiB
C++
184 lines
3.8 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"
|
|
#include "../../math/MovingAverageTS.h"
|
|
|
|
|
|
/**
|
|
* simple step detection based on accelerometer magnitude.
|
|
* magnitude > threshold? -> step!
|
|
* block for several msec until detecting the next one
|
|
*/
|
|
class StepDetection {
|
|
|
|
private:
|
|
|
|
MovingAverageTS<float> avgLong;
|
|
MovingAverageTS<float> avgShort;
|
|
|
|
Timestamp blockUntil;
|
|
bool waitForUp = false;
|
|
|
|
const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good
|
|
const float upperThreshold = +0.4f; // + is usually smaller than down (look at graphs)
|
|
const float lowerThreshold = -0.8f;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
|
|
;
|
|
}
|
|
|
|
/** does the given data indicate a step? */
|
|
bool add(const Timestamp ts, const AccelerometerData& acc) {
|
|
|
|
// update averages
|
|
avgLong.add(ts, acc.magnitude());
|
|
avgShort.add(ts, acc.magnitude());
|
|
|
|
// difference between long-term-average (gravity) and very-short-time average
|
|
const float delta = avgShort.get() - avgLong.get();
|
|
|
|
bool step = false;
|
|
|
|
|
|
if (blockUntil > ts) {return false;}
|
|
|
|
// wait for a rising edge
|
|
if (waitForUp && delta > upperThreshold) {
|
|
blockUntil = ts + blockTime; // block some time
|
|
waitForUp = false;
|
|
}
|
|
|
|
// wait for a falling edge
|
|
if (!waitForUp && delta < lowerThreshold) {
|
|
blockUntil = ts + blockTime; // block some time
|
|
waitForUp = true;
|
|
step = true;
|
|
}
|
|
|
|
// static K::Gnuplot gp;
|
|
// static K::GnuplotPlot plot;
|
|
// static K::GnuplotPlotElementLines lines1; plot.add(&lines1);
|
|
// static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff");
|
|
// static Timestamp ref = ts;
|
|
|
|
// static int i = 0;
|
|
|
|
// //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
|
|
// lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) );
|
|
|
|
// if (++i % 100 == 0) {
|
|
// gp.draw(plot);
|
|
// gp.flush();
|
|
// usleep(1000*25);
|
|
// }
|
|
|
|
return step;
|
|
|
|
|
|
}
|
|
|
|
|
|
//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;
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
|
|
};
|
|
|
|
|
|
#endif // STEPDETECTION_H
|