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

worked on synthetic sensors

Browse Source 
worked on grid-walker
minor changes/fixes/improvements
This commit is contained in:
k-a-z-u
2017-10-18 16:54:57 +02:00
parent 72f083d32a
commit 3e31f6da53
8 changed files with 522 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
grid/walk/v3/GridWalker3.h
View File

@@ -37,6 +37,7 @@ public:
struct WalkResult {
Point3 position;
Heading heading = Heading(0);
double probability = 1.0;
};
@@ -79,18 +80,26 @@ public:
return bbox.contains(pt);
}
/** does one of the given grid-node scontains the provided point-in-question? */
const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
for (const Node* n : nodes) {
if (contains(grid, n, pt)) {return n;}
}
return nullptr;
}
const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {
const GridPoint gpStart = p3ToGp(params.start);
const Node* startNode = grid.getNodePtrFor(gpStart);
static Distribution::Normal<float> dDist(1, 0.02);
static Distribution::Normal<float> dHead(0, 0.02);
static Distribution::Normal<float> dHead(0, 0.01);
// include one additional grid-cell (increased distance)
const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
const float range_m = params.distance_m + secBuffer_m;
const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m);
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);
WalkResult res;
res.heading = params.heading;
@@ -104,33 +113,39 @@ public:
const Point2 dst = params.start.xy() + (dir * realDist_m);
// is dst reachable?
for (const Node* n : nodes) {
//const float distToNode = n->inMeter().xy().getDistance(dst);
//if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) {
if (contains(grid, n, dst)) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = p3ToGp(p3);
if (grid.hasNodeFor(gp)) {
res.position = p3; // new position
res.heading; // keep as-is
return res;
} else {
std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl;
}
const Node* n = contains(grid, reachableNodes, dst);
if (n) {
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
const GridPoint gp = p3ToGp(p3);
if (grid.hasNodeFor(gp)) {
res.position = p3; // update position
res.heading; // keep as-is
res.probability; // keep as-is
return res; // done
} else {
std::cout << "WARN dst not found" << std::endl;
//throw "should not happen";
}
}
// reduce probability with every new run
++cnt;
res.probability /= 2;
// before trying again, modify distance and angle
if (1 == 0) {
realDist_m *= dDist.draw();
res.heading += dHead.draw();
}
//if (1 == 1) {
res.heading = params.heading + dHead.draw() * cnt;
realDist_m = params.distance_m * dDist.draw();// * cnt;
//}
// reached max retries?
if (++cnt > 10) {
WalkResult res;
res.position = params.start;
res.heading = params.heading;
if (cnt > 8) {
res.position = params.start; // reset
res.heading = params.heading; // reset
res.probability = 1e-50; // unlikely
return res;
} // did not work out....

8
sensors/imu/AccelerometerData.h
View File

@@ -42,6 +42,14 @@ struct AccelerometerData {
return AccelerometerData(x/val, y/val, z/val);
}
AccelerometerData operator * (const float val) const {
return AccelerometerData(x*val, y*val, z*val);
}
AccelerometerData operator + (const AccelerometerData o) const {
return AccelerometerData(x+o.x, y+o.y, z+o.z);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";

73
sensors/imu/StepDetection.h
View File

@@ -8,12 +8,14 @@
#include <cmath>
#include <vector>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#ifdef WITH_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
#endif
#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
@@ -43,11 +45,12 @@ private:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lineDet;
K::GnuplotPlotElementPoints pointDet;
K::GnuplotPlotElementLines lineX;
K::GnuplotPlotElementLines lineY;
K::GnuplotPlotElementLines lineZ;
Timestamp plotRef;
int plotCnt = 0;
Timestamp lastPlot;
#endif
@@ -57,10 +60,13 @@ public:
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
#ifdef WITH_DEBUG_PLOT
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000");
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00");
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff");
gp << "set autoscale xfix\n";
plot.setTitle("Step Detection");
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
#endif
}
@@ -78,19 +84,25 @@ public:
bool step = false;
if (blockUntil > ts) {return false;}
if (blockUntil > ts) {
// wait for a rising edge
if (waitForUp && delta > upperThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = false;
}
step = false;
} else {
// wait for a rising edge
if (waitForUp && delta > upperThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = false;
}
// wait for a falling edge
if (!waitForUp && delta < lowerThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = true;
step = true;
}
// wait for a falling edge
if (!waitForUp && delta < lowerThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = true;
step = true;
}
@@ -98,6 +110,7 @@ public:
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
@@ -105,10 +118,24 @@ public:
lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
if (++plotCnt % 25 == 0) {
if (step) {
pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
}
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
lastPlot = tsPlot;
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineX.removeIf(remove);
lineY.removeIf(remove);
lineZ.removeIf(remove);
lineDet.removeIf(remove);
pointDet.removeIf(remove);
gp.draw(plot);
gp.flush();
//usleep(1000*25);
usleep(100);
}
#endif

243
sensors/imu/TurnDetection.h
View File

@@ -5,18 +5,23 @@
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../geo/Point3.h"
#include <eigen3/Eigen/Dense>
#include <cmath>
#include <vector>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#ifdef WITH_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#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/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#endif
#include "../../Assertions.h"
@@ -24,6 +29,37 @@ class TurnDetection {
private:
#ifdef WITH_DEBUG_PLOT
Timestamp plotRef;
Timestamp lastPlot;
K::Gnuplot gp1;
K::Gnuplot gp2;
K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3);
K::GnuplotPlot plotGyroRaw;
K::GnuplotPlotElementLines lineGyroRawX;
K::GnuplotPlotElementLines lineGyroRawY;
K::GnuplotPlotElementLines lineGyroRawZ;
K::GnuplotPlot plotGyroFix;
K::GnuplotPlotElementLines lineGyroFixX;
K::GnuplotPlotElementLines lineGyroFixY;
K::GnuplotPlotElementLines lineGyroFixZ;
K::GnuplotPlot plotAcc;
K::GnuplotPlotElementLines lineAccX;
K::GnuplotPlotElementLines lineAccY;
K::GnuplotPlotElementLines lineAccZ;
K::GnuplotSplot plotPose;
K::GnuplotSplotElementLines linePose;
float plotCurHead = 0;
#endif
//std::vector<GyroscopeData> gyroData;
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -44,7 +80,40 @@ public:
/** ctor */
TurnDetection() {
;
#ifdef WITH_DEBUG_PLOT
gp1 << "set autoscale xfix\n";
gp1 << "set view equal xyz\n";
multiplot.add(&plotGyroRaw);
multiplot.add(&plotGyroFix);
multiplot.add(&plotAcc);
plotGyroRaw.setTitle("Gyroscope (raw)");
plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");
plotGyroFix.setTitle("Gyroscope (fixed)");
plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");
plotAcc.setTitle("Accelerometer");
plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");
plotPose.setTitle("Pose");
plotPose.getView().setEnabled(false);
plotPose.add(&linePose);
plotPose.getAxisX().setRange(-5,+5);
plotPose.getAxisY().setRange(-5,+5);
plotPose.getAxisZ().setRange(-5,+5);
#endif
}
@@ -90,7 +159,6 @@ public:
// otherwise we would use a wrong rotation matrix which yields wrong results!
if (!orientation.isKnown) {return 0.0f;}
// get the current gyro-reading as vector
Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
@@ -117,6 +185,77 @@ public:
// rotation = z-axis only!
const float delta = area(2);
#ifdef WITH_DEBUG_PLOT
plotCurHead += delta;
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
// raw gyro
lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );
// adjusted gyro
lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(0)) );
lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) );
lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) );
// adjusted gyro
lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) );
lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) );
lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) );
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
lastPlot = tsPlot;
// plot 3D pose
std::vector<Point3> pose = {
Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1),
Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1),
};
linePose.clear();
for (const Point3 p : pose) {
Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z;
Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1;
K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2));
linePose.add(gp3);
}
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineGyroRawX.removeIf(remove);
lineGyroRawY.removeIf(remove);
lineGyroRawZ.removeIf(remove);
lineGyroFixX.removeIf(remove);
lineGyroFixY.removeIf(remove);
lineGyroFixZ.removeIf(remove);
lineAccX.removeIf(remove);
lineAccY.removeIf(remove);
lineAccZ.removeIf(remove);
const float ax = 0.85 + std::cos(plotCurHead)*0.1;
const float ay = 0.85 + std::sin(plotCurHead)*0.1;
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.flush();
gp2.draw(plotPose);
gp2.flush();
//usleep(100);
}
#endif
// done
return delta;
@@ -128,17 +267,31 @@ public:
// add accelerometer data
//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
est.addAcc(acc);
est.addAcc(ts, acc);
// start with the first available timestamp
if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
if (1 == 0) {
// FASTER
// start with the first available timestamp
if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
orientation.rotationMatrix = est.get();
orientation.isKnown = true;
orientation.lastEstimation = ts;
est.reset();
}
} else {
// MORE ACCURATE
// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
orientation.rotationMatrix = est.get();
orientation.isKnown = true;
orientation.lastEstimation = ts;
est.reset();
}
}
@@ -207,7 +360,7 @@ public:
}
/** add the given accelerometer reading */
void addAcc(const AccelerometerData& acc) {
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
// did NOT improve the result for every smartphone (only some)
//const float deltaMag = std::abs(acc.magnitude() - 9.81);
@@ -218,6 +371,11 @@ public:
sum += vec;
++cnt;
}
AccelerometerData getAvg() const {
return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
}
/** get the current rotation matrix estimation */
@@ -245,9 +403,64 @@ public:
}
};
struct XYZ2 {
// average the accelerometer
MovingAverageTS<AccelerometerData> avg = MovingAverageTS<AccelerometerData>(Timestamp::fromMS(1250), AccelerometerData());
XYZ2() {
// start approximately
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
}
/** add the given accelerometer reading */
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
// did NOT improve the result for every smartphone (only some)
//const float deltaMag = std::abs(acc.magnitude() - 9.81);
//if (deltaMag > 5.0) {return;}
avg.add(ts, acc);
}
AccelerometerData getAvg() const {
return avg.get();
}
/** get the current rotation matrix estimation */
Eigen::Matrix3f get() const {
// get the current acceleromter average
AccelerometerData avgAcc = getAvg();
const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
// rotate average-accelerometer into (0,0,1)
Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
Eigen::Vector3f aligned = (rotMat * avg).normalized();
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
return rotMat;
}
/** reset the current sum etc. */
void reset() {
;
}
} est;
// struct RotationMatrixEstimationUsingAccAngle {
// Eigen::Vector3f lastAvg;

