Program: 189434
Project: 189434
Buggy source:
/** 
 * Returns the predictive distribution for the given edge.
 * @param roadDistribution
 * @param edge
 * @return
 */
public static TruncatedRoadGaussian getPathEdgePredictive(MultivariateGaussian roadDistribution,PathEdge edge,Coordinate obs,Double startDistance,double deltaTime){
  final Matrix Or=MotionStateEstimatorPredictor.getOr();
  double edgeLength;
  final double distToStartOfEdge;
  LineSegment lineSegment;
  if (startDistance != null && edge.getDistToStartOfEdge() == 0) {
    edgeLength=edge.getLength() - startDistance;
    if (edgeLength < 10d) {
      distToStartOfEdge=Math.max(startDistance - 10d,10d);
      edgeLength=edge.getLength() - distToStartOfEdge;
    }
 else {
      distToStartOfEdge=startDistance;
    }
    lineSegment=new LineSegment(edge.getInferenceGraphSegment().getLine().pointAlong(distToStartOfEdge / edge.getLength()),edge.getInferenceGraphSegment().getLine().p1);
  }
 else {
    edgeLength=edge.getLength();
    distToStartOfEdge=edge.getDistToStartOfEdge();
    lineSegment=edge.getInferenceGraphSegment().getLine();
  }
  final double S=Or.times(roadDistribution.getCovariance()).times(Or.transpose()).getElement(0,0) + Math.pow(edgeLength / Math.sqrt(12),2);
  final Matrix W=roadDistribution.getCovariance().times(Or.transpose()).scale(1 / S);
  final Matrix R=roadDistribution.getCovariance().minus(W.times(W.transpose()).scale(S));
  final double mean;
  if (edgeLength < 1e-5) {
    mean=distToStartOfEdge;
  }
 else {
    mean=lineSegment.segmentFraction(obs) * edgeLength + distToStartOfEdge;
  }
  final Vector beliefMean=roadDistribution.getMean();
  final double e=mean - Or.times(beliefMean).getElement(0);
  W.setElement(0,0,1d);
  W.setElement(1,0,1d / deltaTime);
  final Vector a=beliefMean.plus(W.getColumn(0).scale(e));
  if (a.getElement(1) < 0d) {
    a.setElement(1,0d);
  }
  final double afterEndDist=-Math.min(distToStartOfEdge + edgeLength - a.getElement(0),0d);
  final double beforeBeginDist=-Math.min(a.getElement(0) - distToStartOfEdge,0d);
  if (afterEndDist > 0d) {
    if (afterEndDist > 1d) {
      return null;
    }
 else {
      a.setElement(0,distToStartOfEdge + edgeLength - 1e-5);
    }
  }
 else   if (beforeBeginDist > 0d) {
    if (beforeBeginDist > 1d) {
      return null;
    }
 else {
      a.setElement(0,distToStartOfEdge + 1e-5);
    }
  }
  SvdMatrix covarResult=new SvdMatrix(R);
  final AbstractSingularValueDecomposition svd=covarResult.getSvd();
  covarResult=new SvdMatrix(new SimpleSingularValueDecomposition(svd.getU(),svd.getS(),svd.getU().transpose()));
  final TruncatedRoadGaussian result=new TruncatedRoadGaussian(a,covarResult);
  return result;
}

Fixed version (ground truth):
/** 
 * Returns the predictive distribution for the given edge.
 * @param roadDistribution
 * @param edge
 * @return
 */
public static TruncatedRoadGaussian getPathEdgePredictive(MultivariateGaussian roadDistribution,Matrix measurementError,PathEdge edge,Coordinate obs,Double startDistance,double deltaTime){
  final Matrix Or=MotionStateEstimatorPredictor.getOr();
  double edgeLength;
  final double distToStartOfEdge;
  LineSegment lineSegment;
  if (startDistance != null && edge.getDistToStartOfEdge() == 0) {
    edgeLength=edge.getLength() - startDistance;
    if (edgeLength < 10d) {
      distToStartOfEdge=Math.max(startDistance - 10d,10d);
      edgeLength=edge.getLength() - distToStartOfEdge;
    }
 else {
      distToStartOfEdge=startDistance;
    }
    lineSegment=new LineSegment(edge.getInferenceGraphSegment().getLine().pointAlong(distToStartOfEdge / edge.getLength()),edge.getInferenceGraphSegment().getLine().p1);
  }
 else {
    edgeLength=edge.getLength();
    distToStartOfEdge=edge.getDistToStartOfEdge();
    lineSegment=edge.getInferenceGraphSegment().getLine();
  }
  final double S=Or.times(measurementError).times(Or.transpose()).getElement(0,0) + Math.pow(edgeLength / Math.sqrt(12),2);
  final Matrix W=roadDistribution.getCovariance().times(Or.transpose()).scale(1 / S);
  final Matrix R=roadDistribution.getCovariance().minus(W.times(W.transpose()).scale(S));
  final double mean;
  if (edgeLength < 1e-5) {
    mean=distToStartOfEdge;
  }
 else {
    mean=lineSegment.segmentFraction(obs) * edgeLength + distToStartOfEdge;
  }
  final Vector beliefMean=roadDistribution.getMean();
  final double e=mean - Or.times(beliefMean).getElement(0);
  W.setElement(0,0,1d);
  W.setElement(1,0,1d / deltaTime);
  final Vector a=beliefMean.plus(W.getColumn(0).scale(e));
  if (a.getElement(1) < 0d) {
    a.setElement(1,0d);
  }
  final double afterEndDist=-Math.min(distToStartOfEdge + edgeLength - a.getElement(0),0d);
  final double beforeBeginDist=-Math.min(a.getElement(0) - distToStartOfEdge,0d);
  if (afterEndDist > 0d) {
    if (afterEndDist > 1d) {
      return null;
    }
 else {
      a.setElement(0,distToStartOfEdge + edgeLength - 1e-5);
    }
  }
 else   if (beforeBeginDist > 0d) {
    if (beforeBeginDist > 1d) {
      return null;
    }
 else {
      a.setElement(0,distToStartOfEdge + 1e-5);
    }
  }
  SvdMatrix covarResult=new SvdMatrix(R);
  final AbstractSingularValueDecomposition svd=covarResult.getSvd();
  covarResult=new SvdMatrix(new SimpleSingularValueDecomposition(svd.getU(),svd.getS(),svd.getU().transpose()));
  final TruncatedRoadGaussian result=new TruncatedRoadGaussian(a,covarResult);
  return result;
}