Program: 42669
Project: 42669
Buggy source:
public KfLocalizer(int period){
  super(new BuggyBaseNode(NodeChannel.POSE),period,"String_Node_Str");
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  wheelBase=1.13;
  double[][] startCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  covariance=new Matrix(startCovariance);
  LocTuple startLatLng=new LocTuple(40.4416651,-79.9437577);
  UTMTuple startUTM=LocalizerUtil.Deg2UTM(startLatLng);
  lastGPS=startUTM;
  lastLastGPS=startUTM;
  lastEncoderReadingTime=new Date().getTime();
  mostRecentUpdateTime=new Date();
  double[][] start={{startUTM.Easting},{startUTM.Northing},{0},{0},{-110},{0},{0}};
  state=new Matrix(start);
  double[][] predictCovarianceArray={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  predictCovariance=new Matrix(predictCovarianceArray);
  new Subscriber("String_Node_Str",NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
      LocTuple gpsLatLng=new LocTuple(newGPSData.getLatitude(),newGPSData.getLongitude());
      UTMTuple gpsUTM=LocalizerUtil.Deg2UTM(gpsLatLng);
      double dx=gpsUTM.Easting - lastGPS.Easting;
      double dy=gpsUTM.Northing - lastGPS.Northing;
      double th=Math.toDegrees(Math.atan2(dy,dx));
      lastLastGPS=lastGPS;
      lastGPS=gpsUTM;
      double[][] observationModel={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
      if (Math.sqrt(dx * dx + dy * dy) < .5) {
        observationModel[4][4]=0;
      }
      double[][] meassurement={{gpsUTM.Easting},{gpsUTM.Northing},{0},{0},{th},{0},{0}};
      double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
      updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.IMU_ANG_POS.getMsgPath(),((topicName,m) -> {
    IMUAngularPositionMessage mes=((IMUAngularPositionMessage)m);
    double[][] xVar={{1},{0},{0}};
    double[][] yVar={{0},{1},{0}};
    Matrix xMat=new Matrix(xVar);
    Matrix yMat=new Matrix(yVar);
    Matrix rot=new Matrix(mes.getRot());
    double x=rot.times(xMat).get(0,0);
    double y=rot.times(yMat).get(0,0);
    double th=-(Math.toDegrees(Math.atan2(y,x)) - 90);
    double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
    double[][] meassurement={{0},{0},{0},{0},{th},{0},{0}};
    double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  }
));
  new Subscriber("String_Node_Str",NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public synchronized void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement measurement=(EncoderMeasurement)m;
      double currentEncoderMeasurement=measurement.getDistance();
      double deltaDistance=currentEncoderMeasurement - lastEncoderReading;
      long currentTime=new Date().getTime();
      long dt=currentTime - lastEncoderReadingTime;
      if (dt > 1) {
        double bodySpeed=deltaDistance / (dt / 1000.0);
        lastEncoderReadingTime=currentTime;
        lastEncoderReading=currentEncoderMeasurement;
        double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
        double[][] meassurement={{0},{0},{bodySpeed},{0},{0},{0},{0}};
        double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
        updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
      }
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public synchronized void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,1}};
      double[][] meassurement={{0},{0},{0},{0},{0},{0},{steerM.getAngle()}};
      double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
      updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
    }
  }
);
  resume();
}

