From 628aafaecda786674c6d9682d270f56b5dbb24d2 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 9 May 2018 18:24:07 +0200 Subject: [PATCH] worked on FIR-Convolution and LocalMaxima detection --- main.cpp | 12 +- math/LocalMaxima.h | 64 +++++++++ math/dsp/Convolution.h | 10 ++ math/dsp/FIRComplex.h | 214 ++++++++++++++++++++++++++++++ sensors/imu/StepDetection.h | 83 +----------- sensors/imu/StepDetection2.h | 149 +++++++++++++++++++++ tests/math/dsp/TestFIRComplex.cpp | 45 +++++++ 7 files changed, 490 insertions(+), 87 deletions(-) create mode 100644 math/LocalMaxima.h create mode 100644 math/dsp/Convolution.h create mode 100644 math/dsp/FIRComplex.h create mode 100644 sensors/imu/StepDetection2.h create mode 100644 tests/math/dsp/TestFIRComplex.cpp diff --git a/main.cpp b/main.cpp index 8539cc9..05b1969 100755 --- a/main.cpp +++ b/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 @@ -18,6 +18,9 @@ class Test : public GridPoint { #include #include +#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*"; diff --git a/math/LocalMaxima.h b/math/LocalMaxima.h new file mode 100644 index 0000000..8f507e0 --- /dev/null +++ b/math/LocalMaxima.h @@ -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 diff --git a/math/dsp/Convolution.h b/math/dsp/Convolution.h new file mode 100644 index 0000000..a4d8291 --- /dev/null +++ b/math/dsp/Convolution.h @@ -0,0 +1,10 @@ +#ifndef CONVOLUTION_H +#define CONVOLUTION_H + +class Convolution { + + + +}; + +#endif // CONVOLUTION_H diff --git a/math/dsp/FIRComplex.h b/math/dsp/FIRComplex.h new file mode 100644 index 0000000..0399608 --- /dev/null +++ b/math/dsp/FIRComplex.h @@ -0,0 +1,214 @@ +#ifndef FIRCOMPLEX_H +#define FIRCOMPLEX_H + +#include +#include +#include "../../Assertions.h" + +/** + * FIR filter using complex convolution + */ +class FIRComplex { + + /** signal's sample-rate */ + int sRate_hz; + + /** the created convolution kernel */ + std::vector> kernel; + + /** incoming data */ + std::vector> data; + +public: + + /** ctor with signal's sample-rate */ + FIRComplex(const int sRate_hz) : sRate_hz(sRate_hz) { + ; + } + + /** get the internal kernel */ + const std::vector> 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> append(const std::vector& 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(f, 0)); // real = value, imag = 0; + } + + return processLocalBuffer(); + + } + + /** filter the given incoming complex data */ + std::vector> append(const std::vector>& 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 append(const float val) { + data.push_back(std::complex(val, 0)); + auto tmp = processLocalBuffer(); + if (tmp.size() == 0) {return std::complex(NAN, NAN);} + if (tmp.size() == 1) {return tmp[0];} + throw Exception("FIRComplex:: detected invalid result"); + } + + /** filter the given incoming real value */ + std::complex append(const std::complex c) { + data.push_back(c); + auto tmp = processLocalBuffer(); + if (tmp.size() == 0) {return std::complex(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 c : kernel) { + out << "(" << c.real() << "," << c.imag() << ")" << "\n"; + } + out.close(); + + } + + +private: + + std::vector> 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>();} + + // result-vector + std::vector> 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 void convolve(const std::complex* 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 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(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(real, imag); + } + } + + // https://dsp.stackexchange.com/questions/4693/fir-filter-gain + /** normalize using the DC-part of the kernel */ + static void normalizeDC(std::vector>& kernel) { + std::complex 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>& kernel, const float freq) { + throw std::runtime_error("TODO"); + } + + /** build a lowpass filter kernel */ + static std::vector> getLowpass(const int cutOff, const int sRate, const int n) { + + std::vector> 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(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 diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index e0e7b74..8478bba 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.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; - -// } - -// } - - }; diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h new file mode 100644 index 0000000..36ef37d --- /dev/null +++ b/sensors/imu/StepDetection2.h @@ -0,0 +1,149 @@ +#ifndef STEPDETECTION2_H +#define STEPDETECTION2_H + + +#include "AccelerometerData.h" +#include "../../data/Timestamp.h" + +#include +#include + +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include +#endif + +#ifdef WITH_DEBUG_OUTPUT + #include +#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 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 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 diff --git a/tests/math/dsp/TestFIRComplex.cpp b/tests/math/dsp/TestFIRComplex.cpp new file mode 100644 index 0000000..8b667e4 --- /dev/null +++ b/tests/math/dsp/TestFIRComplex.cpp @@ -0,0 +1,45 @@ +#ifdef WITH_TESTS + +#include +#include "../../Tests.h" +#include "../../../math/dsp/FIRComplex.h" +#include + + +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 noise(0.0, 0.3); + + std::vector> 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 values; + values.push_back(s+n); + fileO << v << "\n"; + std::vector> res = f.append(values); + out.insert(out.end(), res.begin(), res.end()); + } + + + for (const std::complex c : out) { + fileF << c.real() << "\n"; + } + +} + +#endif