12
sensors/offline/FilePlayer.h
View File

@@ -83,6 +83,10 @@ namespace Offline {
// get all sensor events from the offline file
const std::vector<Entry> events = reader->getEntries();
// reference time (system vs. first-event)
Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
Timestamp tsRef2 = Timestamp::fromRunningTime();
// process every event
for (const Entry& e : events) {
@@ -92,6 +96,14 @@ namespace Offline {
// timestamp
const Timestamp ts = Timestamp::fromMS(e.ts);
// ensure events happen occur fast as they did during recording
if (realtime) {
const Timestamp ts1 = ts-tsRef1;
const Timestamp ts2 = Timestamp::fromRunningTime() - tsRef2;
const Timestamp diff = ts1-ts2;
if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
}
// event index
const size_t idx = e.idx;

63
synthetic/SyntheticSteps.h
View File

@@ -1,11 +1,16 @@
#ifndef SYNTHETICSTEPS_H
#define SYNTHETICSTEPS_H
#include "../sensors/imu/AccelerometerData.h"
#include "SyntheticWalker.h"
#include "../sensors/imu/AccelerometerData.h"
#include "../math/distribution/Normal.h"
/** fakes accelerometer-data based on synthetic walking data */
/**
* fakes accelerometer-data
* based on synthetic walking data
*/
class SyntheticSteps : SyntheticWalker::Listener {
public:
@@ -25,9 +30,14 @@ private:
float lastStepAtDistance = 0;
Distribution::Normal<float> dX = Distribution::Normal<float>(0, 1);
Distribution::Normal<float> dY = Distribution::Normal<float>(0, 1);
Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 1);
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;
@@ -35,9 +45,33 @@ 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);
}
@@ -47,15 +81,32 @@ 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();
AccelerometerData acc(x, y, z);
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);}

