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;
}
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;
}
