Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor
This commit is contained in:
toni
33
math/DelayBuffer.h
Normal file
33
math/DelayBuffer.h
Normal file
@@ -0,0 +1,33 @@
|
||||
#ifndef DELAYBUFFER_H
|
||||
#define DELAYBUFFER_H
|
||||
|
||||
#include <vector>
|
||||
|
||||
/** efficient delay using a ring-buffer */
|
||||
template <typename Scalar> class DelayBuffer {
|
||||
|
||||
size_t head = 0;
|
||||
std::vector<Scalar> vec;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
DelayBuffer(int size) {
|
||||
vec.resize(size);
|
||||
}
|
||||
|
||||
/** set all elements to the same value */
|
||||
void setAll(const Scalar s) {
|
||||
std::fill(vec.begin(), vec.end(), s);
|
||||
}
|
||||
|
||||
/** append a new element, get the delayed output */
|
||||
Scalar add(Scalar s) {
|
||||
vec[head] = s;
|
||||
head = (head + 1) % vec.size(); // next to-be-overwritten element = oldest element = tail
|
||||
return vec[head];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // DELAYBUFFER_H
|
||||
@@ -3,7 +3,7 @@
|
||||
|
||||
#include <vector>
|
||||
#include <complex>
|
||||
#include "../../Assertions.h"
|
||||
#include "../../../Assertions.h"
|
||||
|
||||
/**
|
||||
* FIR filter using complex convolution
|
||||
|
||||
322
math/dsp/iir/BiQuad.h
Normal file
322
math/dsp/iir/BiQuad.h
Normal file
@@ -0,0 +1,322 @@
|
||||
#ifndef IIR_BIQUAD
|
||||
#define IIR_BIQUAD
|
||||
|
||||
#include <string.h>
|
||||
#include "../../../Assertions.h"
|
||||
|
||||
namespace IIR {
|
||||
|
||||
|
||||
/** frequency limits */
|
||||
#define BFG_MIN 0.0001
|
||||
#define BFG_MAX 0.4999
|
||||
|
||||
|
||||
/**
|
||||
* a simple biquad filter that can be used
|
||||
* for low- or high-pass filtering
|
||||
* http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
|
||||
*/
|
||||
template <typename Scalar> class BiQuad {
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
BiQuad() : in(), out() {
|
||||
reset();
|
||||
}
|
||||
|
||||
/** filter the given amplitude of the given channel (history) */
|
||||
Scalar filter( const Scalar aIn ) {
|
||||
|
||||
Scalar aOut = 0;
|
||||
aOut += aIn *(b0a0);
|
||||
aOut += in[0] *(b1a0);
|
||||
aOut += in[1] *(b2a0);
|
||||
aOut -= out[0]*(a1a0);
|
||||
aOut -= out[1]*(a2a0);
|
||||
|
||||
in[1] = in[0];
|
||||
in[0] = aIn;
|
||||
|
||||
out[1] = out[0];
|
||||
out[0] = aOut;
|
||||
|
||||
return aOut;
|
||||
|
||||
}
|
||||
|
||||
void preFill(const Scalar s) {
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
filter(s);
|
||||
}
|
||||
}
|
||||
|
||||
/** reset (disable) the filter */
|
||||
void reset() {
|
||||
|
||||
b0a0 = 1.0;
|
||||
b1a0 = 0.0;
|
||||
b2a0 = 0.0;
|
||||
a1a0 = 0.0;
|
||||
a2a0 = 0.0;
|
||||
|
||||
memset(in, 0, sizeof(in));
|
||||
memset(out, 0, sizeof(out));
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as low-pass. freqFact between ]0;0.5[ */
|
||||
void setLowPass( double freqFact, const float octaves ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = (1.0 - cos(w0))/2.0;
|
||||
double b1 = 1.0 - cos(w0);
|
||||
double b2 = (1.0 - cos(w0))/2.0;
|
||||
double a0 = 1.0 + alpha;
|
||||
double a1 = -2.0*cos(w0);
|
||||
double a2 = 1.0 - alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/** configure the filter as low-pass */
|
||||
void setLowPass( const float freq, const float octaves, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setLowPass(freqFact, octaves);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter
|
||||
//http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp
|
||||
/**
|
||||
* configure as low-pass filter with resonance
|
||||
* @param freqFact the frequency factor between ]0;0.5[
|
||||
* @param res
|
||||
*/
|
||||
void setLowPassResonance( double freqFact, float res ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
res *= 10;
|
||||
|
||||
double g = pow(10.0, 0.05 * res);
|
||||
double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
|
||||
|
||||
double theta = M_PI * freqFact;
|
||||
double sn = 0.5 * d * sin(theta);
|
||||
double beta = 0.5 * (1 - sn) / (1 + sn);
|
||||
double gamma = (0.5 + beta) * cos(theta);
|
||||
double alpha = 0.25 * (0.5 + beta - gamma);
|
||||
|
||||
double a0 = 1.0;
|
||||
double b0 = 2.0 * alpha;
|
||||
double b1 = 2.0 * 2.0 * alpha;
|
||||
double b2 = 2.0 * alpha;
|
||||
double a1 = 2.0 * -gamma;
|
||||
double a2 = 2.0 * beta;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as high-pass. freqFact between ]0;0.5[ */
|
||||
void setHighPass( double freqFact, const float octaves ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = (1.0 + cos(w0))/2.0;
|
||||
double b1 = -(1.0 + cos(w0));
|
||||
double b2 = (1.0 + cos(w0))/2.0;
|
||||
double a0 = 1.0 + alpha;
|
||||
double a1 = -2.0*cos(w0);
|
||||
double a2 = 1.0 - alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as high-pass */
|
||||
void setHighPass( const float freq, const float octaves, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setHighPass(freqFact, octaves);
|
||||
}
|
||||
|
||||
/** configure the filter as band-pass. freqFact between ]0;0.5[ */
|
||||
void setBandPass( double freqFact, const float octaves ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
//double w0 = 2 * K_PI * / 2 / freqFact;
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = sin(w0)/2.0;
|
||||
double b1 = 0.0;
|
||||
double b2 = -sin(w0)/2.0;
|
||||
double a0 = 1.0 + alpha;
|
||||
double a1 = -2.0*cos(w0);
|
||||
double a2 = 1.0 - alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as band-pass */
|
||||
void setBandPass( const float freq, const float octaves, float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setBandPass(freqFact, octaves);
|
||||
}
|
||||
|
||||
|
||||
/** configure the filter as all-pass. freqFact between ]0;0.5[ */
|
||||
void setAllPass( double freqFact, const float octaves ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = 1 - alpha;
|
||||
double b1 = -2*cos(w0);
|
||||
double b2 = 1 + alpha;
|
||||
double a0 = 1 + alpha;
|
||||
double a1 = -2*cos(w0);
|
||||
double a2 = 1 - alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as all-pass */
|
||||
void setAllPass( const float freq, const float octaves, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setAllPass(freqFact, octaves);
|
||||
}
|
||||
|
||||
/** configure as notch filter. freqFact between ]0;0.5[ */
|
||||
void setNotch( double freqFact, const float octaves ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = 1.0;
|
||||
double b1 = -2.0*cos(w0);
|
||||
double b2 = 1.0;
|
||||
double a0 = 1.0 + alpha;
|
||||
double a1 = -2.0*cos(w0);
|
||||
double a2 = 1.0 - alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure as notch filter */
|
||||
void setNotch( const float freq, const float octaves, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setNotch(freqFact, octaves);
|
||||
}
|
||||
|
||||
/** configure the filter as low-shelf. increase all aplitudes below freq? freqFact between ]0;0.5[ */
|
||||
void setLowShelf( double freqFact, const float octaves, const float gain ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double A = sqrt( pow(10, (gain/20.0)) );
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = A*( (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha );
|
||||
double b1 = 2.0*A*( (A-1.0) - (A+1.0)*cos(w0) );
|
||||
double b2 = A*( (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha );
|
||||
double a0 = (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha;
|
||||
double a1 = -2.0*( (A-1.0) + (A+1.0)*cos(w0) );
|
||||
double a2 = (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
/** configure the filter as low-shelf. increase all aplitudes below freq? */
|
||||
void setLowShelf( const float freq, const float octaves, const float gain, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setLowShelf(freqFact, octaves, gain);
|
||||
}
|
||||
|
||||
/** configure the filter as high-shelf. increase all amplitues above freq? freqFact between ]0;0.5[ */
|
||||
void setHighShelf( double freqFact, const float octaves, const float gain ) {
|
||||
|
||||
sanityCheck(freqFact);
|
||||
|
||||
double A = sqrt( pow(10, (gain/20.0)) );
|
||||
double w0 = 2.0 * M_PI * freqFact;
|
||||
double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||||
|
||||
double b0 = A*( (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha );
|
||||
double b1 = -2.0*A*( (A-1.0) + (A+1.0)*cos(w0) );
|
||||
double b2 = A*( (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha );
|
||||
double a0 = (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha;
|
||||
double a1 = 2.0*( (A-1.0) - (A+1.0)*cos(w0) );
|
||||
double a2 = (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha;
|
||||
|
||||
setValues(a0, a1, a2, b0, b1, b2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/** configure the filter as high-shelf. increase all amplitues above freq? */
|
||||
void setHighShelf( const float freq, const float octaves, const float gain, const float sRate ) {
|
||||
double freqFact = double(freq) / double(sRate);
|
||||
setHighShelf(freqFact, octaves, gain);
|
||||
}
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
/** pre-calculate the quotients for the filtering */
|
||||
void setValues(double a0, double a1, double a2, double b0, double b1, double b2) {
|
||||
b0a0 = float(b0/a0);
|
||||
b1a0 = float(b1/a0);
|
||||
b2a0 = float(b2/a0);
|
||||
a2a0 = float(a2/a0);
|
||||
a1a0 = float(a1/a0);
|
||||
}
|
||||
|
||||
/** the bi-quad filter params */
|
||||
float b0a0;
|
||||
float b1a0;
|
||||
float b2a0;
|
||||
|
||||
float a1a0;
|
||||
float a2a0;
|
||||
|
||||
/** history for input values, per channel */
|
||||
Scalar in[2];
|
||||
|
||||
/** history for ouput values, per channel */
|
||||
Scalar out[2];
|
||||
|
||||
void sanityCheck(const float freqFact) const {
|
||||
Assert::isTrue(freqFact >= BFG_MIN, "frequency out of bounds");
|
||||
Assert::isTrue(freqFact <= BFG_MAX, "frequency out of bounds");
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif // IIR_BIQUAD
|
||||
@@ -131,7 +131,8 @@ namespace NM {
|
||||
const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
|
||||
const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
|
||||
const float diff = walkedDistance_m - requestedDistance_m;
|
||||
return dist.getProbability(diff);
|
||||
const double res = dist.getProbability(diff);
|
||||
return res;
|
||||
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
|
||||
|
||||
}
|
||||
|
||||
156
navMesh/walk/NavMeshWalkSinkOrSwim.h
Normal file
156
navMesh/walk/NavMeshWalkSinkOrSwim.h
Normal file
@@ -0,0 +1,156 @@
|
||||
#ifndef NAVMESHWALKSINKORSWIM_H
|
||||
#define NAVMESHWALKSINKORSWIM_H
|
||||
|
||||
#include "../NavMesh.h"
|
||||
#include "../NavMeshLocation.h"
|
||||
#include "../../geo/Heading.h"
|
||||
#include "../../math/distribution/Normal.h"
|
||||
#include "../../math/distribution/Uniform.h"
|
||||
|
||||
#include "NavMeshSub.h"
|
||||
#include "NavMeshWalkParams.h"
|
||||
#include "NavMeshWalkEval.h"
|
||||
|
||||
namespace NM {
|
||||
|
||||
/**
|
||||
* try to move to the requested location
|
||||
* and, if not, return null
|
||||
*/
|
||||
template <typename Tria> class NavMeshWalkSinkOrSwim {
|
||||
|
||||
public:
|
||||
|
||||
struct Config {
|
||||
Distribution::Uniform<float>* distanceVariation = nullptr;
|
||||
Distribution::Uniform<float>* headingVariation = nullptr;
|
||||
void check() {
|
||||
Assert::isNotNull(distanceVariation, "distanceVariation must not be null");
|
||||
Assert::isNotNull(headingVariation, "headingVariation must not be null");
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
const NavMesh<Tria>& mesh;
|
||||
|
||||
std::vector<NavMeshWalkEval<Tria>*> evals;
|
||||
|
||||
Config cfg;
|
||||
|
||||
int hits = 0;
|
||||
int misses = 0;
|
||||
|
||||
public:
|
||||
|
||||
|
||||
/** single result */
|
||||
struct ResultEntry {
|
||||
NavMeshLocation<Tria> location;
|
||||
Heading heading;
|
||||
double probability;
|
||||
ResultEntry() : heading(0) {;}
|
||||
};
|
||||
|
||||
ResultEntry lastRes;
|
||||
|
||||
/** list of results */
|
||||
using ResultList = std::vector<ResultEntry>;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor without config */
|
||||
NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh) : mesh(mesh), cfg() {
|
||||
|
||||
}
|
||||
|
||||
/** ctor with config */
|
||||
NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh, Config cfg) : mesh(mesh), cfg(cfg) {
|
||||
cfg.check();
|
||||
}
|
||||
|
||||
/** add a new evaluator to the walker */
|
||||
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
|
||||
this->evals.push_back(eval);
|
||||
}
|
||||
|
||||
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
|
||||
|
||||
// sanity checks
|
||||
params.check();
|
||||
|
||||
ResultEntry re;
|
||||
|
||||
// variation?
|
||||
const float distVar = (cfg.distanceVariation) ? (cfg.distanceVariation->draw()) : (0);
|
||||
const float headingVar = (cfg.headingVariation) ? (cfg.headingVariation->draw()) : (0);
|
||||
|
||||
// to-be-walked distance;
|
||||
const float toBeWalkedDist = params.getToBeWalkedDistance() + distVar;
|
||||
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
|
||||
|
||||
// construct reachable region
|
||||
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
|
||||
|
||||
// get the to-be-reached destination's position (using start+distance+heading)
|
||||
const Heading heading = params.heading + headingVar;
|
||||
const Point2 dir = heading.asVector();
|
||||
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
|
||||
|
||||
const Tria* dstTria = reachable.getContainingTriangle(dst);
|
||||
|
||||
// is above destination reachable?
|
||||
if (dstTria) {
|
||||
|
||||
re.heading = params.heading; // heading was OK -> keep
|
||||
re.location.pos = dstTria->toPoint3(dst); // new destination position
|
||||
re.location.tria = dstTria; // new destination triangle
|
||||
re.probability = 1;
|
||||
++hits;
|
||||
|
||||
// calculate probability
|
||||
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
|
||||
re.probability = 1.0;
|
||||
for (const NavMeshWalkEval<Tria>* eval : evals) {
|
||||
const double p1 = eval->getProbability(pwalk);
|
||||
re.probability *= p1;
|
||||
}
|
||||
|
||||
lastRes = re;
|
||||
|
||||
} else {
|
||||
|
||||
// re.heading = params.heading; // keep
|
||||
// re.location = params.start; // keep
|
||||
// re.probability = 0; // kill
|
||||
re = lastRes;
|
||||
//re.probability *= 0.1;
|
||||
++misses;
|
||||
|
||||
}
|
||||
|
||||
const int total = (hits + misses);
|
||||
if (total % 10000 == 0) {
|
||||
//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
|
||||
}
|
||||
|
||||
// done
|
||||
return re;
|
||||
|
||||
}
|
||||
|
||||
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
|
||||
|
||||
// sanity checks
|
||||
params.check();
|
||||
|
||||
return {getOne(params)};
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // NAVMESHWALKSINKORSWIM_H
|
||||
178
sensors/imu/StepDetection3.h
Normal file
178
sensors/imu/StepDetection3.h
Normal file
@@ -0,0 +1,178 @@
|
||||
#ifndef STEPDETECTION3_H
|
||||
#define STEPDETECTION3_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/iir/BiQuad.h"
|
||||
#include "../../math/FixedFrequencyInterpolator.h"
|
||||
#include "../../math/DelayBuffer.h"
|
||||
|
||||
|
||||
/**
|
||||
* simple step detection based on accelerometer magnitude.
|
||||
* interpolated to a fixed frequency
|
||||
* passed through IIR filter
|
||||
* searching for zero crossings that follow a peak value
|
||||
* that is above a certain threshold
|
||||
*/
|
||||
class StepDetection3 {
|
||||
|
||||
static constexpr float gravity = 9.81;
|
||||
static constexpr float stepRate_hz = 2.0;
|
||||
static constexpr int sRate_hz = 100;
|
||||
static constexpr int every_ms = 1000 / sRate_hz;
|
||||
static constexpr float threshold = 1.0;
|
||||
|
||||
float max = 0;
|
||||
Timestamp maxTS;
|
||||
|
||||
private:
|
||||
|
||||
FixedFrequencyInterpolator<AccelerometerData> interpol;
|
||||
IIR::BiQuad<float> biquad;
|
||||
DelayBuffer<float> delay;
|
||||
|
||||
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines lineRaw;
|
||||
K::GnuplotPlotElementLines lineFiltered;
|
||||
K::GnuplotPlotElementPoints pointDet;
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
#endif
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
std::ofstream outFiltered;
|
||||
std::ofstream outSteps;
|
||||
#endif
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
StepDetection3() : interpol(Timestamp::fromMS(every_ms)), delay(10) {
|
||||
|
||||
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
|
||||
biquad.preFill(gravity);
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
gp << "set autoscale xfix\n";
|
||||
plot.setTitle("Step Detection");
|
||||
plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
|
||||
plot.add(&lineFiltered); lineFiltered.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 gotStep = false;
|
||||
|
||||
// accel-data incoming on a fixed sampling rate (needed for FIR to work)
|
||||
// NOTE!!!! MIGHT TRIGGER MORE THAN ONCE PER add() !!!
|
||||
auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
|
||||
|
||||
bool step = false;
|
||||
const float mag = data.magnitude();
|
||||
|
||||
// apply filter
|
||||
const float fMag = biquad.filter(mag);
|
||||
|
||||
// history buffer
|
||||
float fMagOld = delay.add(fMag);
|
||||
|
||||
// zero crossing?
|
||||
float tmp = max;
|
||||
if (fMagOld > 0 && fMag < 0) {
|
||||
if (max > threshold) {
|
||||
step = true;
|
||||
gotStep = true;
|
||||
}
|
||||
delay.setAll(0);
|
||||
max = 0;
|
||||
}
|
||||
|
||||
// track maximum value
|
||||
if (fMag > max) {max = fMag; maxTS = ts;}
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
if (step) {
|
||||
std::cout << ts.ms() << std::endl;
|
||||
outSteps << maxTS.ms() << " " << tmp << "\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);
|
||||
|
||||
lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), mag) );
|
||||
lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
|
||||
|
||||
if (step) {
|
||||
pointDet.add( K::GnuplotPoint2((maxTS-plotRef).ms(), tmp) );
|
||||
}
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||||
lineRaw.removeIf(remove);
|
||||
lineFiltered.removeIf(remove);
|
||||
pointDet.removeIf(remove);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
usleep(100);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
// ensure fixed sampling rate for FIR freq filters to work!
|
||||
interpol.add(ts, acc, onResample);
|
||||
|
||||
return gotStep;
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // STEPDETECTION3_H
|
||||
197
sensors/imu/StepDetection4.h
Normal file
197
sensors/imu/StepDetection4.h
Normal file
@@ -0,0 +1,197 @@
|
||||
#ifndef STEPDETECTION4_H
|
||||
#define STEPDETECTION4_H
|
||||
|
||||
|
||||
#include "AccelerometerData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
|
||||
#include "PoseDetection.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/iir/BiQuad.h"
|
||||
#include "../../math/FixedFrequencyInterpolator.h"
|
||||
#include "../../math/DelayBuffer.h"
|
||||
|
||||
|
||||
/**
|
||||
* step detection based on accelerometer data,
|
||||
* un-rotated using pose-detection
|
||||
* interpolated to a fixed frequency
|
||||
* passed through IIR filter
|
||||
* searching for zero crossings that follow a peak value
|
||||
* that is above a certain threshold
|
||||
*/
|
||||
class StepDetection4 {
|
||||
|
||||
static constexpr float gravity = 9.81;
|
||||
static constexpr float stepRate_hz = 2.0;
|
||||
static constexpr int sRate_hz = 100;
|
||||
static constexpr int every_ms = 1000 / sRate_hz;
|
||||
static constexpr float threshold = 1.0;
|
||||
|
||||
float max = 0;
|
||||
Timestamp maxTS;
|
||||
|
||||
private:
|
||||
|
||||
PoseDetection* pose;
|
||||
FixedFrequencyInterpolator<Vector3> interpol;
|
||||
IIR::BiQuad<float> biquad;
|
||||
DelayBuffer<float> delay;
|
||||
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines lineRaw;
|
||||
K::GnuplotPlotElementLines lineFiltered;
|
||||
K::GnuplotPlotElementPoints pointDet;
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
#endif
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
std::ofstream outFiltered;
|
||||
std::ofstream outSteps;
|
||||
#endif
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
|
||||
|
||||
plot.getKey().setVisible(true);
|
||||
lineRaw.setTitle("unrotated Z");
|
||||
lineFiltered.setTitle("IIR filtered");
|
||||
|
||||
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
|
||||
biquad.preFill(gravity);
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
gp << "set autoscale xfix\n";
|
||||
plot.setTitle("Step Detection");
|
||||
plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
|
||||
plot.add(&lineFiltered); lineFiltered.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) {
|
||||
|
||||
// ignore readings until the first orientation-estimation is available
|
||||
// otherwise we would use a wrong rotation matrix which yields wrong results!
|
||||
if (!pose->isKnown()) {return false;}
|
||||
|
||||
// get the current accs-reading as vector
|
||||
const Vector3 vec(_acc.x, _acc.y, _acc.z);
|
||||
|
||||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||||
const Vector3 acc = pose->getMatrix() * vec;
|
||||
|
||||
// will be set when a step was detected
|
||||
bool gotStep = false;
|
||||
|
||||
// accel-data incoming on a fixed sampling rate (needed for FIR to work)
|
||||
// NOTE!!!! MIGHT TRIGGER MORE THAN ONCE PER add() !!!
|
||||
auto onResample = [&] (const Timestamp ts, const Vector3 data) {
|
||||
|
||||
bool step = false;
|
||||
const float mag = data.z;
|
||||
|
||||
// apply filter
|
||||
const float fMag = biquad.filter(mag);
|
||||
|
||||
// history buffer
|
||||
float fMagOld = delay.add(fMag);
|
||||
|
||||
// zero crossing?
|
||||
float tmp = max;
|
||||
if (fMagOld > 0 && fMag < 0) {
|
||||
if (max > threshold) {
|
||||
step = true;
|
||||
gotStep = true;
|
||||
}
|
||||
delay.setAll(0);
|
||||
max = 0;
|
||||
}
|
||||
|
||||
// track maximum value
|
||||
if (fMag > max) {max = fMag; maxTS = ts;}
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
if (step) {
|
||||
std::cout << ts.ms() << std::endl;
|
||||
outSteps << maxTS.ms() << " " << tmp << "\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);
|
||||
|
||||
lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), data.z) );
|
||||
lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
|
||||
|
||||
if (step) {
|
||||
pointDet.add( K::GnuplotPoint2((maxTS-plotRef).ms(), tmp) );
|
||||
}
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||||
lineRaw.removeIf(remove);
|
||||
lineFiltered.removeIf(remove);
|
||||
pointDet.removeIf(remove);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
usleep(100);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
// ensure fixed sampling rate for FIR freq filters to work!
|
||||
interpol.add(ts, acc, onResample);
|
||||
|
||||
return gotStep;
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // STEPDETECTION4_H
|
||||
Reference in New Issue
Block a user