worked on FIR-Convolution and LocalMaxima detection
This commit is contained in:
k-a-z-u
12
main.cpp
12
main.cpp
@@ -10,7 +10,7 @@ class Test : public GridPoint {
|
||||
|
||||
#include "tests/Tests.h"
|
||||
|
||||
#include "sensors/radio/scan/WiFiScanLinux.h"
|
||||
|
||||
#include "sensors/radio/VAPGrouper.h"
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
@@ -18,6 +18,9 @@ class Test : public GridPoint {
|
||||
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
|
||||
|
||||
#ifdef WIFI_LINUX
|
||||
#include "sensors/radio/scan/WiFiScanLinux.h"
|
||||
|
||||
void wifi() {
|
||||
|
||||
K::Gnuplot gp;
|
||||
@@ -76,10 +79,9 @@ void wifi() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
@@ -109,7 +111,7 @@ int main(int argc, char** argv) {
|
||||
//::testing::GTEST_FLAG(filter) = "*Matrix4*";
|
||||
//::testing::GTEST_FLAG(filter) = "*Sphere3*";
|
||||
|
||||
::testing::GTEST_FLAG(filter) = "Ray.ModelFac*";
|
||||
::testing::GTEST_FLAG(filter) = "*FIRComplex*";
|
||||
//::testing::GTEST_FLAG(filter) = "Timestamp*";
|
||||
|
||||
//::testing::GTEST_FLAG(filter) = "*RayTrace3*";
|
||||
|
||||
64
math/LocalMaxima.h
Normal file
64
math/LocalMaxima.h
Normal file
@@ -0,0 +1,64 @@
|
||||
#ifndef LOCALMAXIMA_H
|
||||
#define LOCALMAXIMA_H
|
||||
|
||||
class LocalMaxima {
|
||||
|
||||
static constexpr float MAX = 1e40;
|
||||
|
||||
size_t everyNth;
|
||||
|
||||
size_t cnt = 0;
|
||||
float s0;
|
||||
float s1; // center value
|
||||
float s2;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
struct Res {
|
||||
bool isMax;
|
||||
float val;
|
||||
Res(bool isMax, float val) : isMax(isMax), val(val) {;}
|
||||
};
|
||||
|
||||
/** ctor. use only every n-th sample */
|
||||
LocalMaxima(const size_t everyNth) : everyNth(everyNth) {
|
||||
reset();
|
||||
}
|
||||
|
||||
/** is the given value a local maxima? */
|
||||
Res add(const float s) {
|
||||
|
||||
if (cnt == 0*everyNth) {s0 = s;} // set, wait some time
|
||||
else if (cnt == 1*everyNth) {s1 = s;} // set, wait some time
|
||||
else if (cnt == 2*everyNth) {s2 = s;} // set
|
||||
else if (cnt > 2*everyNth) { // now shift values for every time step, until max is found
|
||||
s0 = s1;
|
||||
s1 = s2;
|
||||
s2 = s;
|
||||
}
|
||||
|
||||
++cnt;
|
||||
|
||||
if ((s1 > s0) && (s1 > s2)) {
|
||||
Res res(true, s1);
|
||||
reset();
|
||||
return res;
|
||||
}
|
||||
|
||||
return Res(false, 0);
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
void reset() {
|
||||
s0 = MAX;
|
||||
s1 = MAX;
|
||||
s2 = MAX;
|
||||
cnt = 0;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // LOCALMAXIMA_H
|
||||
10
math/dsp/Convolution.h
Normal file
10
math/dsp/Convolution.h
Normal file
@@ -0,0 +1,10 @@
|
||||
#ifndef CONVOLUTION_H
|
||||
#define CONVOLUTION_H
|
||||
|
||||
class Convolution {
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // CONVOLUTION_H
|
||||
214
math/dsp/FIRComplex.h
Normal file
214
math/dsp/FIRComplex.h
Normal file
@@ -0,0 +1,214 @@
|
||||
#ifndef FIRCOMPLEX_H
|
||||
#define FIRCOMPLEX_H
|
||||
|
||||
#include <vector>
|
||||
#include <complex>
|
||||
#include "../../Assertions.h"
|
||||
|
||||
/**
|
||||
* FIR filter using complex convolution
|
||||
*/
|
||||
class FIRComplex {
|
||||
|
||||
/** signal's sample-rate */
|
||||
int sRate_hz;
|
||||
|
||||
/** the created convolution kernel */
|
||||
std::vector<std::complex<float>> kernel;
|
||||
|
||||
/** incoming data */
|
||||
std::vector<std::complex<float>> data;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor with signal's sample-rate */
|
||||
FIRComplex(const int sRate_hz) : sRate_hz(sRate_hz) {
|
||||
;
|
||||
}
|
||||
|
||||
/** get the internal kernel */
|
||||
const std::vector<std::complex<float>> getKernel() const {
|
||||
return kernel;
|
||||
}
|
||||
|
||||
/** configure as lowpass with the given cutoff and 2*size+1 */
|
||||
void lowPass(const int cutOff_hz, const int size) {
|
||||
this->kernel = getLowpass(cutOff_hz, sRate_hz, size);
|
||||
}
|
||||
|
||||
/** shift the constructed filter by the given hz-rate */
|
||||
void shiftBy(const int shift_hz) {
|
||||
shiftKernel(shift_hz, sRate_hz);
|
||||
}
|
||||
|
||||
/** filter the given incoming real data */
|
||||
std::vector<std::complex<float>> append(const std::vector<float>& newData) {
|
||||
|
||||
// append to local buffer (as we need some history)
|
||||
//data.insert(data.end(), newData.begin(), newData.end());
|
||||
for (const float f : newData) {
|
||||
data.push_back(std::complex<float>(f, 0)); // real = value, imag = 0;
|
||||
}
|
||||
|
||||
return processLocalBuffer();
|
||||
|
||||
}
|
||||
|
||||
/** filter the given incoming complex data */
|
||||
std::vector<std::complex<float>> append(const std::vector<std::complex<float>>& newData) {
|
||||
|
||||
// append to local buffer (as we need some history)
|
||||
data.insert(data.end(), newData.begin(), newData.end());
|
||||
|
||||
return processLocalBuffer();
|
||||
|
||||
}
|
||||
|
||||
/** filter the given incoming real value */
|
||||
std::complex<float> append(const float val) {
|
||||
data.push_back(std::complex<float>(val, 0));
|
||||
auto tmp = processLocalBuffer();
|
||||
if (tmp.size() == 0) {return std::complex<float>(NAN, NAN);}
|
||||
if (tmp.size() == 1) {return tmp[0];}
|
||||
throw Exception("FIRComplex:: detected invalid result");
|
||||
}
|
||||
|
||||
/** filter the given incoming real value */
|
||||
std::complex<float> append(const std::complex<float> c) {
|
||||
data.push_back(c);
|
||||
auto tmp = processLocalBuffer();
|
||||
if (tmp.size() == 0) {return std::complex<float>(NAN, NAN);}
|
||||
if (tmp.size() == 1) {return tmp[0];}
|
||||
throw Exception("FIRComplex:: detected invalid result");
|
||||
}
|
||||
|
||||
void dumpKernel(const std::string& file, const std::string& varName) {
|
||||
|
||||
std::ofstream out(file);
|
||||
out << "# name: " << varName << "\n";
|
||||
out << "# type: complex matrix\n";
|
||||
out << "# rows: " << kernel.size() << "\n";
|
||||
out << "# columns: 1\n";
|
||||
|
||||
for (const std::complex<float> c : kernel) {
|
||||
out << "(" << c.real() << "," << c.imag() << ")" << "\n";
|
||||
}
|
||||
out.close();
|
||||
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
std::vector<std::complex<float>> processLocalBuffer() {
|
||||
|
||||
// sanity check
|
||||
Assert::isNot0(kernel.size(), "FIRComplex:: kernel not yet configured!");
|
||||
|
||||
// number of processable elements (due to filter size)
|
||||
const int processable = data.size() - kernel.size() + 1 - kernel.size()/2;
|
||||
|
||||
// nothing to-do?
|
||||
if (processable <= 0) {return std::vector<std::complex<float>>();}
|
||||
|
||||
// result-vector
|
||||
std::vector<std::complex<float>> res;
|
||||
res.resize(processable);
|
||||
|
||||
// fire
|
||||
convolve(data.data(), res.data(), processable);
|
||||
|
||||
// drop processed elements from the local buffer
|
||||
data.erase(data.begin(), data.begin() + processable);
|
||||
|
||||
// done
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
template <typename T> void convolve(const std::complex<float>* src, T* dst, const size_t cnt) {
|
||||
|
||||
const size_t ks = kernel.size();
|
||||
|
||||
for (size_t i = 0; i < cnt; ++i) {
|
||||
T t = T();
|
||||
for (size_t j = 0; j < ks; ++j) {
|
||||
t += src[j+i] * kernel[j];
|
||||
}
|
||||
if (t != t) {throw std::runtime_error("detected NaN");}
|
||||
dst[i] = t;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// template <typename T> void convolve(const float* src, T* dst, const size_t cnt) {
|
||||
|
||||
// const size_t ks = kernel.size();
|
||||
|
||||
// for (size_t i = 0; i < cnt; ++i) {
|
||||
// T t = T();
|
||||
// for (size_t j = 0; j < ks; ++j) {
|
||||
// t += std::complex<float>(src[j+i], 0) * kernel[j];
|
||||
// }
|
||||
// if (t != t) {throw std::runtime_error("detected NaN");}
|
||||
// dst[i] = t;
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
/** get a value from the hamming window */
|
||||
static double winHamming(const double t, const double size) {
|
||||
return 0.54 - 0.46 * std::cos(2 * M_PI * t / size);
|
||||
}
|
||||
|
||||
/** frequency shift the kernel by multiplying with a frequency */
|
||||
void shiftKernel(const int shift_hz, const int sRate_hz) {
|
||||
for (size_t i = 0; i < kernel.size(); ++i) {
|
||||
const float t = (float) i / (float) sRate_hz;
|
||||
const float real = std::cos(t * 2 * M_PI * shift_hz);
|
||||
const float imag = std::sin(t * 2 * M_PI * shift_hz);
|
||||
kernel[i] = kernel[i] * std::complex<float>(real, imag);
|
||||
}
|
||||
}
|
||||
|
||||
// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
|
||||
/** normalize using the DC-part of the kernel */
|
||||
static void normalizeDC(std::vector<std::complex<float>>& kernel) {
|
||||
std::complex<float> sum;
|
||||
for (auto f : kernel) {sum += f;}
|
||||
for (auto& f : kernel) {f /= sum;}
|
||||
}
|
||||
|
||||
// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
|
||||
static void normalizeAC(std::vector<std::complex<float>>& kernel, const float freq) {
|
||||
throw std::runtime_error("TODO");
|
||||
}
|
||||
|
||||
/** build a lowpass filter kernel */
|
||||
static std::vector<std::complex<float>> getLowpass(const int cutOff, const int sRate, const int n) {
|
||||
|
||||
std::vector<std::complex<float>> kernel;
|
||||
|
||||
for (int i = -n; i <= +n; ++i) {
|
||||
|
||||
const double t = (double) i / (double) sRate;
|
||||
const double tmp = 2 * M_PI * cutOff * t;
|
||||
const double val = (tmp == 0) ? (1) : (std::sin(tmp) / tmp);
|
||||
const double win = winHamming(i+n, n*2);
|
||||
const double res = val * win;// * 0.5f; // why 0.5?
|
||||
if (res != res) {throw std::runtime_error("detected NaN");}
|
||||
kernel.push_back( std::complex<float>(res, 0) );
|
||||
|
||||
}
|
||||
|
||||
// important!!! normalize so the original frequencies stay at 0dB
|
||||
normalizeDC(kernel); // dc works for low-pass filter only as this one contains DC!
|
||||
|
||||
return kernel;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // FIRCOMPLEX_H
|
||||
@@ -19,6 +19,7 @@
|
||||
|
||||
#include "../../Assertions.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/dsp/FIRComplex.h"
|
||||
|
||||
|
||||
/**
|
||||
@@ -145,88 +146,6 @@ public:
|
||||
|
||||
}
|
||||
|
||||
|
||||
//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;
|
||||
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
149
sensors/imu/StepDetection2.h
Normal file
149
sensors/imu/StepDetection2.h
Normal file
@@ -0,0 +1,149 @@
|
||||
#ifndef STEPDETECTION2_H
|
||||
#define STEPDETECTION2_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/FIRComplex.h"
|
||||
#include "../../math/FixedFrequencyInterpolator.h"
|
||||
#include "../../math/LocalMaxima.h"
|
||||
|
||||
|
||||
/**
|
||||
* simple step detection based on accelerometer magnitude.
|
||||
* magnitude > threshold? -> step!
|
||||
* block for several msec until detecting the next one
|
||||
*/
|
||||
class StepDetection2 {
|
||||
|
||||
static constexpr int sRate_hz = 75;
|
||||
static constexpr int every_ms = 1000 / sRate_hz;
|
||||
|
||||
private:
|
||||
|
||||
FixedFrequencyInterpolator<AccelerometerData> interpol;
|
||||
FIRComplex fir;
|
||||
LocalMaxima locMax;
|
||||
|
||||
const float threshold = 0.5;
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines lineMag;
|
||||
K::GnuplotPlotElementPoints pointDet;
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
#endif
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
std::ofstream outFiltered;
|
||||
std::ofstream outSteps;
|
||||
#endif
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) {
|
||||
|
||||
fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz
|
||||
fir.shiftBy(2); // typical step freq ~2Hz
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
gp << "set autoscale xfix\n";
|
||||
plot.setTitle("Step Detection");
|
||||
plot.add(&lineMag); lineMag.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 step = false;
|
||||
|
||||
auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
|
||||
|
||||
const float mag = data.magnitude();
|
||||
|
||||
const std::complex<float> c = fir.append(mag);
|
||||
const float real = c.real();
|
||||
if (real != real) {return;}
|
||||
const float fMag = real;
|
||||
|
||||
LocalMaxima::Res res = locMax.add(fMag);
|
||||
step = (res.isMax) && (res.val > threshold);
|
||||
|
||||
#ifdef WITH_DEBUG_OUTPUT
|
||||
if (step) {
|
||||
outSteps << ts.ms() << " " << fMag << "\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);
|
||||
|
||||
lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
|
||||
|
||||
if (step) {
|
||||
pointDet.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
|
||||
}
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||||
lineMag.removeIf(remove);
|
||||
pointDet.removeIf(remove);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
usleep(100);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
interpol.add(ts, acc, onResample);
|
||||
|
||||
return step;
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // STEPDETECTION2_H
|
||||
45
tests/math/dsp/TestFIRComplex.cpp
Normal file
45
tests/math/dsp/TestFIRComplex.cpp
Normal file
@@ -0,0 +1,45 @@
|
||||
#ifdef WITH_TESTS
|
||||
|
||||
#include <fstream>
|
||||
#include "../../Tests.h"
|
||||
#include "../../../math/dsp/FIRComplex.h"
|
||||
#include <random>
|
||||
|
||||
|
||||
TEST(FIRComplex, filter1) {
|
||||
|
||||
const float sRate = 200;
|
||||
const float freq = 10;
|
||||
|
||||
FIRComplex f(sRate);
|
||||
f.lowPass(5, 50); f.dumpKernel("/tmp/k1.m", "k1");
|
||||
f.shiftBy(freq); f.dumpKernel("/tmp/k2.m", "k2");
|
||||
|
||||
std::minstd_rand gen;
|
||||
std::normal_distribution<float> noise(0.0, 0.3);
|
||||
|
||||
std::vector<std::complex<float>> out;
|
||||
|
||||
std::ofstream fileO("/tmp/orig.dat");
|
||||
std::ofstream fileF("/tmp/filtered.dat");
|
||||
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
const float t = i / sRate;
|
||||
const float n = noise(gen);
|
||||
const float s = std::sin(2*M_PI*freq*t);
|
||||
const float v = s+n;
|
||||
std::vector<float> values;
|
||||
values.push_back(s+n);
|
||||
fileO << v << "\n";
|
||||
std::vector<std::complex<float>> res = f.append(values);
|
||||
out.insert(out.end(), res.begin(), res.end());
|
||||
}
|
||||
|
||||
|
||||
for (const std::complex<float> c : out) {
|
||||
fileF << c.real() << "\n";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user