diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h index ec5d73b..ec59386 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/grid/walk/v3/GridWalker3.h @@ -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& grid, const Nodes& nodes, Point2 pt) { + for (const Node* n : nodes) { + if (contains(grid, n, pt)) {return n;} + } + return nullptr; + } + const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { const GridPoint gpStart = p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); static Distribution::Normal dDist(1, 0.02); - static Distribution::Normal dHead(0, 0.02); + static Distribution::Normal 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.... diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index 4a1566f..55e69d4 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -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 << ")"; diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index b7eed18..68edfbb 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -8,12 +8,14 @@ #include #include -#include -#include -#include -#include -#include - +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include +#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 diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 67473ff..2ab0f6d 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.h @@ -5,18 +5,23 @@ #include "AccelerometerData.h" #include "../../data/Timestamp.h" #include "../../math/MovingAverageTS.h" +#include "../../geo/Point3.h" #include #include #include -#include -#include -#include -#include -#include - +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include + #include + #include +#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 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 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 avg = MovingAverageTS(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; diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h index 7d5c6e2..dcb88ba 100644 --- a/sensors/offline/FilePlayer.h +++ b/sensors/offline/FilePlayer.h @@ -83,6 +83,10 @@ namespace Offline { // get all sensor events from the offline file const std::vector 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; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index ddeae70..849907f 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -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 dX = Distribution::Normal(0, 1); - Distribution::Normal dY = Distribution::Normal(0, 1); - Distribution::Normal dZ = Distribution::Normal(0, 1); + Timestamp refStepPattern; + Interpolator stepPattern; + + Distribution::Normal dX = Distribution::Normal(0, 0.2); + Distribution::Normal dY = Distribution::Normal(0, 0.3); + Distribution::Normal dZ = Distribution::Normal(0, 0.4); + + int stepPatternPos = -1; std::vector 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);} diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index 8faa777..bec5bf7 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -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 dAccX = Distribution::Normal(0, 2.5); + Distribution::Normal dAccY = Distribution::Normal(0, 1.5); + Distribution::Normal dAccZ = Distribution::Normal(0, 1); + + Distribution::Normal dGyroX = Distribution::Normal(0, 0.02); + Distribution::Normal dGyroY = Distribution::Normal(0, 0.02); + Distribution::Normal dGyroZ = Distribution::Normal(0, 0.02); + + Distribution::Normal dMaxChange = Distribution::Normal(0.011, 0.003); + Distribution::Normal dChange = Distribution::Normal(1.0, 0.25); + Distribution::Normal dHeadErr = Distribution::Normal(0.15, 0.10); // heading error, slightly biased + + + std::vector 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 diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index a641871..2bb5c1e 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -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; + }