Program: 189429
Project: 189429
Buggy source:
/** 
 * This method performs the Bayes update for a single vehicle state.
 * @param state
 * @param obs
 * @return
 */
protected VehicleStateDistribution<O> internalUpdate(VehicleStateDistribution<O> state,O obs){
  final VehicleStateDistribution<O> updatedState=state.clone();
  final PathStateDistribution priorPredictivePathStateDist=updatedState.getPathStateParam().getParameterPrior().clone();
  final MultivariateGaussian updatedMotionState=priorPredictivePathStateDist.getMotionDistribution().clone();
  final PathStateDistribution posteriorPathStateDist;
  if (priorPredictivePathStateDist.getPathState().isOnRoad()) {
    final TruncatedRoadKalmanFilter roadFilter=updatedState.getMotionStateEstimatorPredictor().getRoadFilter().clone();
    final AdjMultivariateGaussian obsProj=PathUtils.getRoadObservation(obs.getProjectedPoint(),updatedState.getObservationCovarianceParam().getValue(),priorPredictivePathStateDist.getPathState().getPath(),priorPredictivePathStateDist.getPathState().getEdge());
    roadFilter.setMeasurementCovariance(obsProj.getCovariance());
    roadFilter.measure(updatedMotionState,obsProj.getMean());
    Preconditions.checkState(updatedMotionState.getCovariance().getElement(0,0) > 1e-6);
    posteriorPathStateDist=new PathStateDistribution(priorPredictivePathStateDist.getPathState().getPath().getPathTo(updatedMotionState.getMean().getElement(0)),updatedMotionState);
  }
 else {
    updatedState.getMotionStateEstimatorPredictor().update(updatedMotionState,obs.getProjectedPoint());
    posteriorPathStateDist=new PathStateDistribution(priorPredictivePathStateDist.getPathState().getPath(),updatedMotionState);
  }
  updatedState.getPathStateParam().setValue(posteriorPathStateDist.getPathState());
  updatedState.getPathStateParam().setParameterPrior(posteriorPathStateDist);
  Vector newObsStateSample;
  final RoadModelCovarianceEstimatorPredictor modelCovarianceEstimator=new RoadModelCovarianceEstimatorPredictor(updatedState,state.getMotionStateEstimatorPredictor(),this.random);
  final ScaledInverseGammaCovDistribution currentModelCovDistribution=(ScaledInverseGammaCovDistribution)(posteriorPathStateDist.getPathState().isOnRoad() ? updatedState.getOnRoadModelCovarianceParam().getParameterPrior().clone() : updatedState.getOffRoadModelCovarianceParam().getParameterPrior().clone());
  modelCovarianceEstimator.update(currentModelCovDistribution,obs.getProjectedPoint());
  if (posteriorPathStateDist.getPathState().isOnRoad()) {
    updatedState.getOnRoadModelCovarianceParam().setParameterPrior(currentModelCovDistribution);
    final Matrix stateCovSample=currentModelCovDistribution.sample(this.random);
    updatedState.getOnRoadModelCovarianceParam().setValue(stateCovSample);
    updatedState.getOnRoadModelCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector1D(),stateCovSample));
  }
 else {
    updatedState.getOffRoadModelCovarianceParam().setParameterPrior(currentModelCovDistribution);
    final Matrix stateCovSample=currentModelCovDistribution.sample(this.random);
    updatedState.getOffRoadModelCovarianceParam().setValue(stateCovSample);
    updatedState.getOffRoadModelCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector1D(),stateCovSample));
  }
  newObsStateSample=MotionStateEstimatorPredictor.getOg().times(modelCovarianceEstimator.getNewPosteriorStateSample().getGroundDistribution().getMean());
  final RoadMeasurementCovarianceEstimatorPredictor measurementCovarianceEstimator=this.getRoadMeasurementCovarianceEstimatorPredictor(updatedState,newObsStateSample);
  final ScaledInverseGammaCovDistribution currentObsCovDistribution=(ScaledInverseGammaCovDistribution)updatedState.getObservationCovarianceParam().getParameterPrior().clone();
  measurementCovarianceEstimator.update(currentObsCovDistribution,obs.getProjectedPoint());
  updatedState.getObservationCovarianceParam().setParameterPrior(currentObsCovDistribution);
  final Matrix obsCovSample=currentObsCovDistribution.sample(this.random);
  updatedState.getObservationCovarianceParam().setValue(obsCovSample);
  updatedState.getObservationCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector(obsCovSample.getNumColumns()),obsCovSample));
  updatedState.getMotionStateParam().setParameterPrior(posteriorPathStateDist.getMotionDistribution());
  final MultivariateGaussian obsMotionDist=updatedState.getMotionStateEstimatorPredictor().getObservationDistribution(posteriorPathStateDist.getMotionDistribution(),posteriorPathStateDist.getPathState().getEdge());
  updatedState.getMotionStateParam().setConditionalDistribution(obsMotionDist);
  updatedState.getMotionStateParam().setValue(obsMotionDist.getMean());
  final OnOffEdgeTransPriorDistribution updatedEdgeTransPrior=updatedState.getEdgeTransitionParam().getParameterPrior().clone();
  final InferenceGraphEdge fromEdge=updatedState.getParentState().getPathStateParam().getValue().getEdge().getInferenceGraphSegment();
  final InferenceGraphEdge toEdge=updatedState.getPathStateParam().getValue().getEdge().getInferenceGraphSegment();
  final OnOffEdgeTransitionEstimatorPredictor edgeTransitionEstimatorPredictor=this.getEdgeTransitionEstimatorPredictor(updatedState,fromEdge);
  edgeTransitionEstimatorPredictor.update(updatedEdgeTransPrior,toEdge);
  final OnOffEdgeTransDistribution updatedEdgeTransConditional=new OnOffEdgeTransDistribution(this.inferredGraph,updatedState.getPathStateParam().getValue(),toEdge,updatedState.getObservationCovarianceParam().getValue(),updatedEdgeTransPrior.getEdgeMotionTransProbPrior().getMean(),updatedEdgeTransPrior.getFreeMotionTransProbPrior().getMean());
  updatedState.setEdgeTransitionParam(SimpleBayesianParameter.create(updatedEdgeTransPrior.getMean(),updatedEdgeTransConditional,updatedEdgeTransPrior));
  updatedState.setEdgeTransitionParam(SimpleBayesianParameter.create(updatedEdgeTransPrior.getMean(),updatedEdgeTransConditional,updatedEdgeTransPrior));
  return updatedState;
}

