Program: 53160
Project: 53160
Buggy source:
@Override public void process(double time,Map<String,Object> update){
  boolean act=false;
  Number roll=(Number)update.get(param_Fields_Att[0]);
  Number pitch=(Number)update.get(param_Fields_Att[1]);
  Number yaw=(Number)update.get(param_Fields_Att[2]);
  if (roll != null && pitch != null && yaw != null) {
    rot=RotationConversion.rotationMatrixByEulerAngles(roll.doubleValue(),pitch.doubleValue(),yaw.doubleValue());
    act=true;
  }
  Number baroNum=(Number)update.get(param_Field_Baro);
  if (baroNum != null) {
    baro=baroNum.doubleValue();
    if (!baroInited) {
      baroInited=true;
      est[2][0]=-baro;
    }
    corrBaro=-baro - baroOffset - est[2][0];
  }
  Number latNum=(Number)update.get(param_Fields_GPS[0]);
  Number lonNum=(Number)update.get(param_Fields_GPS[1]);
  Number altNum=(Number)update.get(param_Fields_GPS[2]);
  Number[] velGPSNum=new Number[]{(Number)update.get(param_Fields_GPS[3]),(Number)update.get(param_Fields_GPS[4]),(Number)update.get(param_Fields_GPS[5])};
  Number ephNum=(Number)update.get(param_Fields_GPS[6]);
  Number epvNum=(Number)update.get(param_Fields_GPS[7]);
  if (latNum != null && lonNum != null && altNum != null && velGPSNum[0] != null && velGPSNum[1] != null && velGPSNum[2] != null) {
    double lat=latNum.doubleValue();
    double lon=lonNum.doubleValue();
    double eph=ephNum.doubleValue();
    double epv=epvNum.doubleValue();
    double alt=altNum.doubleValue();
    if (!gpsInited && baroInited) {
      gpsInited=true;
      positionProjector.init(new LatLonAlt(lat,lon,alt));
      est[2][0]=-alt;
      baroOffset=alt - baro;
    }
    if (gpsInited) {
      double[] gpsProj=positionProjector.project(new LatLonAlt(lat,lon,alt));
      gps[0][0]=gpsProj[0];
      gps[1][0]=gpsProj[1];
      gps[2][0]=gpsProj[2];
      for (int axis=0; axis < 3; axis++) {
        gps[axis][1]=velGPSNum[axis].doubleValue();
      }
      double[][] outOld=delayLineGPS.getOutput(time);
      if (outOld != null) {
        for (int axis=0; axis < 3; axis++) {
          for (int posVel=0; posVel < 2; posVel++) {
            corrGPS[axis][posVel]=gps[axis][posVel] - outOld[axis][posVel];
          }
        }
      }
      wGPS[0]=2.0 / Math.max(2.0,eph);
      wGPS[1]=wGPS[0];
      wGPS[2]=4.0 / Math.max(4.0,epv);
      act=true;
    }
  }
  Number accX=(Number)update.get(param_Fields_Acc[0]);
  Number accY=(Number)update.get(param_Fields_Acc[1]);
  Number accZ=(Number)update.get(param_Fields_Acc[2]);
  if (accX != null && accY != null && accZ != null) {
    acc.set(0,accX.doubleValue());
    acc.set(1,accY.doubleValue());
    acc.set(2,accZ.doubleValue());
    acc.subtract(accBias);
    act=true;
  }
  if (act) {
    if (!Double.isNaN(timePrev)) {
      double dt=time - timePrev;
      double dBaro=corrGPS[2][0] * param_W_GPS[2][0] * wGPS[2]* dt;
      baroOffset-=dBaro;
      corrBaro+=dBaro;
      Vector accBiasCorrV=new BasicVector(3);
      for (int axis=0; axis < 3; axis++) {
        double wPos=param_W_GPS[axis][0] * param_W_GPS[axis][0] * wGPS[axis]* wGPS[axis];
        double wVel=param_W_GPS[axis][1] * wGPS[axis];
        accBiasCorrV.set(axis,-corrGPS[axis][0] * wPos - corrGPS[axis][1] * wVel);
      }
      accBiasCorrV.set(2,-corrBaro * param_W_Baro * param_W_Baro);
      Vector b=rot.transpose().multiply(accBiasCorrV).multiply(param_W_Acc_Bias * dt);
      Vector accNED=rot.multiply(acc);
      accNED.set(2,accNED.get(2) + G);
      accBias.addInPlace(b);
      predict(est,dt,new double[]{accNED.get(0),accNED.get(1),accNED.get(2)});
      for (int axis=0; axis < 3; axis++) {
        correct(est[axis],dt,0,corrGPS[axis][0],param_W_GPS[axis][0] * wGPS[axis]);
        correct(est[axis],dt,1,corrGPS[axis][1],param_W_GPS[axis][1] * wGPS[axis]);
      }
      correct(est[2],dt,0,corrBaro,param_W_Baro);
      delayLineGPS.getOutput(time,deepCopy(est));
      if (gpsInited && baroInited) {
        int seriesIdx=0;
        for (int axis=0; axis < 3; axis++) {
          if (show[axis]) {
            addPoint(seriesIdx++,time,est[axis][0] * scales[axis] + offsets[axis]);
            addPoint(seriesIdx++,time,est[axis][1] * scales[axis]);
            addPoint(seriesIdx++,time,corrGPS[axis][1]);
          }
        }
      }
    }
    timePrev=time;
  }
}

