private static final SensorModelResult computeNoEdgeMovementLogProb(
VehicleState state, VehicleState prevState, Observation obs)
throws BadProbabilityParticleFilterException {
final SensorModelResult result = new SensorModelResult("no-edge");
final Observation prevObs = obs.getPreviousObservation();
final double obsDistDelta = obs.getDistanceMoved();
if (ParticleFilter.getDebugEnabled()) result.addResult("dist: " + obsDistDelta, 0d);
final double expAvgDist;
final double logpDistAlong;
if (prevObs != null) {
/*
* Trying to get away with not using a real tracking filter.
* FIXME really lame. use a Kalman filter.
*/
final double obsTimeDelta = obs.getTimeDelta();
if (prevObs.getTimeDelta() != null) {
final double prevObsDistDelta = prevObs.getDistanceMoved();
final double prevObsTimeDelta = prevObs.getTimeDelta();
final double velocityEstimate = prevObsDistDelta / prevObsTimeDelta;
expAvgDist =
state.getMotionState().hasVehicleNotMoved() ? 0d : velocityEstimate * obsTimeDelta;
} else {
expAvgDist = state.getMotionState().hasVehicleNotMoved() ? 0d : _avgVelocity * obsTimeDelta;
}
if (ParticleFilter.getDebugEnabled()) result.addResult("expAvgDist: " + expAvgDist, 0d);
/*
* TODO: Is this needed?
*/
final boolean prevInProgress;
double prevOrientation;
if (prevState != null
&& prevState.getBlockState() != null
&& prevState.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
prevOrientation = prevState.getBlockState().getBlockLocation().getOrientation();
prevInProgress = true;
if (ParticleFilter.getDebugEnabled())
result.addResult("prevOrient (state): " + prevOrientation, 0d);
} else {
prevOrientation = prevObs.getOrientation();
prevInProgress = false;
if (ParticleFilter.getDebugEnabled())
result.addResult("prevOrient: " + prevOrientation, 0d);
}
final boolean isOff;
final double currentOrientation;
if (state.getBlockState() != null
&& state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
/*
* We're entering an edge in progress, so we want to compare the
* entrance orientation to the direction of the edge
*/
isOff = false;
currentOrientation = state.getBlockState().getBlockLocation().getOrientation();
/*
* We don't want to use the obs orientation when we're considered stopped
* since gps error can reverse the orientation, making reverse
* trip transitions look reasonable.
* Similarly, if we were on a trip, use that orientation.
* TODO FIXME: we should have the orientation in the motion state, then
* we wouldn't need to evaluate this logic here.
*/
if (!state.getMotionState().hasVehicleNotMoved()) {
if (!prevInProgress) {
prevOrientation = obs.getOrientation();
}
if (ParticleFilter.getDebugEnabled())
result.addResult("new prevOrient (state): " + prevOrientation, 0d);
}
} else {
isOff = true;
currentOrientation = obs.getOrientation();
}
if (ParticleFilter.getDebugEnabled())
result.addResult("curOrient: " + currentOrientation, 0d);
final double orientDiff;
if (Double.isNaN(prevOrientation) || Double.isNaN(currentOrientation)) {
orientDiff = 0d;
} else {
orientDiff =
Angle.diff(Math.toRadians(prevOrientation), Math.toRadians(currentOrientation));
}
logpDistAlong =
UnivariateGaussian.PDF.logEvaluate(
obsDistDelta, expAvgDist, Math.pow(obsTimeDelta, 4) / 4d)
+ (isOff
? _deadheadOffProb * logVonMisesPdf(orientDiff, _deadheadConcParam)
: _inProgressProb * logVonMisesPdf(orientDiff, _deadheadEntranceConcParam));
} else {
/*
* No movement
*/
logpDistAlong = 1d;
}
result.addLogResultAsAnd("lik", logpDistAlong);
return result;
}
