From 63bc2f30462fd102ed2bfa7ecee935d392daadd6 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 29 Nov 2017 16:35:29 +0100 Subject: [PATCH] worked on grid-walker and synthetic steps/turns --- grid/walk/v3/Helper.h | 16 +- grid/walk/v3/Reachable.h | 491 ++++++++++++++++++++++--------------- grid/walk/v3/Walker.h | 52 +++- synthetic/SyntheticSteps.h | 20 +- synthetic/SyntheticTurns.h | 23 +- 5 files changed, 370 insertions(+), 232 deletions(-) diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index a47b3f6..c62e7b8 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -193,10 +193,22 @@ namespace GW3 { return bbox.contains(pt); } +// /** does one of the given grid-nodes contains the provided point-in-question? */ +// static 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; +// } + /** does one of the given grid-nodes contains the provided point-in-question? */ - static const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { + static const Node* contains(const Grid& grid, const std::vector& nodes, Point2 pt) { for (const Node* n : nodes) { - if (contains(grid, n, pt)) {return n;} + if (contains(grid, n, pt)) { + return n; + } } return nullptr; } diff --git a/grid/walk/v3/Reachable.h b/grid/walk/v3/Reachable.h index e353bcf..fdd3c9b 100644 --- a/grid/walk/v3/Reachable.h +++ b/grid/walk/v3/Reachable.h @@ -7,250 +7,333 @@ namespace GW3 { -#define likely(x) __builtin_expect((x),1) -#define unlikely(x) __builtin_expect((x),0) + #define likely(x) __builtin_expect((x),1) + #define unlikely(x) __builtin_expect((x),0) -/** - * get all grid nodes that are reachable within x-edges (depth) - */ -template class ReachableByDepthUnsorted { + /** + * get all grid nodes that are reachable within x-edges (depth) + */ + template class ReachableByDepthUnsorted { - struct VisitEntry { - const Node* gn; - int depth; - VisitEntry() {;} - VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} - }; + struct VisitEntry { + const Node* gn; + int depth; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} + }; - struct Visits { - VisitEntry visits[512];// __attribute__((aligned(16))); - size_t head = 0; - size_t tail = 0; - VisitEntry& getNext() { - return visits[tail++]; - } - void add(const VisitEntry& e) { - visits[head++] = e; - assert(head < 512); - //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! - } - bool hasMore() const { - return head > tail; - } - }; + struct Visits { + VisitEntry visits[512];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 512); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + }; - const Grid& grid; + const Grid& grid; -public: + public: - ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { - ; - } - - /** get all nodes reachable from start using maxDepth steps */ - std::unordered_set get(const Node& start, const int maxDepth) { - - std::unordered_set checked; - - // assuming max 8 neighbors per node, we need - // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) - // which is 1 + (1+2+3+4+5)*neighbors - // which is 1 + (n*n + n)/2*neighbors - // however this seems to be slow?! - //const int n = maxDepth + 1; - //const int maxEntries = (n * n + n) / 2 * 10 + 1; - //const int toAlloc = 4096 / sizeof(VisitEntry); - //if ( unlikely(toAlloc < maxEntries) ) {return checked;} - //if (maxDepth > 9) {throw Exception("will not fit!");} - - Visits toVisit; - - // directly start with the node itself and all its neighbors - checked.insert(&start); - for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { - const int nIdx = start.getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - checked.insert(&gnNext); - toVisit.add(VisitEntry(&gnNext, 1)); + ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { + ; } - // check all to-be-visited nodes - while ( likely(toVisit.hasMore()) ) { + /** get all nodes reachable from start using maxDepth steps */ + std::unordered_set get(const Node& start, const int maxDepth) { - const VisitEntry& e = toVisit.getNext(); + std::unordered_set checked; - if ( likely(e.depth <= maxDepth) ) { + // assuming max 8 neighbors per node, we need + // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) + // which is 1 + (1+2+3+4+5)*neighbors + // which is 1 + (n*n + n)/2*neighbors + // however this seems to be slow?! + //const int n = maxDepth + 1; + //const int maxEntries = (n * n + n) / 2 * 10 + 1; + //const int toAlloc = 4096 / sizeof(VisitEntry); + //if ( unlikely(toAlloc < maxEntries) ) {return checked;} + //if (maxDepth > 9) {throw Exception("will not fit!");} - const Node* gnCur = e.gn; - for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { - const int nIdx = gnCur->getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - if ( unlikely(checked.find(&gnNext) == checked.end()) ) { - toVisit.add(VisitEntry(&gnNext, e.depth+1)); - checked.insert(&gnNext); + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + checked.insert(&gnNext); + toVisit.add(VisitEntry(&gnNext, 1)); + } + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + toVisit.add(VisitEntry(&gnNext, e.depth+1)); + checked.insert(&gnNext); + } } + } } - } + return checked; - return checked; - - } - -}; - - -/** - * get all grid nodes that are reachable within x-edges (depth) - * additionally returns the needed walking distance in meter - */ -template class ReachableByDepthWithDistanceSorted { - - struct VisitEntry { - const Node* gn; - int depth; - float dist_m; - int myIdx; - VisitEntry() {;} - VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : - gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} - }; - - struct Visits { - VisitEntry visits[1024];// __attribute__((aligned(16))); - size_t head = 0; - size_t tail = 0; - VisitEntry& getNext() { - return visits[tail++]; - } - void add(const VisitEntry& e) { - visits[head++] = e; - assert(head < 1024); - //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! - } - bool hasMore() const { - return head > tail; - } - void sort() { - const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { - return e1.dist_m < e2.dist_m; - }; - std::sort(&visits[tail], &visits[head], comp); - } - }; - - const Grid& grid; - -public: - - /** result */ - struct Entry { - - const Node* node; - const float walkDistToStart_m; - const int prevIdx; - - Entry(const Node* node, const float dist, const size_t prevIdx) : - node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} - - bool hasPrev() const { - return prevIdx >= 0; } }; - ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { - ; - } - /** get all nodes reachable from start using maxDepth steps */ - std::vector get(const Node& start, const int maxDepth) { + /** + * get all grid nodes that are reachable within x-edges (depth) + * additionally returns the needed walking distance in meter + */ + template class ReachableByDepthWithDistanceSorted { - std::unordered_set checked; - std::vector res; + struct VisitEntry { + const Node* gn; + int depth; + float dist_m; + int myIdx; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : + gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} + }; - Visits toVisit; - - // directly start with the node itself and all its neighbors - checked.insert(&start); - res.push_back(Entry(&start, 0, -1)); - for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { - const int nIdx = start.getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - const float dist_m = gnNext.getDistanceInMeter(start); - toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); - res.push_back(Entry(&gnNext, dist_m, 0)); - checked.insert(&gnNext); - } - toVisit.sort(); - - // check all to-be-visited nodes - while ( likely(toVisit.hasMore()) ) { - - const VisitEntry& e = toVisit.getNext(); - - if ( likely(e.depth <= maxDepth) ) { - - const Node* gnCur = e.gn; - -// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { -// const int nIdx = gnCur->getNeighborIdx(i); -// const Node& gnNext = grid[nIdx]; -// if ( unlikely(checked.find(&gnNext) == checked.end()) ) { -// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); -// const float dist_m = e.dist_m + nodeNodeDist_m; -// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); -// res.push_back(Entry(&gnNext, dist_m, e.myIdx)); -// checked.insert(&gnNext); -// } -// } - -// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; - - std::vector sub; - - for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { - const int nIdx = gnCur->getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - if ( unlikely(checked.find(&gnNext) == checked.end()) ) { - const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); - const float dist_m = e.dist_m + nodeNodeDist_m; - //toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); - sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); - res.push_back(Entry(&gnNext, dist_m, e.myIdx)); - checked.insert(&gnNext); - - } - } - - // dijkstra.. sort the new nodes by destination to start - // only sorting the 8 new nodes seems enough due to the graph's layout + struct Visits { + VisitEntry visits[1024];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 1024); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + void sort() { const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { return e1.dist_m < e2.dist_m; }; + std::sort(&visits[tail], &visits[head], comp); + } + }; - std::sort(sub.begin(), sub.end(), comp); + const Grid& grid; + + public: + + /** result */ + struct Entry { + + const Node* node; + const float walkDistToStart_m; + const int prevIdx; + + Entry(const Node* node, const float dist, const size_t prevIdx) : + node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} + + bool hasPrev() const { + return prevIdx >= 0; + } + + }; + + ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::vector get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + std::vector res; + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + res.push_back(Entry(&start, 0, -1)); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + const float dist_m = gnNext.getDistanceInMeter(start); + toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, 0)); + checked.insert(&gnNext); + } + toVisit.sort(); + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + + // for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + // const int nIdx = gnCur->getNeighborIdx(i); + // const Node& gnNext = grid[nIdx]; + // if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + // const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); + // const float dist_m = e.dist_m + nodeNodeDist_m; + // toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + // res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + // checked.insert(&gnNext); + // } + // } + + // const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; + + std::vector sub; + + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); + const float dist_m = e.dist_m + nodeNodeDist_m; + //toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + checked.insert(&gnNext); + + } + } + + // dijkstra.. sort the new nodes by destination to start + // only sorting the 8 new nodes seems enough due to the graph's layout + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + + std::sort(sub.begin(), sub.end(), comp); + + for (const VisitEntry& e : sub) { + toVisit.add(e); + } + + } + + // slower with same result ;) + //toVisit.sort(); + + } + + return res; + + } + + }; + + + /** + * data-structure to track to-be-visited nodes + * push_back, pop_front + * as pop_front is costly, we omit the pop and use a head-index instead + * memory-consumption vs speed + */ + struct _ToVisit { + size_t nextIdx = 0; + std::vector vec; + _ToVisit() {vec.reserve(256);} + void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);} + uint32_t next() {return vec[nextIdx++];} + bool empty() const {return nextIdx >= vec.size();} + }; + + + + /** get a list of all nodes that are reachable after checking several conditions */ + template class ReachableByConditionUnsorted { + + + public: + + + static std::vector get(const Grid& grid, const Node& start, const Conditions cond) { + + //Node* curNode = nullptr; + std::unordered_set scheduled; + _ToVisit toVisit; + toVisit.add(start.getIdx()); + + std::vector res; + + while(!toVisit.empty()) { + + // get the next to-be-visited node + const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance) + const Node& curNode = grid[curIdx]; + + // process current node + res.push_back(&curNode); + scheduled.insert(curIdx); + + // get all neighbors + const int numNeighbors = curNode.getNumNeighbors(); + for (int i = 0; i < numNeighbors; ++i) { + + const uint32_t neighborIdx = curNode.getNeighborIdx(i); + + // already visited? + if (scheduled.find(neighborIdx) != scheduled.end()) {continue;} + scheduled.insert(neighborIdx); + + // matches the used condition? + const Node& neighbor = grid[neighborIdx]; + if (!cond.visit(neighbor)) {continue;} + + // OK! + toVisit.add(neighborIdx); - for (const VisitEntry& e : sub) { - toVisit.add(e); } } - // slower with same result ;) - //toVisit.sort(); + // done + return res; } - return res; - } -}; + //const Node& next(const std::function& skip) { + //template const Node& next(const Skip skip) { + const Node& next() { + + + + } + + }; + } diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index c666042..c1a29f2 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -65,35 +65,63 @@ namespace GW3 { const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); + // calculate a walk's probability + auto getP = [&] (const Point3 dst) { + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params); + p *= p1; + } + return p; + }; + // include one additional grid-cell (increased distance) - const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1); - ReachableSettings set; - set.limitDistance = true; - set.dist_m = params.distance_m + secBuffer_m; - set.limitHeading = false; - set.heading = params.heading; - set.maxHeadingDiff_rad = M_PI/2; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); + //const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1); + const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15); + +// ReachableSettings set; +// set.limitDistance = true; +// set.dist_m = params.distance_m + secBuffer_m; +// set.limitHeading = false; +// set.heading = params.heading; +// set.maxHeadingDiff_rad = M_PI/2; + +// // get all nodes that satisfy above constraints +// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); + + struct Cond { + const float maxDist_m; + const Node* startNode; + Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;} + bool visit(const Node& n) const { + return (startNode->getDistanceInMeter(n)) < maxDist_m; + } + }; + Cond cond(params.distance_m+secBuffer_m, startNode); + std::vector reachableNodes = ReachableByConditionUnsorted::get(grid, *startNode, cond); WalkResult res; res.heading = params.heading; res.position = params.start; - + // get the to-be-reached destination's position (using start+distance+heading) const Point2 dir = res.heading.asVector(); const Point2 dst = params.start.xy() + (dir * params.distance_m); - // is dst reachable? + // is above destination reachable? const Node* n = Helper::contains(grid, reachableNodes, dst); + //const Node* n = ri.contains(dst); if (n) { const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); const GridPoint gp = Helper::p3ToGp(p3); + + if (grid.hasNodeFor(gp)) { res.position = p3; // update position //res.heading; // keep as-is - //res.probability; // keep as-is + res.probability *= getP(p3); // keep as-is return res; // done } else { @@ -129,7 +157,7 @@ namespace GW3 { } res.heading = Heading(start.xy(), end.xy()); - res.probability = p; + res.probability *= getP(end); res.position = end; return res; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index 849907f..7bfb885 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -37,14 +37,22 @@ private: Distribution::Normal dY = Distribution::Normal(0, 0.3); Distribution::Normal dZ = Distribution::Normal(0, 0.4); + int stepPatternPos = -1; std::vector listeners; + //float stepSize_m; + //float stepSizeSigma_m; + float noiseLevel; + Distribution::Normal 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; } } diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index f94db0c..d9d8ef8 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -39,17 +39,23 @@ private: 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 Distribution::Uniform dRadDiff = Distribution::Uniform(40,100); + //float headingDrift_rad; + //float headingSigma_rad; + float noiseLevel; + Distribution::Normal dHeadErr; std::vector 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);}