Program: 42658
Project: 42658
Buggy source:
/** 
 * Constructor for the High Trust Localizer which will initialize the system to an identity (zero position)
 */
public HighTrustGPSLocalizer(){
  buggyFrameGpsX=0.0;
  buggyFrameGpsY=0.0;
  buggyFrameRotZ=0.0;
  buggySteeringAngle=0.0;
  lastEncoderReading=0.0;
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  new Subscriber("String_Node_Str",NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      buggySteeringAngle=steerM.getAngle();
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
synchronized (this) {
        buggyFrameGpsY=newGPSData.getLatitude();
        buggyFrameGpsX=newGPSData.getLongitude();
        double dLat=buggyFrameGpsY - oldGPSY;
        double dLon=buggyFrameGpsX - oldGPSX;
        oldGPSX=buggyFrameGpsX;
        oldGPSY=buggyFrameGpsY;
        buggyFrameRotZ=Math.toDegrees(Math.atan2(LocalizerUtil.convertLatToMeters(dLat),LocalizerUtil.convertLonToMeters(dLon)));
        publishUpdate();
      }
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement measurement=(EncoderMeasurement)m;
      double currentEncoderMeasurement=measurement.getDistance();
      double deltaDistance=currentEncoderMeasurement - lastEncoderReading;
      buggyFrameRotZ+=MotionModel.getHeadingChange(deltaDistance,buggySteeringAngle);
      LocTuple deltaPos=LocalizerUtil.convertMetersToLatLng(deltaDistance,buggyFrameRotZ);
      buggyFrameGpsY+=deltaPos.getLatitude();
      buggyFrameGpsX+=deltaPos.getLongitude();
      lastEncoderReading=currentEncoderMeasurement;
      publishUpdate();
    }
  }
);
}

Fixed version (ground truth):
/** 
 * Constructor for the High Trust Localizer which will initialize the system to an identity (zero position)
 */
public HighTrustGPSLocalizer(){
  buggyFrameGpsX=0.0;
  buggyFrameGpsY=0.0;
  buggyFrameRotZ=0.0;
  buggySteeringAngle=0.0;
  lastEncoderReading=0.0;
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  new Subscriber("String_Node_Str",NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      buggySteeringAngle=steerM.getAngle();
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
synchronized (this) {
        buggyFrameGpsY=newGPSData.getLatitude();
        buggyFrameGpsX=newGPSData.getLongitude();
        double dLat=buggyFrameGpsY - oldGPSY;
        double dLon=buggyFrameGpsX - oldGPSX;
        oldGPSX=buggyFrameGpsX;
        oldGPSY=buggyFrameGpsY;
        buggyFrameRotZ=Math.toDegrees(Math.atan2(LocalizerUtil.convertLatToMeters(dLat),LocalizerUtil.convertLonToMeters(dLon)));
        publishUpdate();
      }
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.IMU_ANG_POS.getMsgPath(),((topicName,m) -> {
    IMUAngularPositionMessage mes=((IMUAngularPositionMessage)m);
    Matrix r=new Matrix(mes.getRot());
    double[][] xVec={{1},{0},{0}};
    double[][] yVec={{0},{1},{0}};
    double x=r.times(new Matrix(xVec)).get(0,0);
    double y=r.times(new Matrix(yVec)).get(0,0);
    buggyFrameRotZ=Util.normalizeAngleDeg(Math.toDegrees(-Math.atan2(y,x)) + 90);
    publishUpdate();
  }
));
  new Subscriber("String_Node_Str",NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement measurement=(EncoderMeasurement)m;
      double currentEncoderMeasurement=measurement.getDistance();
      double deltaDistance=currentEncoderMeasurement - lastEncoderReading;
      buggyFrameRotZ+=MotionModel.getHeadingChange(deltaDistance,buggySteeringAngle);
      LocTuple deltaPos=LocalizerUtil.convertMetersToLatLng(deltaDistance,buggyFrameRotZ);
      buggyFrameGpsY+=deltaPos.getLatitude();
      buggyFrameGpsX+=deltaPos.getLongitude();
      lastEncoderReading=currentEncoderMeasurement;
      publishUpdate();
    }
  }
);
}