FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

195
sensors/imu/StepDetection.h
View File

@@ -16,6 +16,7 @@
#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
/**
@@ -27,85 +28,155 @@ class StepDetection {
private:
/** low pass acc-magnitude */
float avg1 = 0;
MovingAverageTS<float> avgLong;
MovingAverageTS<float> avgShort;
/** even-more low-pass acc-magnitude */
float avg2 = 0;
Timestamp blockUntil;
bool waitForUp = false;
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;
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 isStep(const Timestamp ts, const AccelerometerData& acc) {
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]
// update averages
avgLong.add(ts, acc.magnitude());
avgShort.add(ts, acc.magnitude());
// average maginitude must be > 9.0 to be stable enough to proceed
if (avg2 > 9) {
// difference between long-term-average (gravity) and very-short-time average
const float delta = avgShort.get() - avgLong.get();
// gravity-free magnitude
const float avg = avg1 - avg2;
bool step = false;
// detect steps
return stepper.isStep(ts, avg);
} else {
return 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;
// }
// }
};