Program: 175214
Project: 175214
Buggy source:
/** 
 * @param statepVecInterp
 * @param pXYZ
 * @param timeStampInterp
 * @return double array with 2 elements [0]=zeroDopplerTimeSmooth  [1]=sRdistSmooth
 */
public double[] findZeroDoppler(List<double[]> statepVecInterp,double[] pXYZ,double[] timeStampInterp){
  try {
    int iOptFactor=10;
    int nPointsAroundMin=100;
    int nWindowLength=7;
    double nWindowLengthSize=FastMath.floor(nWindowLength / 2);
    int size=statepVecInterp.size();
    int sizeFactor=size / iOptFactor;
    double[][] diffSubsample=new double[sizeFactor][statepVecInterp.get(0).length];
    double[] vDistSubsample=new double[sizeFactor];
    for (int i=0; i < sizeFactor; i++) {
      int pos=i * iOptFactor;
      diffSubsample[i][0]=(statepVecInterp.get(pos)[0] - pXYZ[0]) * (statepVecInterp.get(pos)[0] - pXYZ[0]);
      diffSubsample[i][1]=(statepVecInterp.get(pos)[1] - pXYZ[1]) * (statepVecInterp.get(pos)[1] - pXYZ[1]);
      diffSubsample[i][2]=(statepVecInterp.get(pos)[2] - pXYZ[2]) * (statepVecInterp.get(pos)[2] - pXYZ[2]);
      vDistSubsample[i]=diffSubsample[i][0] + diffSubsample[i][1] + diffSubsample[i][2];
    }
    int idxMinSubsample=0;
    double distMinSubsample=vDistSubsample[0];
    for (int i=1; i < vDistSubsample.length; i++) {
      if (distMinSubsample > vDistSubsample[i]) {
        distMinSubsample=vDistSubsample[i];
        idxMinSubsample=i;
      }
    }
    double idxMin=(idxMinSubsample) * iOptFactor;
    double[] vDist=new double[size];
    for (int i=0; i < vDistSubsample.length; i++) {
      for (int j=0; j < iOptFactor; j++) {
        vDist[(i * iOptFactor) + j]=vDistSubsample[i];
      }
    }
    double iDistWInit=idxMin - nPointsAroundMin;
    if (iDistWInit <= 0)     iDistWInit=1;
    double iDistWEnd=idxMin + nPointsAroundMin;
    if (iDistWEnd > vDist.length)     iDistWEnd=vDist.length;
    int start=((Double)iDistWInit).intValue();
    double[] vDistOptimization=new double[((Double)iDistWEnd).intValue() - start];
    for (int i=start; i < iDistWEnd; i++) {
      double v1=(statepVecInterp.get(i)[0] - pXYZ[0]) * (statepVecInterp.get(i)[0] - pXYZ[0]);
      double v2=(statepVecInterp.get(i)[1] - pXYZ[1]) * (statepVecInterp.get(i)[1] - pXYZ[1]);
      double v3=(statepVecInterp.get(i)[2] - pXYZ[2]) * (statepVecInterp.get(i)[2] - pXYZ[2]);
      vDistOptimization[i - start]=v1 + v2 + v3;
    }
    double w[]=new double[nWindowLength];
    Arrays.fill(w,1.0 / nWindowLength);
    double[] vdistSmooth=MathUtil.linearConvolutionMatlabValid(vDistOptimization,w);
    double distMinSmooth=vdistSmooth[0];
    int idxMinSmoothW=0;
    for (int i=0; i < vdistSmooth.length; i++) {
      if (distMinSmooth > vdistSmooth[i]) {
        distMinSmooth=vdistSmooth[i];
        idxMinSmoothW=i;
      }
    }
    int idxMinSmooth=new Double(idxMinSmoothW + iDistWInit + nWindowLengthSize - 1).intValue();
    double res[]={timeStampInterp[idxMinSmooth],FastMath.sqrt(distMinSmooth)};
    return res;
  }
 catch (  Exception e) {
    logger.error(e.getMessage(),e);
    throw e;
  }
}

