  private InferredPath traverseEdge(
    EdgeTransitionDistributions edgeTransDist,
    final MultivariateGaussian belief, PathEdge startEdge,
    StandardRoadTrackingFilter movementFilter) {
    rng.setSeed(this.localSeed);
    PathEdge currentEdge = startEdge;
    PathEdge previousEdge = null;
    final MultivariateGaussian newBelief = belief.clone();
    final List<PathEdge> currentPath = Lists.newArrayList();
    double distTraveled = 0d;
    Double totalDistToTravel = null;
    while (totalDistToTravel == null ||
        Math.abs(totalDistToTravel) > Math.abs(distTraveled)) {
      final List<InferredEdge> transferEdges = Lists.newArrayList();
      if (currentEdge.getInferredEdge() == InferredEdge
          .getEmptyEdge()) {
        final Vector projLocation =
            StandardRoadTrackingFilter.getOg().times(
                newBelief.getMean());
        for (final StreetEdge edge : this.inferredGraph
            .getNearbyEdges(projLocation,
                movementFilter.getObservationErrorAbsRadius())) {
          transferEdges.add(this.inferredGraph.getInferredEdge(edge));
        }
      } else {
        if (totalDistToTravel == null) {
          transferEdges.add(startEdge.getInferredEdge());
        } else {
          if (newBelief.getMean().getElement(0) < 0d) {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getIncomingTransferableEdges());
          } else if (newBelief.getMean().getElement(0) > 0d) {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getOutgoingTransferableEdges());
          } else {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getIncomingTransferableEdges());
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getOutgoingTransferableEdges());
          }
          transferEdges.remove(currentEdge.getInferredEdge());
        }
      }
      final InferredEdge sampledEdge =
          edgeTransDist.sample(rng, transferEdges,
              currentEdge == null ? startEdge.getInferredEdge()
                  : currentEdge.getInferredEdge());
      if (sampledEdge == InferredEdge.getEmptyEdge()) {
        if (totalDistToTravel == null) {
          movementFilter.predict(newBelief,
              PathEdge.getEmptyPathEdge(), startEdge);
        } else {
          StandardRoadTrackingFilter.convertToGroundBelief(newBelief,
              currentEdge, true);
        }
        currentEdge = PathEdge.getEmptyPathEdge();
        currentPath.add(PathEdge.getEmptyPathEdge());
        break;
      }
      double direction =
          newBelief.getMean().getElement(0) >= 0d ? 1d : -1d;
      final PathEdge sampledPathEdge =
          PathEdge.getEdge(sampledEdge, previousEdge == null
              || previousEdge.isEmptyEdge() ? 0d : direction
              * previousEdge.getInferredEdge().getLength()
              + previousEdge.getDistToStartOfEdge());
      if (sampledPathEdge == null) {
        newBelief.getMean().setElement(0,
            direction * currentEdge.getInferredEdge().getLength());
        newBelief.getMean().setElement(1, 0d);
        break;
      }
      if (totalDistToTravel == null) {
        final PathEdge initialEdge =
            startEdge.isEmptyEdge() ? sampledPathEdge : startEdge;
        currentEdge = initialEdge;
        if (newBelief.getInputDimensionality() == 4) {
          StandardRoadTrackingFilter.convertToRoadBelief(newBelief,
              InferredPath.getInferredPath(initialEdge));
        }
        double previousLocation = newBelief.getMean().getElement(0);
        movementFilter.predict(newBelief, initialEdge, initialEdge);
        final Vector transStateSample =
            StandardRoadTrackingFilter.sampleMovementBelief(rng,
                newBelief.getMean(), movementFilter);
        newBelief.setMean(transStateSample);
        totalDistToTravel =
            newBelief.getMean().getElement(0) - previousLocation;
        double newLocation = newBelief.getMean().getElement(0);
        final double L = initialEdge.getInferredEdge().getLength();
        if (newLocation < 0d && previousLocation > 0d) {
          previousLocation = -L + previousLocation;
          newLocation = previousLocation + totalDistToTravel;
          newBelief.getMean().setElement(0, newLocation);
        } else if (newLocation >= 0d && previousLocation < 0d) {
          previousLocation = L + previousLocation;
          newLocation = previousLocation + totalDistToTravel;
          newBelief.getMean().setElement(0, newLocation);
        }
        direction = totalDistToTravel >= 0d ? 1d : -1d;
        if (L < Math.abs(newLocation)) {
          final double r = L - Math.abs(previousLocation);
          distTraveled += r * direction;
        } else {
          distTraveled += totalDistToTravel;
        }
      } else {
        distTraveled +=
            direction * sampledPathEdge.getInferredEdge().getLength();
        currentEdge = sampledPathEdge;
      }
      previousEdge = currentEdge;
      currentPath.add(currentEdge);
    }
    if(!Iterables.getLast(currentPath).isEmptyEdge() && 
          !Iterables.getLast(currentPath).isOnEdge(newBelief.getMean().getElement(0))) {
      Iterables.getLast(currentPath).isOnEdge(newBelief.getMean().getElement(0));
    }
    assert (Iterables.getLast(currentPath).isEmptyEdge() || Iterables
        .getLast(currentPath).isOnEdge(
            newBelief.getMean().getElement(0)));
    belief.setMean(newBelief.getMean());
    belief.setCovariance(newBelief.getCovariance());
    return InferredPath.getInferredPath(currentPath,
        (totalDistToTravel != null && totalDistToTravel < 0d) ? true
            : false);
  }

  private InferredPath traverseEdge(
    EdgeTransitionDistributions edgeTransDist,
    final MultivariateGaussian belief, PathEdge startEdge,
    StandardRoadTrackingFilter movementFilter) {
    rng.setSeed(this.localSeed);
    PathEdge currentEdge = startEdge;
    PathEdge previousEdge = null;
    final MultivariateGaussian newBelief = belief.clone();
    final List<PathEdge> currentPath = Lists.newArrayList();
    double distTraveled = 0d;
    Double totalDistToTravel = null;
    while (totalDistToTravel == null ||
        Math.abs(totalDistToTravel) > Math.abs(distTraveled)) {
      final List<InferredEdge> transferEdges = Lists.newArrayList();
      if (currentEdge.getInferredEdge() == InferredEdge
          .getEmptyEdge()) {
        final Vector projLocation =
            StandardRoadTrackingFilter.getOg().times(
                newBelief.getMean());
        for (final StreetEdge edge : this.inferredGraph
            .getNearbyEdges(projLocation,
                movementFilter.getObservationErrorAbsRadius())) {
          transferEdges.add(this.inferredGraph.getInferredEdge(edge));
        }
      } else {
        if (totalDistToTravel == null) {
          transferEdges.add(startEdge.getInferredEdge());
        } else {
          if (newBelief.getMean().getElement(0) < 0d) {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getIncomingTransferableEdges());
          } else if (newBelief.getMean().getElement(0) > 0d) {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getOutgoingTransferableEdges());
          } else {
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getIncomingTransferableEdges());
            transferEdges.addAll(currentEdge.getInferredEdge()
                .getOutgoingTransferableEdges());
          }
          transferEdges.remove(currentEdge.getInferredEdge());
        }
      }
      final InferredEdge sampledEdge =
          edgeTransDist.sample(rng, transferEdges,
              currentEdge == null ? startEdge.getInferredEdge()
                  : currentEdge.getInferredEdge());
      if (sampledEdge == InferredEdge.getEmptyEdge()) {
        if (totalDistToTravel == null) {
          movementFilter.predict(newBelief,
              PathEdge.getEmptyPathEdge(), startEdge);
        } else {
          StandardRoadTrackingFilter.convertToGroundBelief(newBelief,
              currentEdge, true);
        }
        currentEdge = PathEdge.getEmptyPathEdge();
        currentPath.add(PathEdge.getEmptyPathEdge());
        break;
      }
      double direction =
          newBelief.getMean().getElement(0) >= 0d ? 1d : -1d;
      final PathEdge sampledPathEdge =
          PathEdge.getEdge(sampledEdge, previousEdge == null
              || previousEdge.isEmptyEdge() ? 0d : direction
              * previousEdge.getInferredEdge().getLength()
              + previousEdge.getDistToStartOfEdge(), direction < 0d);
      if (sampledPathEdge == null) {
        newBelief.getMean().setElement(0,
            direction * currentEdge.getInferredEdge().getLength());
        newBelief.getMean().setElement(1, 0d);
        break;
      }
      if (totalDistToTravel == null) {
        final PathEdge initialEdge =
            startEdge.isEmptyEdge() ? sampledPathEdge : startEdge;
        currentEdge = initialEdge;
        if (newBelief.getInputDimensionality() == 4) {
          StandardRoadTrackingFilter.convertToRoadBelief(newBelief,
              InferredPath.getInferredPath(initialEdge));
        }
        double previousLocation = newBelief.getMean().getElement(0);
        movementFilter.predict(newBelief, initialEdge, initialEdge);
        final Vector transStateSample =
            StandardRoadTrackingFilter.sampleMovementBelief(rng,
                newBelief.getMean(), movementFilter);
        newBelief.setMean(transStateSample);
        totalDistToTravel =
            newBelief.getMean().getElement(0) - previousLocation;
        double newLocation = newBelief.getMean().getElement(0);
        final double L = initialEdge.getInferredEdge().getLength();
        if (newLocation < 0d && previousLocation > 0d) {
          previousLocation = -L + previousLocation;
          newLocation = previousLocation + totalDistToTravel;
          newBelief.getMean().setElement(0, newLocation);
        } else if (newLocation >= 0d && previousLocation < 0d) {
          previousLocation = L + previousLocation;
          newLocation = previousLocation + totalDistToTravel;
          newBelief.getMean().setElement(0, newLocation);
        }
        direction = totalDistToTravel >= 0d ? 1d : -1d;
        if (L < Math.abs(newLocation)) {
          final double r = L - Math.abs(previousLocation);
          distTraveled += r * direction;
        } else {
          distTraveled += totalDistToTravel;
        }
      } else {
        distTraveled +=
            direction * sampledPathEdge.getInferredEdge().getLength();
        currentEdge = sampledPathEdge;
      }
      previousEdge = currentEdge;
      currentPath.add(currentEdge);
    }
    assert (Iterables.getLast(currentPath).isEmptyEdge() || Iterables
        .getLast(currentPath).isOnEdge(
            newBelief.getMean().getElement(0)));
    belief.setMean(newBelief.getMean());
    belief.setCovariance(newBelief.getCovariance());
    return InferredPath.getInferredPath(currentPath,
        (totalDistToTravel != null && totalDistToTravel < 0d) ? true
            : false);
  }
