Program: 42733
Project: 42733
Buggy source:
public HighTrustLocalizer(){
  wheelOrintation_buggyFrame=0.0;
  buggyFrame_gps_x=0.0;
  buggyFrame_gps_y=0.0;
  wheelOrintation_buggyFrame=0.0;
  new Subscriber(NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      wheelOrintation_buggyFrame=steerM.getAngle();
    }
  }
);
  new Subscriber(NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement encM=(EncoderMeasurement)m;
      secondOldestEncoder=mostRecentEncoder;
      mostRecentEncoder=encM.getDistance();
      double worldOrintation=buggyFrame_rot_z + wheelOrintation_buggyFrame;
      double deltaEncoder=mostRecentEncoder - secondOldestEncoder;
      So2Pose deltaPose=new So2Pose(deltaEncoder,0.0,worldOrintation);
      com.roboclub.robobuggy.map.Point deltaPoint=deltaPose.getSe2Point();
    }
  }
);
  new Subscriber(NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement gpsM=(GpsMeasurement)m;
      double oldX=buggyFrame_gps_x;
      double oldY=buggyFrame_gps_y;
      buggyFrame_gps_y=gpsM.getLongitude();
      buggyFrame_gps_x=gpsM.getLatitude();
      double dy=buggyFrame_gps_y - oldY;
      double dx=buggyFrame_gps_x - oldX;
      buggyFrame_rot_z=180 * Math.atan2(dy,dx) / Math.PI;
      publishUpdate();
    }
  }
);
  new Subscriber(NodeChannel.IMU.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      ImuMeasurement imuM=(ImuMeasurement)m;
    }
  }
);
  new Subscriber(NodeChannel.IMU_MAGNETIC.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      MagneticMeasurement magM=(MagneticMeasurement)m;
    }
  }
);
}

Fixed version (ground truth):
/** 
 * Constructor for the High Trust Localizer which will initialize the system to an identity (zero position) 
 */
public HighTrustLocalizer(){
  wheelOrintationBuggyFrame=0.0;
  buggyFrameGpsX=0.0;
  buggyFrameGpsY=0.0;
  wheelOrintationBuggyFrame=0.0;
  new Subscriber(NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      wheelOrintationBuggyFrame=steerM.getAngle();
    }
  }
);
  new Subscriber(NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement encM=(EncoderMeasurement)m;
      secondOldestEncoder=mostRecentEncoder;
      mostRecentEncoder=encM.getDistance();
      double worldOrintation=buggyFrameRotZ + wheelOrintationBuggyFrame;
      double deltaEncoder=mostRecentEncoder - secondOldestEncoder;
      So2Pose deltaPose=new So2Pose(deltaEncoder,0.0,worldOrintation);
      com.roboclub.robobuggy.map.Point deltaPoint=deltaPose.getSe2Point();
    }
  }
);
  new Subscriber(NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement gpsM=(GpsMeasurement)m;
      double oldX=buggyFrameGpsX;
      double oldY=buggyFrameGpsY;
      buggyFrameGpsY=gpsM.getLongitude();
      buggyFrameGpsX=gpsM.getLatitude();
      double dy=buggyFrameGpsY - oldY;
      double dx=buggyFrameGpsX - oldX;
      buggyFrameRotZ=180 * Math.atan2(dy,dx) / Math.PI;
      publishUpdate();
    }
  }
);
  new Subscriber(NodeChannel.IMU.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      ImuMeasurement imuM=(ImuMeasurement)m;
    }
  }
);
  new Subscriber(NodeChannel.IMU_MAGNETIC.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      MagneticMeasurement magM=(MagneticMeasurement)m;
    }
  }
);
}