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

worked on grid-walker and synthetic steps/turns

Browse Source
This commit is contained in:
k-a-z-u
2017-11-29 16:35:29 +01:00
parent 55c061b344
commit 63bc2f3046
5 changed files with 370 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
synthetic/SyntheticSteps.h
View File

@@ -37,14 +37,22 @@ private:
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;
public:
/** ctor with the walker to follow */
SyntheticSteps(SyntheticWalker* walker) {
SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, 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) {
walker->addListener(this);
dX.setSeed(1);
@@ -82,15 +90,15 @@ protected:
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
(void) curPos;
const float nextStepAt = (lastStepAtDistance + stepSize_m);
const float nextStepAt = lastStepAtDistance + dNextStep.draw();
// 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;
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) {
@@ -105,7 +113,7 @@ protected:
refStepPattern = Timestamp::fromMS(0);
} else {
const AccelerometerData step = stepPattern.get(curPatPos);
acc = base + noise*2.5f + step;
acc = aBase + (aNoise * noiseLevel) + step;
}
}

23
synthetic/SyntheticTurns.h
View File

@@ -39,17 +39,23 @@ private:
Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
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) {
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);
@@ -57,6 +63,7 @@ public:
dGyroX.setSeed(7);
dGyroY.setSeed(9);
dGyroZ.setSeed(11);
}
/** attach a listener to this provider */
@@ -116,14 +123,14 @@ protected:
// convert to gyro's radians-per-second
const double radPerSec = change * 1000 / deltaTs.ms();;
const float accX = 0.00 + dAccX.draw();
const float accY = 0.00 + dAccY.draw();
const float accZ = 9.81 + dAccZ.draw();
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();
const float gyroY = dGyroY.draw();
const float gyroZ = dGyroZ.draw() + radPerSec;
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);}