Fixed version (ground truth):
/** 
 * @param statepVecInterp
 * @param pXYZ
 * @param timeStampInterp
 * @return double array with 2 elements [0]=zeroDopplerTimeSmooth  [1]=sRdistSmooth
 */
public double[] findZeroDoppler(final List<double[]> statepVecInterp,double[] pXYZ,double[] timeStampInterp){
  try {
    int iOptFactor=10;
    int nPointsAroundMin=100;
    int nWindowLength=7;
    double nWindowLengthSize=FastMath.floor(nWindowLength / 2);
    int size=statepVecInterp.size();
    int sizeFactor=(int)Math.ceil((new Double(size * 1.0 / iOptFactor)));
    double[][] diffSubsample=new double[sizeFactor][statepVecInterp.get(0).length];
    double[] vDistSubsample=new double[sizeFactor];
    for (int i=0; i < sizeFactor; i++) {
      int pos=i * iOptFactor;
      diffSubsample[i][0]=(statepVecInterp.get(pos)[0] - pXYZ[0]) * (statepVecInterp.get(pos)[0] - pXYZ[0]);
      diffSubsample[i][1]=(statepVecInterp.get(pos)[1] - pXYZ[1]) * (statepVecInterp.get(pos)[1] - pXYZ[1]);
      diffSubsample[i][2]=(statepVecInterp.get(pos)[2] - pXYZ[2]) * (statepVecInterp.get(pos)[2] - pXYZ[2]);
      vDistSubsample[i]=diffSubsample[i][0] + diffSubsample[i][1] + diffSubsample[i][2];
    }
    int idxMinSubsample=0;
    double distMinSubsample=vDistSubsample[0];
    for (int i=1; i < vDistSubsample.length; i++) {
      if (distMinSubsample > vDistSubsample[i]) {
        distMinSubsample=vDistSubsample[i];
        idxMinSubsample=i;
      }
    }
    double idxMin=(idxMinSubsample) * iOptFactor;
    double[] vDist=new double[size];
    for (int i=0; i < vDistSubsample.length; i++) {
      for (int j=0; j < iOptFactor && ((i * iOptFactor) + j) < vDist.length; j++) {
        vDist[(i * iOptFactor) + j]=vDistSubsample[i];
      }
    }
    double iDistWInit=idxMin - nPointsAroundMin;
    if (iDistWInit <= 0)     iDistWInit=1;
    double iDistWEnd=idxMin + nPointsAroundMin + 1;
    if (iDistWEnd > vDist.length)     iDistWEnd=vDist.length;
    int start=((Double)iDistWInit).intValue();
    double[] vDistOptimization=new double[((Double)iDistWEnd).intValue() - start];
    for (int i=start; i < iDistWEnd; i++) {
      double v1=(statepVecInterp.get(i)[0] - pXYZ[0]) * (statepVecInterp.get(i)[0] - pXYZ[0]);
      double v2=(statepVecInterp.get(i)[1] - pXYZ[1]) * (statepVecInterp.get(i)[1] - pXYZ[1]);
      double v3=(statepVecInterp.get(i)[2] - pXYZ[2]) * (statepVecInterp.get(i)[2] - pXYZ[2]);
      vDistOptimization[i - start]=v1 + v2 + v3;
    }
    double w[]=new double[nWindowLength];
    Arrays.fill(w,1.0 / nWindowLength);
    double[] vdistSmooth=MathUtil.linearConvolutionMatlabValid(vDistOptimization,w);
    double distMinSmooth=vdistSmooth[0];
    int idxMinSmoothW=0;
    for (int i=0; i < vdistSmooth.length; i++) {
      if (distMinSmooth > vdistSmooth[i]) {
        distMinSmooth=vdistSmooth[i];
        idxMinSmoothW=i;
      }
    }
    int idxMinSmooth=new Double(idxMinSmoothW + iDistWInit + nWindowLengthSize - 1).intValue();
    double res[]={timeStampInterp[idxMinSmooth],FastMath.sqrt(distMinSmooth)};
    return res;
  }
 catch (  Exception e) {
    logger.error(e.getMessage(),e);
    throw e;
  }
}