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

worked on step-detection

Browse Source 
adjusted iir biquad
added biquad-stacking
This commit is contained in:
FrankE
2018-08-07 19:36:07 +02:00
parent d6ac8a72ca
commit f990485d44
5 changed files with 135 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
math/dsp/iir/BiQuad.h
View File

@@ -97,9 +97,9 @@ namespace IIR {
/** configure the filter as low-pass */
void setLowPass( const float freq, const float octaves, const float sRate ) {
void setLowPass( const float freq, const float Q, const float sRate ) {
double freqFact = double(freq) / double(sRate);
setLowPass(freqFact, octaves);
setLowPass(freqFact, Q);
}
@@ -159,9 +159,9 @@ namespace IIR {
}
/** configure the filter as high-pass */
void setHighPass( const float freq, const float octaves, const float sRate ) {
void setHighPass( const float freq, const float Q, const float sRate ) {
double freqFact = double(freq) / double(sRate);
setHighPass(freqFact, octaves);
setHighPass(freqFact, Q);
}
/** configure the filter as band-pass. freqFact between ]0;0.5[ */
@@ -189,9 +189,9 @@ namespace IIR {
}
/** configure the filter as band-pass */
void setBandPass( const float freq, const float octaves, float sRate ) {
void setBandPass( const float freq, const float Q, float sRate ) {
double freqFact = double(freq) / double(sRate);
setBandPass(freqFact, octaves);
setBandPass(freqFact, Q);
}

42
math/dsp/iir/BiQuadStack.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef BIQUADSTACK_H
#define BIQUADSTACK_H
#include <vector>
#include "BiQuad.h"
namespace IIR {
template <typename Scalar> class BiQuadStack {
std::vector<BiQuad<Scalar>> filters;
public:
BiQuadStack() {
;
}
BiQuadStack(const int num) {
filters.resize(num);
}
void resize(const int num) {
filters.resize(num);
}
BiQuad<Scalar>& operator [] (const size_t idx) {
return filters.at(idx);
}
Scalar filter(Scalar val) {
for (BiQuad<Scalar>& bq : filters) {
val = bq.filter(val);
}
return val;
}
};
}
#endif // BIQUADSTACK_H

47
sensors/imu/StepDetection2.h
View File

@@ -22,8 +22,9 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/fir/Real.h"
#include "../../math/dsp/fir/RealFactory.h"
#include "../../math/dsp/iir/BiQuadStack.h"
//#include "../../math/dsp/fir/Real.h"
//#include "../../math/dsp/fir/RealFactory.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/LocalMaxima.h"
#include "../../math/MovingAverageTS.h"
@@ -36,19 +37,20 @@
*/
class StepDetection2 {
static constexpr int sRate_hz = 75;
static constexpr int sRate_hz = 100;
static constexpr int every_ms = 1000 / sRate_hz;
private:
FixedFrequencyInterpolator<AccelerometerData> interpol;
FIR::Real::Filter fir;
//FIR::Real::Filter fir;
IIR::BiQuadStack<float> biquad;
LocalMaxima locMax;
// longterm average to center around zero
MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
const float threshold = 0.50;
float threshold = 0.50;
#ifdef WITH_DEBUG_PLOT
K::Gnuplot gp;
@@ -69,16 +71,28 @@ private:
public:
/** ctor */
StepDetection2() : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
StepDetection2(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
//fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz
//fir.shiftBy(2.00); // typical step freq ~2Hz
//fir.lowPass(3.5, 25); // everything up to 3 HZ
FIR::Real::Factory fac(sRate_hz);
fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
//FIR::Real::Factory fac(sRate_hz);
//fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";
@@ -98,7 +112,7 @@ public:
/** does the given data indicate a step? */
bool add(const Timestamp ts, const AccelerometerData& acc) {
bool step = false;
bool gotStep = false;
// accel-data incoming on a fixed sampling rate (needed for FIR to work)
auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
@@ -109,17 +123,16 @@ public:
avg.add(ts, mag);
const float mag0 = mag - avg.get();
//const std::complex<float> c = fir.append(mag0);
//const float real = c.real();
//if (real != real) {return;}
//const float fMag = real;
const float f = fir.append(mag0);
if (f != f) {return;}
// const float f = fir.append(mag0);
// if (f != f) {return;}
const float f = biquad.filter(mag0);
const float fMag = f;
const bool isMax = locMax.add(fMag);
step = (isMax) && (locMax.get() > threshold);
const bool step = (isMax) && (locMax.get() > threshold);
if (step) {gotStep = true;}
#ifdef WITH_DEBUG_OUTPUT
if (step) {
@@ -163,7 +176,7 @@ public:
// ensure fixed sampling rate for FIR freq filters to work!
interpol.add(ts, acc, onResample);
return step;
return gotStep;
}

44
sensors/imu/StepDetection3.h
View File

@@ -22,7 +22,7 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/iir/BiQuad.h"
#include "../../math/dsp/iir/BiQuadStack.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/DelayBuffer.h"
@@ -38,9 +38,12 @@ class StepDetection3 {
static constexpr float gravity = 9.81;
static constexpr float stepRate_hz = 2.0;
static constexpr float cutOff_hz = 3.0;
static constexpr float iirQ = 0.70;
static constexpr int sRate_hz = 100;
static constexpr int every_ms = 1000 / sRate_hz;
static constexpr float threshold = 1.0;
float threshold = 0.8;
float max = 0;
Timestamp maxTS;
@@ -48,8 +51,8 @@ class StepDetection3 {
private:
FixedFrequencyInterpolator<AccelerometerData> interpol;
IIR::BiQuad<float> biquad;
DelayBuffer<float> delay;
IIR::BiQuadStack<float> biquad;
DelayBuffer<float> lookBehind;
@@ -72,10 +75,23 @@ private:
public:
/** ctor */
StepDetection3() : interpol(Timestamp::fromMS(every_ms)), delay(10) {
StepDetection3(bool useBandPass) : interpol(Timestamp::fromMS(every_ms)), lookBehind(5) {
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
biquad.preFill(gravity);
//biquad.setBandPass(stepRate_hz, 1.5, sRate_hz);
//biquad.preFill(gravity);
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";
@@ -105,10 +121,10 @@ public:
const float mag = data.magnitude();
// apply filter
const float fMag = biquad.filter(mag);
const float fMag = biquad.filter(mag - 9.81); // remove gravity
// history buffer
float fMagOld = delay.add(fMag);
float fMagOld = lookBehind.add(fMag);
// zero crossing?
float tmp = max;
@@ -117,16 +133,22 @@ public:
step = true;
gotStep = true;
}
delay.setAll(0);
lookBehind.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;
// // track delay due to zero crossing
// const float tsDelay = ts.ms() - maxTS.ms();
// std::cout << "step at " << ts.ms() << ". delay due to zero crossing: " << tsDelay << std::endl;
outSteps << maxTS.ms() << " " << tmp << "\n";
outSteps.flush();
}

34
sensors/imu/StepDetection4.h
View File

@@ -24,7 +24,7 @@
#endif
#include "../../Assertions.h"
#include "../../math/dsp/iir/BiQuad.h"
#include "../../math/dsp/iir/BiQuadStack.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/DelayBuffer.h"
@@ -43,7 +43,7 @@ class StepDetection4 {
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 threshold = 0.8;
float max = 0;
Timestamp maxTS;
@@ -52,7 +52,7 @@ private:
PoseDetection* pose;
FixedFrequencyInterpolator<Vector3> interpol;
IIR::BiQuad<float> biquad;
IIR::BiQuadStack<float> biquad;
DelayBuffer<float> delay;
@@ -75,16 +75,30 @@ private:
public:
/** ctor */
StepDetection4(PoseDetection* pose) : pose(pose), interpol(Timestamp::fromMS(every_ms)), delay(10) {
StepDetection4(PoseDetection* pose, bool useBandPass) : 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);
biquad.setBandPass(stepRate_hz, 1.0, sRate_hz);
biquad.preFill(gravity);
if (useBandPass) {
biquad.resize(3);
biquad[0].setHighPass(1, 0.7, sRate_hz);
biquad[1].setLowPass(3.0, 0.7, sRate_hz);
biquad[2].setLowPass(3.0, 1.0, sRate_hz);
//biquad.setBandPass(2, 3.0, sRate_hz);
threshold = 0.6; // needs a little reduction
} else {
threshold = 0.8;
biquad.resize(1);
biquad[0].setLowPass(3, 0.7, sRate_hz);
}
#ifdef WITH_DEBUG_PLOT
plot.getKey().setVisible(true);
lineRaw.setTitle("unrotated Z");
lineFiltered.setTitle("IIR filtered");
gp << "set autoscale xfix\n";
plot.setTitle("Step Detection");
plot.add(&lineRaw); lineRaw.getStroke().getColor().setHexStr("#0000FF");
@@ -123,7 +137,7 @@ public:
const float mag = data.z;
// apply filter
const float fMag = biquad.filter(mag);
const float fMag = biquad.filter(mag - gravity);
// history buffer
float fMagOld = delay.add(fMag);