143 lines
4.3 KiB
C++
143 lines
4.3 KiB
C++
#ifndef INDOOR_SYNTHETICTURNS_H
|
|
#define INDOOR_SYNTHETICTURNS_H
|
|
|
|
#include "SyntheticWalker.h"
|
|
|
|
#include "../sensors/imu/AccelerometerData.h"
|
|
#include "../sensors/imu/GyroscopeData.h"
|
|
#include "../geo/Heading.h"
|
|
|
|
#include "../math/Distributions.h"
|
|
|
|
/**
|
|
* simulates acceleromter and gyroscope data
|
|
* based on synthetic walking data
|
|
*
|
|
* @brief The SyntheticTurns class
|
|
*/
|
|
class SyntheticTurns : public SyntheticWalker::Listener {
|
|
|
|
public:
|
|
|
|
class Listener {
|
|
public:
|
|
virtual void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) = 0;
|
|
};
|
|
|
|
private:
|
|
|
|
/** the walker to listen to */
|
|
SyntheticWalker* walker;
|
|
|
|
Distribution::Normal<float> dAccX = Distribution::Normal<float>(0, 2.5);
|
|
Distribution::Normal<float> dAccY = Distribution::Normal<float>(0, 1.5);
|
|
Distribution::Normal<float> dAccZ = Distribution::Normal<float>(0, 1);
|
|
|
|
Distribution::Normal<float> dGyroX = Distribution::Normal<float>(0, 0.02);
|
|
Distribution::Normal<float> dGyroY = Distribution::Normal<float>(0, 0.02);
|
|
Distribution::Normal<float> dGyroZ = Distribution::Normal<float>(0, 0.02);
|
|
|
|
Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
|
|
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
|
|
|
|
Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
|
|
|
|
//float headingDrift_rad;
|
|
//float headingSigma_rad;
|
|
float noiseLevel;
|
|
Distribution::Normal<float> dHeadErr;
|
|
|
|
std::vector<Listener*> listeners;
|
|
|
|
public:
|
|
|
|
/** ctor with the walker to follow */
|
|
SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) :
|
|
//headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad),
|
|
noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) {
|
|
|
|
walker->addListener(this);
|
|
dAccX.setSeed(1);
|
|
dAccY.setSeed(3);
|
|
dAccZ.setSeed(5);
|
|
dGyroX.setSeed(7);
|
|
dGyroY.setSeed(9);
|
|
dGyroZ.setSeed(11);
|
|
|
|
}
|
|
|
|
/** attach a listener to this provider */
|
|
void addListener(Listener* l) {
|
|
this->listeners.push_back(l);
|
|
}
|
|
|
|
protected:
|
|
|
|
Timestamp lastTs;
|
|
Heading desiredHead = Heading(0);
|
|
Heading curHead = Heading(0);
|
|
Point3 lastPos = Point3(NAN, NAN, NAN);
|
|
double change = 0;
|
|
|
|
inline float clamp(const float val, const float min, const float max) {
|
|
if (val < min) {return min;}
|
|
if (val > max) {return max;}
|
|
return val;
|
|
}
|
|
|
|
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
|
|
|
|
// time sine last onWalk();
|
|
if (lastTs.isZero()) {lastTs = walkedTime; return;}
|
|
const Timestamp deltaTs = walkedTime - lastTs;
|
|
lastTs = walkedTime;
|
|
|
|
if (lastPos.x != lastPos.x) {
|
|
lastPos = curPos;
|
|
} else {
|
|
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();
|
|
lastPos = curPos;
|
|
}
|
|
|
|
// difference between current-heading and desired-heading
|
|
const double maxChange = dMaxChange.draw();
|
|
const double diffRad = Heading::getSignedDiff(curHead, desiredHead);
|
|
//change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange);
|
|
change = clamp(diffRad / 25, -maxChange, +maxChange);
|
|
|
|
|
|
// // slowly change the current heading to match the desired one
|
|
// //const double maxChange = dMaxChange.draw();
|
|
// //const double toChange = clamp(diffRad, -maxChange, +maxChange);
|
|
// const double toChange = diffRad;
|
|
// //if (change < toChange) {change += toChange*0.01;}
|
|
// if (change > toChange) {change *= 0.93;}
|
|
// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?!
|
|
// if (change < toChange) {change += (toChange-change) * 0.07;}
|
|
// //if (change > toChange) {change -= dChange.draw();}
|
|
|
|
|
|
|
|
curHead += change;
|
|
|
|
// convert to gyro's radians-per-second
|
|
const double radPerSec = change * 1000 / deltaTs.ms();;
|
|
|
|
const float accX = 0.00 + dAccX.draw() * (noiseLevel);
|
|
const float accY = 0.00 + dAccY.draw() * (noiseLevel);
|
|
const float accZ = 9.81 + dAccZ.draw() * (noiseLevel);
|
|
AccelerometerData acc(accX, accY, accZ);
|
|
|
|
const float gyroX = dGyroX.draw() * (noiseLevel);
|
|
const float gyroY = dGyroY.draw() * (noiseLevel);
|
|
const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec;
|
|
GyroscopeData gyro(gyroX, gyroY, gyroZ);
|
|
|
|
for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // INDOOR_SYNTHETICTURNS_H
|