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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
kazu
2018-06-06 11:40:22 +02:00
parent 2036469491 38b633b9be
commit b50995ffe6
6 changed files with 143 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
math/Delay.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef DELAY_H
#define DELAY_H
#include <cstdint>
#include <vector>
template <typename T> class Delay {
private:
size_t size;
/** up to "size" elements */
std::vector<T> values;
public:
/** ctor */
Delay(const int size) : size(size) {;}
/** add a new value */
void add(const T val) {
// add new value
values.push_back(val);
// remove old ones
while(values.size() > size) {
values.erase(values.begin());
}
}
/** get the delayed value */
T get() const {
return values.front();
}
/** delay output valid? */
bool isValid() const {
return values.size() == size;
}
};
#endif // DELAY_H

63
math/LocalMaxima.h
View File

@@ -1,6 +1,63 @@
#ifndef LOCALMAXIMA_H
#define LOCALMAXIMA_H
#include "Delay.h"
class LocalMaxima {
int delay;
Delay<float> d0;
Delay<float> d1;
float cur;
int block = 0;
public:
LocalMaxima(int delay) : delay(delay), d0(delay*2), d1(delay) {
;
}
bool add(const float s) {
if (block > 0) {--block;}
d0.add(s);
d1.add(s);
this->cur = s;
if (isMax()) {
block = delay;
return true;
}
return false;
}
float get() {
return d1.get();
}
private:
bool isMax() {
if (block > 0) {return false;}
if (!d0.isValid()) {return false;}
if (!d1.isValid()) {return false;}
const float s0 = d0.get();
const float s1 = d1.get();
const float s2 = cur;
return (s1 > s0) && (s1 > s2);
}
};
/*
class LocalMaxima {
static constexpr float MAX = 1e40;
@@ -21,12 +78,12 @@ public:
Res(bool isMax, float val) : isMax(isMax), val(val) {;}
};
/** ctor. use only every n-th sample */
// ctor. use only every n-th sample
LocalMaxima(const size_t everyNth) : everyNth(everyNth) {
reset();
}
/** is the given value a local maxima? */
// is the given value a local maxima?
Res add(const float s) {
if (cnt == 0*everyNth) {s0 = s;} // set, wait some time
@@ -60,5 +117,5 @@ private:
}
};
*/
#endif // LOCALMAXIMA_H

4
sensors/imu/PoseDetectionPlot.h
View File

@@ -8,10 +8,10 @@
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
//#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
//#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
#include "../../data/Timestamp.h"
#include "../../math/Matrix3.h"

4
sensors/imu/StepDetection.h
View File

@@ -38,8 +38,8 @@ private:
bool waitForUp = false;
const Timestamp blockTime; // 150-250 looks good
const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing!
const float upperThreshold = +0.4*0.8f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -1.5*0.8f; // the 0.8 is for testing!
#ifdef WITH_DEBUG_PLOT

39
sensors/imu/StepDetection2.h
View File

@@ -25,6 +25,7 @@
#include "../../math/dsp/FIRComplex.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/LocalMaxima.h"
#include "../../math/MovingAverageTS.h"
/**
@@ -43,12 +44,16 @@ private:
FIRComplex fir;
LocalMaxima locMax;
const float threshold = 0.5;
// longterm average to center around zero
MovingAverageTS<float> avg = MovingAverageTS<float>(Timestamp::fromMS(2000), 0);
const float threshold = 0.50;
#ifdef WITH_DEBUG_PLOT
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lineMag;
K::GnuplotPlotElementLines lineRaw;
K::GnuplotPlotElementLines lineFiltered;
K::GnuplotPlotElementPoints pointDet;
Timestamp plotRef;
Timestamp lastPlot;
@@ -63,15 +68,18 @@ private:
public:
/** ctor */
StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) {
StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(8) {
fir.lowPass(0.66, 40); // allow deviation of +/- 0.66Hz
fir.shiftBy(2); // typical step freq ~2Hz
//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
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";
plot.setTitle("Step Detection");
plot.add(&lineMag); lineMag.getStroke().getColor().setHexStr("#000000");
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
@@ -87,17 +95,23 @@ public:
bool step = false;
// accel-data incoming on a fixed sampling rate (needed for FIR to work)
auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {
const float mag = data.magnitude();
const std::complex<float> c = fir.append(mag);
// use long-term average to center around zero
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;
LocalMaxima::Res res = locMax.add(fMag);
step = (res.isMax) && (res.val > threshold);
const bool isMax = locMax.add(fMag);
step = (isMax) && (locMax.get() > threshold);
#ifdef WITH_DEBUG_OUTPUT
if (step) {
@@ -113,7 +127,8 @@ public:
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
lineRaw.add( K::GnuplotPoint2(tsPlot.ms(), mag) );
lineFiltered.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
if (step) {
pointDet.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );
@@ -123,7 +138,8 @@ public:
lastPlot = tsPlot;
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineMag.removeIf(remove);
lineRaw.removeIf(remove);
lineFiltered.removeIf(remove);
pointDet.removeIf(remove);
gp.draw(plot);
@@ -136,6 +152,7 @@ public:
};
// ensure fixed sampling rate for FIR freq filters to work!
interpol.add(ts, acc, onResample);
return step;

5
sensors/imu/TurnDetectionPlot.h
View File

@@ -104,7 +104,10 @@
gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
gp1.draw(multiplot);
//gp1.draw(plotGyroRaw); // raw only
gp1.draw(plotGyroFix); // fixed only
//gp1.draw(multiplot); // both
gp1.flush();
}