Program: 189435
Project: 189435
Buggy source:
private void updateAndCheckStats(DataDistribution<VehicleStateDistribution<GpsObservation>> vehicleStateDist,VehicleStateDistribution<GpsObservation> trueVehicleState,SufficientStatistic obsErrorSS,SufficientStatistic stateErrorSS,SufficientStatistic obsCovErrorSS,SufficientStatistic onRoadCovErrorSS,SufficientStatistic offRoadCovErrorSS,SufficientStatistic transitionsSS,boolean generalizeMoveDiff){
  final SufficientStatistic groundStateMeanStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic obsCovMeanStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic onRoadCovStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic offRoadCovStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic transitionStat=new MultivariateGaussian.SufficientStatistic();
  int truePathsFound=0;
  int numOnTrueEdge=0;
  final boolean hasPriorOnVariances=false;
  Preconditions.checkState(vehicleStateDist.getDomainSize() > 0);
  trueVehicleState.getObservation().getProjectedPoint().minus(trueVehicleState.getMeanLocation());
  for (  final VehicleStateDistribution<GpsObservation> state : vehicleStateDist.getDomain()) {
    AssertJUnit.assertEquals(new TrueObservation(trueVehicleState.getObservation(),null),state.getObservation());
    final int stateCount;
    if (vehicleStateDist instanceof CountedDataDistribution<?>) {
      stateCount=((CountedDataDistribution<VehicleStateDistribution<GpsObservation>>)vehicleStateDist).getCount(state);
    }
 else {
      stateCount=1;
    }
    final PathState pathState=state.getPathStateParam().getValue();
    final PathState truePathState=state.getPathStateParam().getValue();
    final VehicleStateDistribution<GpsObservation> parentState=state.getParentState();
    Vector transType=null;
    if (parentState != null) {
      final PathState parentPathState=parentState.getPathStateParam().getValue();
      transType=OnOffEdgeTransDistribution.getTransitionType(parentPathState.getEdge().getInferenceGraphEdge(),pathState.getEdge().getInferenceGraphEdge());
      if (parentPathState.isOnRoad()) {
        transType=transType.stack(MotionStateEstimatorPredictor.zeros2D);
      }
 else {
        transType=MotionStateEstimatorPredictor.zeros2D.stack(transType);
      }
    }
    for (int i=0; i < stateCount; i++) {
      if ((pathState.isOnRoad() && truePathState.isOnRoad() && pathState.getPath().getGeometry().covers(truePathState.getPath().getGeometry())) || (!pathState.isOnRoad() && !truePathState.isOnRoad())) {
        truePathsFound++;
      }
      if (pathState.getEdge().getInferenceGraphEdge().equals(truePathState.getEdge().getInferenceGraphEdge())) {
        numOnTrueEdge++;
      }
      groundStateMeanStat.update(pathState.getGroundState());
      final Matrix obsCovMean=state.getObservationCovarianceParam().getParameterPrior().getMean();
      obsCovMeanStat.update(obsCovMean.convertToVector());
      final Matrix onRoadCovMean=state.getOnRoadModelCovarianceParam().getParameterPrior().getMean();
      onRoadCovStat.update(onRoadCovMean.convertToVector());
      offRoadCovStat.update(state.getOffRoadModelCovarianceParam().getValue().convertToVector());
      if (transType != null) {
        transitionStat.update(transType);
      }
    }
  }
  this.log.debug("String_Node_Str" + truePathsFound);
  this.log.debug("String_Node_Str" + numOnTrueEdge);
  Preconditions.checkNotNull(groundStateMeanStat.getMean());
  final Vector obsError=trueVehicleState.getObservation().getProjectedPoint().minus(MotionStateEstimatorPredictor.getOg().times(groundStateMeanStat.getMean()));
  final PathState truePathState=trueVehicleState.getPathStateParam().getValue();
  final Vector stateError=truePathState.getGroundState().minus(groundStateMeanStat.getMean());
  final Vector obsCovError;
  final Vector onRoadCovError;
  final Vector offRoadCovError;
  obsCovError=obsCovMeanStat.getMean().minus(trueVehicleState.getObservationCovarianceParam().getValue().convertToVector());
  onRoadCovError=onRoadCovStat.getMean().minus(trueVehicleState.getOnRoadModelCovarianceParam().getValue().convertToVector());
  offRoadCovError=offRoadCovStat.getMean().minus(trueVehicleState.getOffRoadModelCovarianceParam().getValue().convertToVector());
  this.log.info("String_Node_Str" + truePathState.getMotionState());
  this.log.debug("String_Node_Str" + obsError);
  this.log.debug("String_Node_Str" + stateError);
  this.log.debug("String_Node_Str" + obsCovMeanStat.getMean());
  this.log.debug("String_Node_Str" + onRoadCovStat.getMean());
  this.log.debug("String_Node_Str" + offRoadCovStat.getMean());
  if (transitionStat.getMean() != null) {
    final Vector trueTransProbs=trueVehicleState.getEdgeTransitionParam().getParameterPrior().getEdgeMotionTransProbPrior().getMean().stack(trueVehicleState.getEdgeTransitionParam().getParameterPrior().getFreeMotionTransProbPrior().getMean());
    transitionsSS.update(transitionStat.getMean().minus(trueTransProbs));
  }
  obsErrorSS.update(obsError);
  stateErrorSS.update(stateError);
  obsCovErrorSS.update(obsCovError);
  onRoadCovErrorSS.update(onRoadCovError);
  offRoadCovErrorSS.update(offRoadCovError);
  if (!hasPriorOnVariances) {
    this.log.debug("String_Node_Str" + obsErrorSS.getMean());
    this.log.debug("String_Node_Str" + stateErrorSS.getMean());
  }
  if (obsErrorSS.getCount() > 1) {
    VehicleStatePLFilterSimulationTest.assertVectorWithCovarianceError(obsErrorSS.getMean(),trueVehicleState.getObservationCovarianceParam().getValue(),5d);
    final Matrix stateModelCovariance=this.createModelCovariance(trueVehicleState,stateErrorSS);
    VehicleStatePLFilterSimulationTest.assertVectorWithCovarianceError(stateErrorSS.getMean(),stateModelCovariance,5d);
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.fourZeros,obsCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getObservationCovarianceParam().getValue().normFrobenius());
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.oneZero,onRoadCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getOnRoadModelCovarianceParam().getValue().normFrobenius());
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.fourZeros,offRoadCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getOffRoadModelCovarianceParam().getValue().normFrobenius());
  }
}