129
synthetic/SyntheticTurns.h
View File

@@ -1,4 +1,127 @@
#ifndef SYNTHETICTURNS_H
#define SYNTHETICTURNS_H
#ifndef INDOOR_SYNTHETICTURNS_H
#define INDOOR_SYNTHETICTURNS_H
#endif // SYNTHETICTURNS_H
#include "SyntheticWalker.h"
#include "../sensors/imu/AccelerometerData.h"
#include "../sensors/imu/GyroscopeData.h"
#include "../geo/Heading.h"
#include "../math/distribution/Normal.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::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
std::vector<Listener*> listeners;
public:
/** ctor with the walker to follow */
SyntheticTurns(SyntheticWalker* walker) {
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);
float change;
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 float diffRad = Heading::getSignedDiff(curHead, desiredHead);
// slowly change the current heading to match the desired one
const float maxChange = dMaxChange.draw();
const float toChange = clamp(diffRad, -maxChange, +maxChange);
//if (change < toChange) {change += toChange*0.01;}
if (change > toChange) {change *= 0.93;}
if (change < toChange) {change += dChange.draw()/10000;}
//if (change > toChange) {change -= dChange.draw();}
curHead += change;
// convert to gyro's radians-per-second
const float 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();
AccelerometerData acc(accX, accY, accZ);
const float gyroX = dGyroX.draw();
const float gyroY = dGyroY.draw();
const float gyroZ = dGyroZ.draw() + radPerSec;
GyroscopeData gyro(gyroX, gyroY, gyroZ);
for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}
}
};
#endif // INDOOR_SYNTHETICTURNS_H

4
synthetic/SyntheticWalker.h
View File

@@ -45,7 +45,7 @@ public:
}
/** increment the walk */
void tick(const Timestamp timePassed) {
Point3 tick(const Timestamp timePassed) {
// update time
this->walkedTime += timePassed;
@@ -61,6 +61,8 @@ public:
// inform listener
for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
return curPosOnPath;
}