Program: 42615
Project: 42615
Buggy source:
private double purePursuitController(){
  int closestIndex=getClosestIndex(wayPoints,pose);
  double K=3.0;
  double velocity=pose.getCurrentState().get(2,0);
  double lookaheadLowerBound=5.0;
  double lookaheadUpperBound=25.0;
  double lookahead=K * velocity;
  if (lookahead < lookaheadLowerBound) {
    lookahead=lookaheadLowerBound;
  }
 else   if (lookahead > lookaheadUpperBound) {
    lookahead=lookaheadUpperBound;
  }
  int lookaheadIndex=0;
  for (lookaheadIndex=closestIndex; lookaheadIndex < wayPoints.size(); lookaheadIndex++) {
    if (GPSPoseMessage.getDistance(pose,wayPoints.get(lookaheadIndex).toGpsPoseMessage(0)) > lookahead) {
      break;
    }
  }
  if (lookaheadIndex >= wayPoints.size()) {
    new RobobuggyLogicNotification("String_Node_Str",RobobuggyMessageLevel.EXCEPTION);
    return 0;
  }
  GpsMeasurement target=wayPoints.get(lookaheadIndex);
  currentWaypoint=target;
  double dx=LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude());
  double dy=LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude());
  double deltaHeading=Math.atan2(dy,dx) - pose.getHeading();
  GpsMeasurement B=wayPoints.get(closestIndex);
  GpsMeasurement A=wayPoints.get(closestIndex + 1);
  GPSPoseMessage P=pose;
  double padx=LocalizerUtil.convertLonToMeters(P.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude());
  double pady=LocalizerUtil.convertLatToMeters(P.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude());
  double phi=Math.atan2(pady,padx);
  double badx=LocalizerUtil.convertLonToMeters(B.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude());
  double bady=LocalizerUtil.convertLatToMeters(B.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude());
  double psi=Math.atan2(bady,badx);
  double theta=phi - psi;
  double L=GPSPoseMessage.getDistance(P,A.toGpsPoseMessage(0));
  double E=L * Math.sin(theta);
  double thetaDelta=psi - Math.atan2(dy,dx);
  thetaDelta*=E;
  deltaHeading+=thetaDelta;
  double commandedAngle=Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading),lookahead);
  commandedAngle=Util.normalizeAngleRad(commandedAngle);
  return commandedAngle;
}

Fixed version (ground truth):
private double purePursuitController(){
  int closestIndex=getClosestIndex(wayPoints,pose);
  double K=2.5;
  double velocity=pose.getCurrentState().get(2,0);
  double lookaheadLowerBound=5.0;
  double lookaheadUpperBound=25.0;
  double lookahead=K * velocity;
  if (lookahead < lookaheadLowerBound) {
    lookahead=lookaheadLowerBound;
  }
 else   if (lookahead > lookaheadUpperBound) {
    lookahead=lookaheadUpperBound;
  }
  int lookaheadIndex=0;
  for (lookaheadIndex=closestIndex; lookaheadIndex < wayPoints.size(); lookaheadIndex++) {
    if (GPSPoseMessage.getDistance(pose,wayPoints.get(lookaheadIndex).toGpsPoseMessage(0)) > lookahead) {
      break;
    }
  }
  if (lookaheadIndex >= wayPoints.size()) {
    new RobobuggyLogicNotification("String_Node_Str",RobobuggyMessageLevel.EXCEPTION);
    return 0;
  }
  GpsMeasurement target=wayPoints.get(lookaheadIndex);
  currentWaypoint=target;
  double dx=LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude());
  double dy=LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude());
  double deltaHeading=Math.atan2(dy,dx) - pose.getHeading();
  GpsMeasurement B=wayPoints.get(closestIndex);
  GpsMeasurement A=wayPoints.get(closestIndex + 1);
  GPSPoseMessage P=pose;
  double padx=LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(P.getLongitude());
  double pady=LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(P.getLatitude());
  double phi=Math.atan2(pady,padx);
  double badx=LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(B.getLongitude());
  double bady=LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(B.getLatitude());
  double psi=Math.atan2(bady,badx);
  double theta=phi - psi;
  double L=GPSPoseMessage.getDistance(P,B.toGpsPoseMessage(0));
  double E=L * Math.sin(theta);
  double thetaDelta=phi - psi;
  thetaDelta*=E / 10.0;
  deltaHeading+=thetaDelta;
  double commandedAngle=Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading),lookahead);
  commandedAngle=Util.normalizeAngleRad(commandedAngle);
  return commandedAngle;
}