Fixed version (ground truth):
private void updateAndCheckStats(DataDistribution<VehicleStateDistribution<GpsObservation>> vehicleStateDist,VehicleStateDistribution<GpsObservation> trueVehicleState,SufficientStatistic obsErrorSS,SufficientStatistic stateErrorSS,SufficientStatistic obsCovErrorSS,SufficientStatistic onRoadCovErrorSS,SufficientStatistic offRoadCovErrorSS,SufficientStatistic transitionsSS,boolean generalizeMoveDiff){
  final SufficientStatistic groundStateMeanStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic obsCovMeanStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic onRoadCovStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic offRoadCovStat=new MultivariateGaussian.SufficientStatistic();
  final SufficientStatistic transitionStat=new MultivariateGaussian.SufficientStatistic();
  int truePathsFound=0;
  int numOnTrueEdge=0;
  final boolean hasPriorOnVariances=false;
  Preconditions.checkState(vehicleStateDist.getDomainSize() > 0);
  trueVehicleState.getObservation().getProjectedPoint().minus(trueVehicleState.getMeanLocation());
  for (  final VehicleStateDistribution<GpsObservation> state : vehicleStateDist.getDomain()) {
    AssertJUnit.assertEquals(new TrueObservation(trueVehicleState.getObservation(),null),state.getObservation());
    final int stateCount;
    if (vehicleStateDist instanceof CountedDataDistribution<?>) {
      stateCount=((CountedDataDistribution<VehicleStateDistribution<GpsObservation>>)vehicleStateDist).getCount(state);
    }
 else {
      stateCount=1;
    }
    final PathState pathState=state.getPathStateParam().getValue();
    final PathState truePathState=state.getPathStateParam().getValue();
    final VehicleStateDistribution<GpsObservation> parentState=state.getParentState();
    Vector transType=null;
    if (parentState != null) {
      final PathState parentPathState=parentState.getPathStateParam().getValue();
      transType=OnOffEdgeTransDistribution.getTransitionType(parentPathState.getEdge().getInferenceGraphSegment(),pathState.getEdge().getInferenceGraphSegment());
      if (parentPathState.isOnRoad()) {
        transType=transType.stack(MotionStateEstimatorPredictor.zeros2D);
      }
 else {
        transType=MotionStateEstimatorPredictor.zeros2D.stack(transType);
      }
    }
    for (int i=0; i < stateCount; i++) {
      if ((pathState.isOnRoad() && truePathState.isOnRoad() && pathState.getPath().getGeometry().covers(truePathState.getPath().getGeometry())) || (!pathState.isOnRoad() && !truePathState.isOnRoad())) {
        truePathsFound++;
      }
      if (pathState.getEdge().getInferenceGraphSegment().equals(truePathState.getEdge().getInferenceGraphSegment())) {
        numOnTrueEdge++;
      }
      groundStateMeanStat.update(pathState.getGroundState());
      final Matrix obsCovMean=state.getObservationCovarianceParam().getParameterPrior().getMean();
      obsCovMeanStat.update(obsCovMean.convertToVector());
      final Matrix onRoadCovMean=state.getOnRoadModelCovarianceParam().getParameterPrior().getMean();
      onRoadCovStat.update(onRoadCovMean.convertToVector());
      offRoadCovStat.update(state.getOffRoadModelCovarianceParam().getValue().convertToVector());
      if (transType != null) {
        transitionStat.update(transType);
      }
    }
  }
  this.log.debug("String_Node_Str" + truePathsFound);
  this.log.debug("String_Node_Str" + numOnTrueEdge);
  Preconditions.checkNotNull(groundStateMeanStat.getMean());
  final Vector obsError=trueVehicleState.getObservation().getProjectedPoint().minus(MotionStateEstimatorPredictor.getOg().times(groundStateMeanStat.getMean()));
  final PathState truePathState=trueVehicleState.getPathStateParam().getValue();
  final Vector stateError=truePathState.getGroundState().minus(groundStateMeanStat.getMean());
  final Vector obsCovError;
  final Vector onRoadCovError;
  final Vector offRoadCovError;
  obsCovError=obsCovMeanStat.getMean().minus(trueVehicleState.getObservationCovarianceParam().getValue().convertToVector());
  onRoadCovError=onRoadCovStat.getMean().minus(trueVehicleState.getOnRoadModelCovarianceParam().getValue().convertToVector());
  offRoadCovError=offRoadCovStat.getMean().minus(trueVehicleState.getOffRoadModelCovarianceParam().getValue().convertToVector());
  this.log.info("String_Node_Str" + truePathState.getMotionState());
  this.log.debug("String_Node_Str" + obsError);
  this.log.debug("String_Node_Str" + stateError);
  this.log.debug("String_Node_Str" + obsCovMeanStat.getMean());
  this.log.debug("String_Node_Str" + onRoadCovStat.getMean());
  this.log.debug("String_Node_Str" + offRoadCovStat.getMean());
  if (transitionStat.getMean() != null) {
    final Vector trueTransProbs=trueVehicleState.getEdgeTransitionParam().getParameterPrior().getEdgeMotionTransProbPrior().getMean().stack(trueVehicleState.getEdgeTransitionParam().getParameterPrior().getFreeMotionTransProbPrior().getMean());
    transitionsSS.update(transitionStat.getMean().minus(trueTransProbs));
  }
  obsErrorSS.update(obsError);
  stateErrorSS.update(stateError);
  obsCovErrorSS.update(obsCovError);
  onRoadCovErrorSS.update(onRoadCovError);
  offRoadCovErrorSS.update(offRoadCovError);
  if (!hasPriorOnVariances) {
    this.log.debug("String_Node_Str" + obsErrorSS.getMean());
    this.log.debug("String_Node_Str" + stateErrorSS.getMean());
  }
  if (obsErrorSS.getCount() > 1) {
    VehicleStatePLFilterSimulationTest.assertVectorWithCovarianceError(obsErrorSS.getMean(),trueVehicleState.getObservationCovarianceParam().getValue(),5d);
    final Matrix stateModelCovariance=this.createModelCovariance(trueVehicleState,stateErrorSS);
    VehicleStatePLFilterSimulationTest.assertVectorWithCovarianceError(stateErrorSS.getMean(),stateModelCovariance,5d);
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.fourZeros,obsCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getObservationCovarianceParam().getValue().normFrobenius());
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.oneZero,onRoadCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getOnRoadModelCovarianceParam().getValue().normFrobenius());
    ArrayAsserts.assertArrayEquals(VehicleStatePLFilterSimulationTest.fourZeros,offRoadCovErrorSS.getMean().toArray(),0.7d * trueVehicleState.getOffRoadModelCovarianceParam().getValue().normFrobenius());
  }
}