140 lines
4.3 KiB
C++
140 lines
4.3 KiB
C++
#ifndef SYNTHETICSTEPS_H
|
|
#define SYNTHETICSTEPS_H
|
|
|
|
#include "SyntheticWalker.h"
|
|
|
|
#include "../sensors/imu/AccelerometerData.h"
|
|
|
|
#include "../math/distribution/Normal.h"
|
|
|
|
/**
|
|
* fakes accelerometer-data
|
|
* based on synthetic walking data
|
|
*/
|
|
class SyntheticSteps : SyntheticWalker::Listener {
|
|
|
|
public:
|
|
|
|
class Listener {
|
|
public:
|
|
virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0;
|
|
};
|
|
|
|
private:
|
|
|
|
/** the walker to listen to */
|
|
SyntheticWalker* walker;
|
|
|
|
///** the pedestrian's step-size (in meter) */
|
|
//float stepSize_m = 0;
|
|
|
|
///** when walking stairs, the step size is much smaller */
|
|
//float stepSizeStair_m = 0;
|
|
|
|
float lastStepAtDistance = 0;
|
|
|
|
Timestamp refStepPattern;
|
|
Interpolator<Timestamp, AccelerometerData> stepPatternPlain;
|
|
Interpolator<Timestamp, AccelerometerData> stepPatternStair;
|
|
|
|
Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
|
|
Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
|
|
Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
|
|
|
|
|
|
int stepPatternPos = -1;
|
|
|
|
std::vector<Listener*> listeners;
|
|
|
|
//float stepSize_m;
|
|
//float stepSizeSigma_m;
|
|
float noiseLevel;
|
|
Distribution::Normal<float> dNextStep;
|
|
Distribution::Normal<float> dNextStepStair;
|
|
|
|
public:
|
|
|
|
/** ctor with the walker to follow */
|
|
SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.35, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) :
|
|
//stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m),
|
|
noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) {
|
|
|
|
walker->addListener(this);
|
|
dX.setSeed(1);
|
|
dY.setSeed(3);
|
|
dZ.setSeed(5);
|
|
|
|
// build the step-pattern (how does a step look-like on the accelerometer)
|
|
// TODO: switch to MS?! use interpolator?
|
|
// int duration_ms = 350;
|
|
// for (int i = 0; i < duration_ms; i += 10) {
|
|
// float z = std::sin(i*M_PI*2/duration_ms) * 3.0;
|
|
// if (z < 0) {z *= 0.75;} // less pronounced in the negative part
|
|
// float y = std::cos(i*M_PI*2/duration_ms) * 0.5;
|
|
// const float x = dO.draw();
|
|
// z += dO.draw()*2;
|
|
// y += dO.draw();
|
|
// AccelerometerData acc(x,y,z);
|
|
// stepPattern.add(Timestamp::fromMS(i), acc);
|
|
// }
|
|
|
|
stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
|
|
stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
|
|
stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
|
|
stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
|
|
|
|
stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
|
|
stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4));
|
|
stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5));
|
|
stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0));
|
|
|
|
}
|
|
|
|
/** attach a listener to this provider */
|
|
void addListener(Listener* l) {
|
|
this->listeners.push_back(l);
|
|
}
|
|
|
|
protected:
|
|
|
|
|
|
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {
|
|
|
|
(void) curPos;
|
|
const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw());
|
|
const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair);
|
|
const float nextStepAt = lastStepAtDistance + distAdd;
|
|
|
|
// 1st, start with random noise on the accelerometer
|
|
const float x = dX.draw();
|
|
const float y = dY.draw();
|
|
const float z = dZ.draw();
|
|
const AccelerometerData aBase(0, 4, 9.7);
|
|
const AccelerometerData aNoise(x, y, z);
|
|
AccelerometerData acc = aBase + aNoise * noiseLevel;
|
|
|
|
// is it time to inject a "step" into the accelerometer data?
|
|
if (walkedDistance > nextStepAt) {
|
|
lastStepAtDistance = walkedDistance;
|
|
refStepPattern = walkedTime;
|
|
}
|
|
|
|
// overlay the noise with a step-pattern (see ctor)
|
|
if (refStepPattern.ms() > 0) {
|
|
Timestamp curPatPos = walkedTime - refStepPattern;
|
|
if (curPatPos >= stepPattern.getMaxKey()) {
|
|
refStepPattern = Timestamp::fromMS(0);
|
|
} else {
|
|
const AccelerometerData step = stepPattern.get(curPatPos);
|
|
acc = aBase + (aNoise * noiseLevel) + step;
|
|
}
|
|
}
|
|
|
|
for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // SYNTHETICSTEPS_H
|