Fixed version (ground truth):
/** 
 * This method performs the Bayes update for a single vehicle state.
 * @param state
 * @param obs
 * @return
 */
protected VehicleStateDistribution<O> internalUpdate(VehicleStateDistribution<O> state,O obs){
  final VehicleStateDistribution<O> updatedState=state.clone();
  final PathStateDistribution priorPredictivePathStateDist=updatedState.getPathStateParam().getParameterPrior().clone();
  final MultivariateGaussian updatedMotionState=priorPredictivePathStateDist.getMotionDistribution().clone();
  final PathStateDistribution posteriorPathStateDist;
  if (priorPredictivePathStateDist.getPathState().isOnRoad()) {
    final TruncatedRoadKalmanFilter roadFilter=updatedState.getMotionStateEstimatorPredictor().getRoadFilter().clone();
    final AdjMultivariateGaussian obsProj=PathUtils.getRoadObservation(obs.getProjectedPoint(),updatedState.getObservationCovarianceParam().getValue(),priorPredictivePathStateDist.getPathState().getPath(),priorPredictivePathStateDist.getPathState().getEdge());
    final double currentDist=updatedMotionState.getMean().getElement(0);
    if (obsProj.getMean().getElement(0) < currentDist) {
      obsProj.getMean().setElement(0,currentDist);
    }
    roadFilter.setMeasurementCovariance(obsProj.getCovariance());
    roadFilter.measure(updatedMotionState,obsProj.getMean());
    Preconditions.checkState(updatedMotionState.getCovariance().getElement(0,0) > 1e-6);
    final Path postPath=priorPredictivePathStateDist.getPathState().getPath();
    updatedMotionState.setMean(Iterables.getLast(postPath.getPathEdges()).clampToEdge(updatedMotionState.getMean()));
    posteriorPathStateDist=new PathStateDistribution(postPath,updatedMotionState);
  }
 else {
    updatedState.getMotionStateEstimatorPredictor().update(updatedMotionState,obs.getProjectedPoint());
    posteriorPathStateDist=new PathStateDistribution(priorPredictivePathStateDist.getPathState().getPath(),updatedMotionState);
  }
  updatedState.getPathStateParam().setValue(posteriorPathStateDist.getPathState());
  updatedState.getPathStateParam().setParameterPrior(posteriorPathStateDist);
  Vector newObsStateSample;
  final RoadModelCovarianceEstimatorPredictor modelCovarianceEstimator=new RoadModelCovarianceEstimatorPredictor(updatedState,state.getMotionStateEstimatorPredictor(),this.random);
  final ScaledInverseGammaCovDistribution currentModelCovDistribution=(ScaledInverseGammaCovDistribution)(posteriorPathStateDist.getPathState().isOnRoad() ? updatedState.getOnRoadModelCovarianceParam().getParameterPrior().clone() : updatedState.getOffRoadModelCovarianceParam().getParameterPrior().clone());
  modelCovarianceEstimator.update(currentModelCovDistribution,obs.getProjectedPoint());
  if (posteriorPathStateDist.getPathState().isOnRoad()) {
    updatedState.getOnRoadModelCovarianceParam().setParameterPrior(currentModelCovDistribution);
    final Matrix stateCovSample=currentModelCovDistribution.sample(this.random);
    updatedState.getOnRoadModelCovarianceParam().setValue(stateCovSample);
    updatedState.getOnRoadModelCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector1D(),stateCovSample));
  }
 else {
    updatedState.getOffRoadModelCovarianceParam().setParameterPrior(currentModelCovDistribution);
    final Matrix stateCovSample=currentModelCovDistribution.sample(this.random);
    updatedState.getOffRoadModelCovarianceParam().setValue(stateCovSample);
    updatedState.getOffRoadModelCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector1D(),stateCovSample));
  }
  newObsStateSample=MotionStateEstimatorPredictor.getOg().times(modelCovarianceEstimator.getNewPosteriorStateSample().getGroundDistribution().getMean());
  final RoadMeasurementCovarianceEstimatorPredictor measurementCovarianceEstimator=this.getRoadMeasurementCovarianceEstimatorPredictor(updatedState,newObsStateSample);
  final ScaledInverseGammaCovDistribution currentObsCovDistribution=(ScaledInverseGammaCovDistribution)updatedState.getObservationCovarianceParam().getParameterPrior().clone();
  measurementCovarianceEstimator.update(currentObsCovDistribution,obs.getProjectedPoint());
  updatedState.getObservationCovarianceParam().setParameterPrior(currentObsCovDistribution);
  final Matrix obsCovSample=currentObsCovDistribution.sample(this.random);
  updatedState.getObservationCovarianceParam().setValue(obsCovSample);
  updatedState.getObservationCovarianceParam().setConditionalDistribution(new MultivariateGaussian(VectorFactory.getDefault().createVector(obsCovSample.getNumColumns()),obsCovSample));
  updatedState.getMotionStateParam().setParameterPrior(posteriorPathStateDist.getMotionDistribution());
  final MultivariateGaussian obsMotionDist=updatedState.getMotionStateEstimatorPredictor().getObservationDistribution(posteriorPathStateDist.getMotionDistribution(),posteriorPathStateDist.getPathState().getEdge());
  updatedState.getMotionStateParam().setConditionalDistribution(obsMotionDist);
  updatedState.getMotionStateParam().setValue(obsMotionDist.getMean());
  final OnOffEdgeTransPriorDistribution updatedEdgeTransPrior=updatedState.getEdgeTransitionParam().getParameterPrior().clone();
  final InferenceGraphEdge fromEdge=updatedState.getParentState().getPathStateParam().getValue().getEdge().getInferenceGraphSegment();
  final InferenceGraphEdge toEdge=updatedState.getPathStateParam().getValue().getEdge().getInferenceGraphSegment();
  final OnOffEdgeTransitionEstimatorPredictor edgeTransitionEstimatorPredictor=this.getEdgeTransitionEstimatorPredictor(updatedState,fromEdge);
  edgeTransitionEstimatorPredictor.update(updatedEdgeTransPrior,toEdge);
  final OnOffEdgeTransDistribution updatedEdgeTransConditional=new OnOffEdgeTransDistribution(this.inferredGraph,updatedState.getPathStateParam().getValue(),toEdge,updatedState.getObservationCovarianceParam().getValue(),updatedEdgeTransPrior.getEdgeMotionTransProbPrior().getMean(),updatedEdgeTransPrior.getFreeMotionTransProbPrior().getMean());
  updatedState.setEdgeTransitionParam(SimpleBayesianParameter.create(updatedEdgeTransPrior.getMean(),updatedEdgeTransConditional,updatedEdgeTransPrior));
  updatedState.setEdgeTransitionParam(SimpleBayesianParameter.create(updatedEdgeTransPrior.getMean(),updatedEdgeTransConditional,updatedEdgeTransPrior));
  return updatedState;
}