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