Program: 189475
Project: 189475
Buggy source:
private PathState sampleSmoothedPrevState(PathStateBelief prior,PathStateBelief priorPred,PathStateBelief posterior,Vector obs,Random rng){
  final PathStateBelief priorOnPath=priorPred.getPath().getStateBeliefOnPath(prior);
  Preconditions.checkState(Iterables.getFirst(priorOnPath.getPath().getPathEdges(),null).equals(Iterables.getFirst(priorPred.getPath().getPathEdges(),null)) && Iterables.getFirst(priorPred.getPath().getPathEdges(),null).equals(Iterables.getFirst(posterior.getPath().getPathEdges(),null)));
  final Matrix F;
  final Matrix G;
  final Matrix C;
  final Vector m;
  final Matrix Omega;
  final Vector y;
  final Matrix Sigma;
  if (posterior.isOnRoad()) {
    final MultivariateGaussian obsProjBelief=PathUtils.getRoadObservation(obs,this.obsCovar,priorPred.getPath(),Iterables.getLast(priorPred.getPath().getPathEdges()));
    y=obsProjBelief.getMean();
    Sigma=obsProjBelief.getCovariance();
    F=AbstractRoadTrackingFilter.getOr();
    G=this.getRoadModel().getA();
    C=priorOnPath.getGlobalStateBelief().getCovariance();
    m=priorOnPath.getGlobalState();
    Omega=this.getOnRoadStateTransCovar();
  }
 else {
    y=obs;
    F=AbstractRoadTrackingFilter.getOg();
    G=this.getGroundModel().getA();
    C=priorOnPath.getGroundBelief().getCovariance();
    m=priorOnPath.getGroundState();
    Omega=this.getOffRoadStateTransCovar();
    Sigma=this.getObsCovar();
  }
  final Matrix W=F.times(Omega).times(F.transpose()).plus(Sigma);
  final Matrix FG=F.times(G);
  final Matrix A=FG.times(C).times(FG.transpose()).plus(W);
  final Matrix Wtil=A.transpose().solve(FG.times(C.transpose())).transpose();
  final Vector mSmooth=m.plus(Wtil.times(y.minus(FG.times(m))));
  final Matrix CSmooth=C.minus(Wtil.times(A).times(Wtil.transpose()));
  final Matrix Csqrt=StatisticsUtil.rootOfSemiDefinite(CSmooth);
  final Vector result=MultivariateGaussian.sample(mSmooth,Csqrt,rng);
  return posterior.getPath().getStateOnPath(result);
}

Fixed version (ground truth):
private PathState sampleSmoothedPrevState(PathStateBelief prior,PathStateBelief priorPred,PathStateBelief posterior,Vector obs,Random rng){
  final PathStateBelief priorOnPath=priorPred.getPath().getStateBeliefOnPath(prior);
  Preconditions.checkState(Iterables.getFirst(priorOnPath.getPath().getPathEdges(),null).equals(Iterables.getFirst(priorPred.getPath().getPathEdges(),null)) && Iterables.getFirst(priorPred.getPath().getPathEdges(),null).equals(Iterables.getFirst(posterior.getPath().getPathEdges(),null)));
  final Matrix F;
  final Matrix G;
  final Matrix C;
  final Vector m;
  final Matrix Omega;
  final Vector y;
  final Matrix Sigma;
  if (posterior.isOnRoad()) {
    final MultivariateGaussian obsProjBelief=PathUtils.getRoadObservation(obs,this.obsCovar,priorPred.getPath(),posterior.getEdge());
    y=obsProjBelief.getMean();
    Sigma=obsProjBelief.getCovariance();
    F=AbstractRoadTrackingFilter.getOr();
    G=this.getRoadModel().getA();
    C=priorOnPath.getGlobalStateBelief().getCovariance();
    m=priorOnPath.getGlobalState();
    Omega=this.getOnRoadStateTransCovar();
  }
 else {
    y=obs;
    F=AbstractRoadTrackingFilter.getOg();
    G=this.getGroundModel().getA();
    C=priorOnPath.getGroundBelief().getCovariance();
    m=priorOnPath.getGroundState();
    Omega=this.getOffRoadStateTransCovar();
    Sigma=this.getObsCovar();
  }
  final Matrix W=F.times(Omega).times(F.transpose()).plus(Sigma);
  final Matrix FG=F.times(G);
  final Matrix A=FG.times(C).times(FG.transpose()).plus(W);
  final Matrix Wtil=A.transpose().solve(FG.times(C.transpose())).transpose();
  final Vector mSmooth=m.plus(Wtil.times(y.minus(FG.times(m))));
  final Matrix CSmooth=C.minus(Wtil.times(A).times(Wtil.transpose()));
  final Matrix Csqrt=StatisticsUtil.rootOfSemiDefinite(CSmooth);
  final Vector result=MultivariateGaussian.sample(mSmooth,Csqrt,rng);
  return posterior.getPath().getStateOnPath(result);
}