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

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);