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

added IIR stuff

Browse Source 
worked on StepDetection
This commit is contained in:
FrankE
2018-07-17 09:53:30 +02:00
parent 19d9ce3961
commit 2dee085131
4 changed files with 532 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
math/DelayBuffer.h Normal file
View 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

2
math/dsp/fir/Complex.h
View File

@@ -3,7 +3,7 @@
#include <vector> #include <vector>
#include <complex> #include <complex>
#include "../../Assertions.h" #include "../../../Assertions.h"
/** /**
* FIR filter using complex convolution * FIR filter using complex convolution

322
math/dsp/iir/BiQuad.h Normal file
View 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

176
sensors/imu/StepDetection3.h Normal file
View File

@@ -0,0 +1,176 @@
#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.
* magnitude > threshold? -> step!
* block for several msec until detecting the next one
*/
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