コード例 #1
0
  private static double getAvgVelocityToNextStop(Observation obs, BlockState blockState) {

    StopTimeEntry stop = blockState.getBlockLocation().getNextStop().getStopTime();
    final double distToDest =
        TurboButton.distance(obs.getLocation(), stop.getStop().getStopLocation());
    final int currSchedTime =
        (int) (obs.getTime() - blockState.getBlockInstance().getServiceDate()) / 1000;

    final double expectedVelocity;
    if (currSchedTime >= stop.getArrivalTime()) {
      /*
       * Assumption here is that the driver will speed up to get to the stop on time.
       * TODO: check this.
       */
      expectedVelocity = _avgVelocity * 3d / 2d;
    } else {
      expectedVelocity = distToDest / (stop.getArrivalTime() - currSchedTime);
    }

    return expectedVelocity;
  }
コード例 #2
0
  private static final SensorModelResult computeEdgeMovementLogProb(
      Observation obs,
      VehicleState state,
      VehicleState parentState,
      boolean hasMoved,
      boolean isDuring)
      throws BadProbabilityParticleFilterException {

    final BlockState blockState = state.getBlockState();
    final BlockState parentBlockState = parentState.getBlockState();
    final double currentDab;
    final double prevDab;
    if (!blockState.getBlockInstance().equals(parentBlockState.getBlockInstance())) {
      /*
       * Note: this isn't really working like we want it to, since there are
       * separate shapes that share the same segments of geometry.
       */
      if (!Objects.equal(
          parentBlockState.getBlockLocation().getActiveTrip().getTrip().getShapeId(),
          blockState.getBlockLocation().getActiveTrip().getTrip().getShapeId())) {
        return computeNoEdgeMovementLogProb(state, parentState, obs);
      }
      currentDab =
          blockState.getBlockLocation().getDistanceAlongBlock()
              - blockState.getBlockLocation().getActiveTrip().getDistanceAlongBlock();
      prevDab =
          parentBlockState.getBlockLocation().getDistanceAlongBlock()
              - parentBlockState.getBlockLocation().getActiveTrip().getDistanceAlongBlock();
    } else {
      currentDab = blockState.getBlockLocation().getDistanceAlongBlock();
      prevDab = parentBlockState.getBlockLocation().getDistanceAlongBlock();
    }

    final SensorModelResult result = new SensorModelResult("edge-move");
    final double dabDelta = currentDab - prevDab;
    if (ParticleFilter.getDebugEnabled()) result.addResult("dist: " + dabDelta, 0d);

    final double obsTimeDelta = (obs.getTime() - obs.getPreviousObservation().getTime()) / 1000d;
    final double expAvgDist;
    if (hasMoved) {
      if (!isDuring || blockState.getBlockLocation().getNextStop() == null) {
        expAvgDist = getAvgVelocityBetweenStops(blockState) * obsTimeDelta;
      } else {
        expAvgDist = getAvgVelocityToNextStop(obs, blockState) * obsTimeDelta;
      }
    } else {
      expAvgDist = 0d;
    }
    if (ParticleFilter.getDebugEnabled()) result.addResult("expAvgDist: " + expAvgDist, 0d);

    final double x = dabDelta - expAvgDist;

    /*
     * Now, we need to measure the state transition likelihood
     * wrt. heading.
     * For the in-progress -> in-progress transition, we need
     * to calculate the expected turn-rate/orientation between the two states.
     */
    double obsOrientation = Math.toRadians(obs.getOrientation());
    if (Double.isNaN(obsOrientation)) {
      obsOrientation = Math.toRadians(blockState.getBlockLocation().getOrientation());
    }
    if (ParticleFilter.getDebugEnabled()) result.addResult("obsOrient:" + obsOrientation, 0d);

    final double orientDiff;
    final double logpOrient;
    if (hasMoved) {
      if (!isDuring || blockState.getBlockLocation().getNextStop() == null) {
        /*
         * TODO
         * We could use a polar velocity model and the turn rate...
         */
        final double prevOrientation =
            Math.toRadians(parentBlockState.getBlockLocation().getOrientation());
        final double currentOrientation =
            Math.toRadians(blockState.getBlockLocation().getOrientation());
        final double angleDiff = Angle.diff(prevOrientation, currentOrientation);
        final double avgExpOrientation =
            Angle.normalizePositive(
                prevOrientation
                    + (Angle.getTurn(prevOrientation, currentOrientation) == Angle.CLOCKWISE
                            ? -1d
                            : 1d)
                        * angleDiff
                        / 2d);
        orientDiff = Angle.diff(obsOrientation, avgExpOrientation);
        if (ParticleFilter.getDebugEnabled())
          result.addResult("expOrient: " + avgExpOrientation, 0d);
        logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _inProgressConcParam);
      } else {
        /*
         * The orientation is wrt. the next stop destination, i.e.
         * we want to check that we're heading toward it.
         */
        double pathOrDestOrientation =
            Math.toRadians(
                SphericalGeometryLibrary.getOrientation(
                    obs.getLocation().getLat(),
                    obs.getLocation().getLon(),
                    blockState
                        .getBlockLocation()
                        .getNextStop()
                        .getStopTime()
                        .getStop()
                        .getStopLocation()
                        .getLat(),
                    blockState
                        .getBlockLocation()
                        .getNextStop()
                        .getStopTime()
                        .getStop()
                        .getStopLocation()
                        .getLon()));
        if (Double.isNaN(pathOrDestOrientation)) {
          pathOrDestOrientation = obsOrientation;
        }
        orientDiff = Angle.diff(obsOrientation, pathOrDestOrientation);
        if (ParticleFilter.getDebugEnabled())
          result.addResult("expOrient: " + pathOrDestOrientation, 0d);
        logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _deadheadEntranceConcParam);
      }
    } else {
      //      /*
      //       * What if we're switching trips while in the middle of doing one?
      //       * We assume that it's unlikely to do a complete 180 when we're
      //       * stopped.
      //       */
      //      if (parentState.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS
      //          && state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
      //        orientDiff = Math.toRadians(blockState.getBlockLocation().getOrientation()
      //            - parentBlockState.getBlockLocation().getOrientation());
      //      } else {
      orientDiff =
          Angle.diff(
              obsOrientation, Math.toRadians(blockState.getBlockLocation().getOrientation()));
      //      }
      if (ParticleFilter.getDebugEnabled())
        result.addResult("orientDiff (stopped):" + orientDiff, 0d);
      logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _stoppedConcParam);
    }

    final double logpMove =
        UnivariateGaussian.PDF.logEvaluate(x, 0d, Math.pow(obsTimeDelta, 4) / 4d) + logpOrient;

    result.addLogResultAsAnd("lik", logpMove);

    return result;
  }