Fixed version (ground truth):
public KfLocalizer(int period){
  super(new BuggyBaseNode(NodeChannel.POSE),period,"String_Node_Str");
  posePub=new Publisher(NodeChannel.POSE.getMsgPath());
  wheelBase=1.13;
  double[][] startCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  covariance=new Matrix(startCovariance);
  LocTuple startLatLng=new LocTuple(40.4416651,-79.9437577);
  UTMTuple startUTM=LocalizerUtil.Deg2UTM(startLatLng);
  lastGPS=startUTM;
  lastLastGPS=startUTM;
  lastEncoderReadingTime=new Date().getTime();
  mostRecentUpdateTime=new Date();
  double[][] start={{startUTM.Easting},{startUTM.Northing},{0},{0},{-110},{0},{0}};
  state=new Matrix(start);
  double[][] predictCovarianceArray={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  predictCovariance=new Matrix(predictCovarianceArray);
  new Subscriber("String_Node_Str",NodeChannel.GPS.getMsgPath(),new MessageListener(){
    @Override public void actionPerformed(    String topicName,    Message m){
      GpsMeasurement newGPSData=(GpsMeasurement)m;
      LocTuple gpsLatLng=new LocTuple(newGPSData.getLatitude(),newGPSData.getLongitude());
      UTMTuple gpsUTM=LocalizerUtil.Deg2UTM(gpsLatLng);
      double dx=gpsUTM.Easting - lastGPS.Easting;
      double dy=gpsUTM.Northing - lastGPS.Northing;
      double th=Math.toDegrees(Math.atan2(dy,dx));
      lastLastGPS=lastGPS;
      lastGPS=gpsUTM;
      System.out.println(th);
      double[][] observationModel={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
      if (Math.sqrt(dx * dx + dy * dy) < .5) {
        observationModel[4][4]=0;
      }
      double[][] meassurement={{gpsUTM.Easting},{gpsUTM.Northing},{0},{0},{th},{0},{0}};
      double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
      updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.IMU_ANG_POS.getMsgPath(),((topicName,m) -> {
    IMUAngularPositionMessage mes=((IMUAngularPositionMessage)m);
    double[][] xVar={{1},{0},{0}};
    double[][] yVar={{0},{1},{0}};
    Matrix xMat=new Matrix(xVar);
    Matrix yMat=new Matrix(yVar);
    Matrix rot=new Matrix(mes.getRot());
    double x=rot.times(xMat).get(0,0);
    double y=rot.times(yMat).get(0,0);
    double th=-(Math.toDegrees(Math.atan2(y,x)) - 90);
    double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
    double[][] meassurement={{0},{0},{0},{0},{th},{0},{0}};
    double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
  }
));
  new Subscriber("String_Node_Str",NodeChannel.ENCODER.getMsgPath(),new MessageListener(){
    @Override public synchronized void actionPerformed(    String topicName,    Message m){
      EncoderMeasurement measurement=(EncoderMeasurement)m;
      double currentEncoderMeasurement=measurement.getDistance();
      double deltaDistance=currentEncoderMeasurement - lastEncoderReading;
      long currentTime=new Date().getTime();
      long dt=currentTime - lastEncoderReadingTime;
      if (dt > 1) {
        double bodySpeed=deltaDistance / (dt / 1000.0);
        lastEncoderReadingTime=currentTime;
        lastEncoderReading=currentEncoderMeasurement;
        double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0}};
        double[][] meassurement={{0},{0},{bodySpeed},{0},{0},{0},{0}};
        double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
        updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
      }
    }
  }
);
  new Subscriber("String_Node_Str",NodeChannel.STEERING.getMsgPath(),new MessageListener(){
    @Override public synchronized void actionPerformed(    String topicName,    Message m){
      SteeringMeasurement steerM=(SteeringMeasurement)m;
      double[][] observationModel={{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,0},{0,0,0,0,0,0,1}};
      double[][] meassurement={{0},{0},{0},{0},{0},{0},{steerM.getAngle()}};
      double[][] updateCovariance={{1,0,0,0,0,0,0},{0,1,0,0,0,0,0},{0,0,1,0,0,0,0},{0,0,0,1,0,0,0},{0,0,0,0,1,0,0},{0,0,0,0,0,1,0},{0,0,0,0,0,0,1}};
      updateStep(new Matrix(observationModel),new Matrix(meassurement),new Matrix(updateCovariance));
    }
  }
);
  resume();
}