119 lines
3.1 KiB
C++
119 lines
3.1 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.7;
|
|
|
|
float lastStepAtDistance = 0;
|
|
|
|
Timestamp refStepPattern;
|
|
Interpolator<Timestamp, AccelerometerData> stepPattern;
|
|
|
|
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;
|
|
|
|
public:
|
|
|
|
/** ctor with the walker to follow */
|
|
SyntheticSteps(SyntheticWalker* walker) {
|
|
|
|
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);
|
|
// }
|
|
stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
|
|
stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
|
|
stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
|
|
stepPattern.add(Timestamp::fromMS(450), 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) override {
|
|
|
|
(void) curPos;
|
|
const float nextStepAt = (lastStepAtDistance + stepSize_m);
|
|
|
|
// 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 base(0, 4, 9.7);
|
|
const AccelerometerData noise(x, y, z);
|
|
AccelerometerData acc = base + noise;
|
|
|
|
// 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 = base + noise*2.5f + step;
|
|
}
|
|
}
|
|
|
|
for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // SYNTHETICSTEPS_H
|