Fixed version (ground truth):
@Override public void process(double time,Map<String,Object> update){
  boolean act=false;
  Number roll=(Number)update.get(param_Fields_Att[0]);
  Number pitch=(Number)update.get(param_Fields_Att[1]);
  Number yaw=(Number)update.get(param_Fields_Att[2]);
  if (roll != null && pitch != null && yaw != null) {
    rot=RotationConversion.rotationMatrixByEulerAngles(roll.doubleValue(),pitch.doubleValue(),yaw.doubleValue());
    act=true;
  }
  Number baroNum=(Number)update.get(param_Field_Baro);
  if (baroNum != null) {
    baro=baroNum.doubleValue();
    if (!baroInited) {
      baroInited=true;
      est[2][0]=-baro;
    }
    corrBaro=-baro - baroOffset - est[2][0];
  }
  Number latNum=(Number)update.get(param_Fields_GPS[0]);
  Number lonNum=(Number)update.get(param_Fields_GPS[1]);
  Number altNum=(Number)update.get(param_Fields_GPS[2]);
  Number[] velGPSNum=new Number[]{(Number)update.get(param_Fields_GPS[3]),(Number)update.get(param_Fields_GPS[4]),(Number)update.get(param_Fields_GPS[5])};
  Number ephNum=(Number)update.get(param_Fields_GPS[6]);
  Number epvNum=(Number)update.get(param_Fields_GPS[7]);
  if (latNum != null && lonNum != null && altNum != null && velGPSNum[0] != null && velGPSNum[1] != null && velGPSNum[2] != null) {
    double lat=latNum.doubleValue();
    double lon=lonNum.doubleValue();
    double eph=ephNum.doubleValue();
    double epv=epvNum.doubleValue();
    double alt=altNum.doubleValue();
    if (!gpsInited && baroInited) {
      gpsInited=true;
      positionProjector.init(new LatLonAlt(lat,lon,alt));
      est[2][0]=-alt;
      baroOffset=alt - baro;
    }
    if (gpsInited) {
      double[] gpsProj=positionProjector.project(new LatLonAlt(lat,lon,alt));
      gps[0][0]=gpsProj[0];
      gps[1][0]=gpsProj[1];
      gps[2][0]=gpsProj[2];
      for (int axis=0; axis < 3; axis++) {
        gps[axis][1]=velGPSNum[axis].doubleValue();
      }
      double[][] outOld=delayLineGPS.getOutput(time);
      if (outOld != null) {
        for (int axis=0; axis < 3; axis++) {
          for (int posVel=0; posVel < 2; posVel++) {
            corrGPS[axis][posVel]=gps[axis][posVel] - outOld[axis][posVel];
          }
        }
      }
      wGPS[0]=2.0 / Math.max(2.0,eph);
      wGPS[1]=wGPS[0];
      wGPS[2]=4.0 / Math.max(4.0,epv);
      act=true;
    }
  }
  Number accX=(Number)update.get(param_Fields_Acc[0]);
  Number accY=(Number)update.get(param_Fields_Acc[1]);
  Number accZ=(Number)update.get(param_Fields_Acc[2]);
  if (accX != null && accY != null && accZ != null) {
    acc.set(0,accX.doubleValue());
    acc.set(1,accY.doubleValue());
    acc.set(2,accZ.doubleValue());
    acc.subtract(accBias);
    act=true;
  }
  if (act) {
    if (!Double.isNaN(timePrev) && rot != null) {
      double dt=time - timePrev;
      double dBaro=corrGPS[2][0] * param_W_GPS[2][0] * wGPS[2]* dt;
      baroOffset-=dBaro;
      corrBaro+=dBaro;
      Vector accBiasCorrV=new BasicVector(3);
      for (int axis=0; axis < 3; axis++) {
        double wPos=param_W_GPS[axis][0] * param_W_GPS[axis][0] * wGPS[axis]* wGPS[axis];
        double wVel=param_W_GPS[axis][1] * wGPS[axis];
        accBiasCorrV.set(axis,-corrGPS[axis][0] * wPos - corrGPS[axis][1] * wVel);
      }
      accBiasCorrV.set(2,-corrBaro * param_W_Baro * param_W_Baro);
      Vector b=rot.transpose().multiply(accBiasCorrV).multiply(param_W_Acc_Bias * dt);
      Vector accNED=rot.multiply(acc);
      accNED.set(2,accNED.get(2) + G);
      accBias.addInPlace(b);
      predict(est,dt,new double[]{accNED.get(0),accNED.get(1),accNED.get(2)});
      for (int axis=0; axis < 3; axis++) {
        correct(est[axis],dt,0,corrGPS[axis][0],param_W_GPS[axis][0] * wGPS[axis]);
        correct(est[axis],dt,1,corrGPS[axis][1],param_W_GPS[axis][1] * wGPS[axis]);
      }
      correct(est[2],dt,0,corrBaro,param_W_Baro);
      delayLineGPS.getOutput(time,deepCopy(est));
      if (gpsInited && baroInited) {
        int seriesIdx=0;
        for (int axis=0; axis < 3; axis++) {
          if (show[axis]) {
            addPoint(seriesIdx++,time,est[axis][0] * scales[axis] + offsets[axis]);
            addPoint(seriesIdx++,time,est[axis][1] * scales[axis]);
            addPoint(seriesIdx++,time,corrGPS[axis][1]);
          }
        }
      }
    }
    timePrev=time;
  }
}