Program: 42702
Project: 42702
Buggy source:
/** 
 * Constructor for the High Trust Localizer which will initialize the system to an identity (zero position)
 */
public HighTrustGPSLocalizer(){
  buggyFrameGpsLon=0.0;
  buggyFrameGpsLat=0.0;
  lastEncoderReading=0.0;
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  mostRecentUpdate=new Date();
  new Subscriber(NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
synchronized (this) {
        long dt=newGPSData.getTimestamp().getTime() - mostRecentUpdate.getTime();
        if (dt > 0.0) {
          double oldGPSX=buggyFrameGpsLon;
          double oldGPSY=buggyFrameGpsLat;
          double dLat=buggyFrameGpsLat - oldGPSY;
          double dLon=buggyFrameGpsLon - oldGPSX;
          double oldRotZ=buggyFrameRotZ;
          publishUpdate();
          mostRecentUpdate=newGPSData.getTimestamp();
        }
      }
    }
  }
);
  new Subscriber(NodeChannel.IMU_MAGNETIC.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      MagneticMeasurement magM=(MagneticMeasurement)m;
      double currAngle=magM.getRotationZ();
      double offset=0.0;
      publishUpdate();
    }
  }
);
  new Subscriber(NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement magM=(EncoderMeasurement)m;
      double dEncoder=magM.getDistance() - lastEncoderReading;
      lastEncoderReading=magM.getDistance();
      buggyFrameGpsLon=buggyFrameGpsLon + dEncoder * Math.cos(buggyFrameRotZ) + dEncoder * Math.sin(buggyFrameRotZ);
      buggyFrameGpsLat=-buggyFrameGpsLat + dEncoder * Math.sin(buggyFrameRotZ) + dEncoder * Math.cos(buggyFrameRotZ);
      publishUpdate();
    }
  }
);
}

Fixed version (ground truth):
/** 
 * Constructor for the High Trust Localizer which will initialize the system to an identity (zero position)
 */
public HighTrustGPSLocalizer(){
  buggyFrameGpsLon=0.0;
  buggyFrameGpsLat=0.0;
  lastEncoderReading=0.0;
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  mostRecentUpdate=new Date();
  new Subscriber(NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
synchronized (this) {
        long dt=newGPSData.getTimestamp().getTime() - mostRecentUpdate.getTime();
        if (dt > 0.0) {
          publishUpdate();
          mostRecentUpdate=newGPSData.getTimestamp();
        }
      }
    }
  }
);
  new Subscriber(NodeChannel.IMU_MAGNETIC.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      MagneticMeasurement magM=(MagneticMeasurement)m;
      double currAngle=magM.getRotationZ();
      double offset=0.0;
      buggyFrameRotZ=currAngle - offset;
      publishUpdate();
    }
  }
);
  new Subscriber(NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement magM=(EncoderMeasurement)m;
      double dEncoder=magM.getDistance() - lastEncoderReading;
      lastEncoderReading=magM.getDistance();
      buggyFrameGpsLon=buggyFrameGpsLon + dEncoder * Math.cos(buggyFrameRotZ) + dEncoder * Math.sin(buggyFrameRotZ);
      buggyFrameGpsLat=-buggyFrameGpsLat + dEncoder * Math.sin(buggyFrameRotZ) + dEncoder * Math.cos(buggyFrameRotZ);
      publishUpdate();
    }
  }
);
}