Program: 42612
Project: 42612
Buggy source:
/** 
 * Create a new   {@link PeriodicNode} decorator
 * @param name - name of full simulation runner
 * @param initialPos - initial position of full simulation runner
 */
public FullSimRunner(String name,LocTuple initialPos){
  super(NodeChannel.SIMULATION);
  double[][] motionModelArr={{1,0,0,0,0},{0,1,0,0,0},{0,0,1,0,0},{0,0,0,1,0},{0,0,0,0,0}};
  motionModel=new Matrix(motionModelArr);
  UTMTuple t=LocalizerUtil.deg2UTM(initialPos);
  double[][] stateArr={{t.getEasting()},{t.getNorthing()},{VELOCITY},{INITIAL_HEADING_IN_RADS},{0}};
  state=new Matrix(stateArr);
  gpsPub=new Publisher(NodeChannel.GPS.getMsgPath());
  encPub=new Publisher(NodeChannel.ENCODER.getMsgPath());
  steerPub=new Publisher(NodeChannel.STEERING.getMsgPath());
  steerSub=new Subscriber("String_Node_Str",NodeChannel.DRIVE_CTRL.getMsgPath(),(topicName,m) -> {
    updateMotionModel(((DriveControlMessage)m).getAngleDouble());
    steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage)m).getAngleDouble())));
  }
);
  updateMotionModel(0);
  try {
    localizer=new RobobuggyKFLocalizer(LOCALIZER_UPDATE_PERIOD,"String_Node_Str",initialPos);
    localizer.startNode();
    controller=new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()));
    controller.startNode();
  }
 catch (  FileNotFoundException e) {
    e.printStackTrace();
  }
  gpsTimer=new Timer();
  gpsTimer.scheduleAtFixedRate(new TimerTask(){
    @Override public void run(){
      Matrix nextStep=generateNextStep();
      UTMTuple nextLocUTM=new UTMTuple(UTMZONE,'T',state.get(0,0),state.get(1,0));
      nextLocUTM.setEasting(nextLocUTM.getEasting());
      nextLocUTM.setNorthing(nextLocUTM.getNorthing());
      LocTuple nextLocLL=LocalizerUtil.utm2Deg(nextLocUTM);
      gpsPub.publish(new GpsMeasurement(new Date(),nextLocLL.getLatitude(),true,nextLocLL.getLongitude(),true,0,0,0,0,0,0));
      state=nextStep;
    }
  }
,0,GPS_UPDATE_PERIOD);
  encTimer=new Timer();
  encTimer.scheduleAtFixedRate(new TimerTask(){
    @Override public void run(){
      state=generateNextStep();
      double deltaDist=ENC_UPDATE_PERIOD / 1000.0 * state.get(2,0);
      totalDistance+=deltaDist;
      encPub.publish(new EncoderMeasurement(totalDistance,state.get(2,0)));
    }
  }
,0,ENC_UPDATE_PERIOD);
}

Fixed version (ground truth):
/** 
 * Create a new   {@link PeriodicNode} decorator
 * @param name - name of full simulation runner
 * @param initialPos - initial position of full simulation runner
 */
public FullSimRunner(String name,LocTuple initialPos){
  super(NodeChannel.SIMULATION);
  double[][] motionModelArr={{1,0,0,0,0},{0,1,0,0,0},{0,0,1,0,0},{0,0,0,1,0},{0,0,0,0,0}};
  motionModel=new Matrix(motionModelArr);
  UTMTuple t=LocalizerUtil.deg2UTM(initialPos);
  double[][] stateArr={{t.getEasting()},{t.getNorthing()},{VELOCITY},{INITIAL_HEADING_IN_RADS},{0}};
  state=new Matrix(stateArr);
  gpsPub=new Publisher(NodeChannel.GPS.getMsgPath());
  encPub=new Publisher(NodeChannel.ENCODER.getMsgPath());
  steerPub=new Publisher(NodeChannel.STEERING.getMsgPath());
  new Subscriber("String_Node_Str",NodeChannel.DRIVE_CTRL.getMsgPath(),(topicName,m) -> {
    updateMotionModel(((DriveControlMessage)m).getAngleDouble());
    steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage)m).getAngleDouble())));
  }
);
  updateMotionModel(0);
  try {
    localizer=new RobobuggyKFLocalizer(LOCALIZER_UPDATE_PERIOD,"String_Node_Str",initialPos);
    localizer.startNode();
    controller=new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()));
    controller.startNode();
  }
 catch (  FileNotFoundException e) {
    e.printStackTrace();
  }
  gpsTimer=new Timer();
  gpsTimer.scheduleAtFixedRate(new TimerTask(){
    @Override public void run(){
      Matrix nextStep=generateNextStep();
      UTMTuple nextLocUTM=new UTMTuple(UTMZONE,'T',state.get(0,0),state.get(1,0));
      nextLocUTM.setEasting(nextLocUTM.getEasting());
      nextLocUTM.setNorthing(nextLocUTM.getNorthing());
      LocTuple nextLocLL=LocalizerUtil.utm2Deg(nextLocUTM);
      gpsPub.publish(new GpsMeasurement(new Date(),nextLocLL.getLatitude(),true,nextLocLL.getLongitude(),true,0,0,0,0,0,0));
      state=nextStep;
    }
  }
,0,GPS_UPDATE_PERIOD);
  encTimer=new Timer();
  encTimer.scheduleAtFixedRate(new TimerTask(){
    @Override public void run(){
      state=generateNextStep();
      double deltaDist=ENC_UPDATE_PERIOD / 1000.0 * state.get(2,0);
      totalDistance+=deltaDist;
      encPub.publish(new EncoderMeasurement(totalDistance,state.get(2,0)));
    }
  }
,0,ENC_UPDATE_PERIOD);
}