levenbergmarquardtestimator.java

Apache的common math数学软件包

      // depending on the sign of the function, update parl or paru.
      if (fp > 0) {
        parl = Math.max(parl, lmPar);
      } else if (fp < 0) {
        paru = Math.min(paru, lmPar);
      }

      // compute an improved estimate for lmPar
      lmPar = Math.max(parl, lmPar + correction);

    }
  }

  /** 
   * Solve a*x = b and d*x = 0 in the least squares sense.
   * <p>This implementation is a translation in Java of the MINPACK
   * <a href="http://www.netlib.org/minpack/qrsolv.f">qrsolv</a>
   * routine.</p>
   * <p>This method sets the lmDir and lmDiag attributes.</p>
   * <p>The authors of the original fortran function are:</p>
   * <ul>
   *   <li>Argonne National Laboratory. MINPACK project. March 1980</li>
   *   <li>Burton  S. Garbow</li>
   *   <li>Kenneth E. Hillstrom</li>
   *   <li>Jorge   J. More</li>
   * </ul>
   * <p>Luc Maisonobe did the Java translation.</p>
   * 
   * @param qy array containing qTy
   * @param diag diagonal matrix
   * @param lmDiag diagonal elements associated with lmDir
   * @param work work array
   */
  private void determineLMDirection(double[] qy, double[] diag,
                                    double[] lmDiag, double[] work) {

    // copy R and Qty to preserve input and initialize s
    //  in particular, save the diagonal elements of R in lmDir
    for (int j = 0; j < solvedCols; ++j) {
      int pj = permutation[j];
      for (int i = j + 1; i < solvedCols; ++i) {
        jacobian[i * cols + pj] = jacobian[j * cols + permutation[i]];
      }
      lmDir[j] = diagR[pj];
      work[j]  = qy[j];
    }

    // eliminate the diagonal matrix d using a Givens rotation
    for (int j = 0; j < solvedCols; ++j) {

      // prepare the row of d to be eliminated, locating the
      // diagonal element using p from the Q.R. factorization
      int pj = permutation[j];
      double dpj = diag[pj];
      if (dpj != 0) {
        Arrays.fill(lmDiag, j + 1, lmDiag.length, 0);
      }
      lmDiag[j] = dpj;

      //  the transformations to eliminate the row of d
      // modify only a single element of Qty
      // beyond the first n, which is initially zero.
      double qtbpj = 0;
      for (int k = j; k < solvedCols; ++k) {
        int pk = permutation[k];

        // determine a Givens rotation which eliminates the
        // appropriate element in the current row of d
        if (lmDiag[k] != 0) {

          double sin, cos;
          double rkk = jacobian[k * cols + pk];
          if (Math.abs(rkk) < Math.abs(lmDiag[k])) {
            double cotan = rkk / lmDiag[k];
            sin   = 1.0 / Math.sqrt(1.0 + cotan * cotan);
            cos   = sin * cotan;
          } else {
            double tan = lmDiag[k] / rkk;
            cos = 1.0 / Math.sqrt(1.0 + tan * tan);
            sin = cos * tan;
          }

          // compute the modified diagonal element of R and
          // the modified element of (Qty,0)
          jacobian[k * cols + pk] = cos * rkk + sin * lmDiag[k];
          double temp = cos * work[k] + sin * qtbpj;
          qtbpj = -sin * work[k] + cos * qtbpj;
          work[k] = temp;

          // accumulate the tranformation in the row of s
          for (int i = k + 1; i < solvedCols; ++i) {
            double rik = jacobian[i * cols + pk];
            temp = cos * rik + sin * lmDiag[i];
            lmDiag[i] = -sin * rik + cos * lmDiag[i];
            jacobian[i * cols + pk] = temp;
          }

        }
      }

      // store the diagonal element of s and restore
      // the corresponding diagonal element of R
      int index = j * cols + permutation[j];
      lmDiag[j]       = jacobian[index];
      jacobian[index] = lmDir[j];

    }

    // solve the triangular system for z, if the system is
    // singular, then obtain a least squares solution
    int nSing = solvedCols;
    for (int j = 0; j < solvedCols; ++j) {
      if ((lmDiag[j] == 0) && (nSing == solvedCols)) {
        nSing = j;
      }
      if (nSing < solvedCols) {
        work[j] = 0;
      }
    }
    if (nSing > 0) {
      for (int j = nSing - 1; j >= 0; --j) {
        int pj = permutation[j];
        double sum = 0;
        for (int i = j + 1; i < nSing; ++i) {
          sum += jacobian[i * cols + pj] * work[i];
        }
        work[j] = (work[j] - sum) / lmDiag[j];
      }
    }

    // permute the components of z back to components of lmDir
    for (int j = 0; j < lmDir.length; ++j) {
      lmDir[permutation[j]] = work[j];
    }

  }

  /** 
   * Decompose a matrix A as A.P = Q.R using Householder transforms.
   * <p>As suggested in the P. Lascaux and R. Theodor book
   * <i>Analyse num&eacute;rique matricielle appliqu&eacute;e &agrave;
   * l'art de l'ing&eacute;nieur</i> (Masson, 1986), instead of representing
   * the Householder transforms with u<sub>k</sub> unit vectors such that:
   * <pre>
   * H<sub>k</sub> = I - 2u<sub>k</sub>.u<sub>k</sub><sup>t</sup>
   * </pre>
   * we use <sub>k</sub> non-unit vectors such that:
   * <pre>
   * H<sub>k</sub> = I - beta<sub>k</sub>v<sub>k</sub>.v<sub>k</sub><sup>t</sup>
   * </pre>
   * where v<sub>k</sub> = a<sub>k</sub> - alpha<sub>k</sub> e<sub>k</sub>.
   * The beta<sub>k</sub> coefficients are provided upon exit as recomputing
   * them from the v<sub>k</sub> vectors would be costly.</p>
   * <p>This decomposition handles rank deficient cases since the tranformations
   * are performed in non-increasing columns norms order thanks to columns
   * pivoting. The diagonal elements of the R matrix are therefore also in
   * non-increasing absolute values order.</p>
   */
  private void qrDecomposition() {

    // initializations
    for (int k = 0; k < cols; ++k) {
      permutation[k] = k;
      double norm2 = 0;
      for (int index = k; index < jacobian.length; index += cols) {
        double akk = jacobian[index];
        norm2 += akk * akk;
      }
      jacNorm[k] = Math.sqrt(norm2);
    }

    // transform the matrix column after column
    for (int k = 0; k < cols; ++k) {

      // select the column with the greatest norm on active components
      int nextColumn = -1;
      double ak2 = Double.NEGATIVE_INFINITY;
      for (int i = k; i < cols; ++i) {
        double norm2 = 0;
        int iDiag = k * cols + permutation[i];
        for (int index = iDiag; index < jacobian.length; index += cols) {
          double aki = jacobian[index];
          norm2 += aki * aki;
        }
        if (norm2 > ak2) {
          nextColumn = i;
          ak2        = norm2;
        }
      }
      if (ak2 == 0) {
        rank = k;
        return;
      }
      int pk                  = permutation[nextColumn];
      permutation[nextColumn] = permutation[k];
      permutation[k]          = pk;

      // choose alpha such that Hk.u = alpha ek
      int    kDiag = k * cols + pk;
      double akk   = jacobian[kDiag];
      double alpha = (akk > 0) ? -Math.sqrt(ak2) : Math.sqrt(ak2);
      double betak = 1.0 / (ak2 - akk * alpha);
      beta[pk]     = betak;

      // transform the current column
      diagR[pk]        = alpha;
      jacobian[kDiag] -= alpha;

      // transform the remaining columns
      for (int dk = cols - 1 - k; dk > 0; --dk) {
        int dkp = permutation[k + dk] - pk;
        double gamma = 0;
        for (int index = kDiag; index < jacobian.length; index += cols) {
          gamma += jacobian[index] * jacobian[index + dkp];
        }
        gamma *= betak;
        for (int index = kDiag; index < jacobian.length; index += cols) {
          jacobian[index + dkp] -= gamma * jacobian[index];
        }
      }

    }

    rank = solvedCols;

  }

  /** 
   * Compute the product Qt.y for some Q.R. decomposition.
   * 
   * @param y vector to multiply (will be overwritten with the result)
   */
  private void qTy(double[] y) {
    for (int k = 0; k < cols; ++k) {
      int pk = permutation[k];
      int kDiag = k * cols + pk;
      double gamma = 0;
      for (int i = k, index = kDiag; i < rows; ++i, index += cols) {
        gamma += jacobian[index] * y[i];
      }
      gamma *= beta[pk];
      for (int i = k, index = kDiag; i < rows; ++i, index += cols) {
        y[i] -= gamma * jacobian[index];
      }
    }
  }

  /** Number of solved variables. */
  private int solvedCols;

  /** Diagonal elements of the R matrix in the Q.R. decomposition. */
  private double[] diagR;

  /** Norms of the columns of the jacobian matrix. */
  private double[] jacNorm;

  /** Coefficients of the Householder transforms vectors. */
  private double[] beta;

  /** Columns permutation array. */
  private int[] permutation;

  /** Rank of the jacobian matrix. */
  private int rank;

  /** Levenberg-Marquardt parameter. */
  private double lmPar;

  /** Parameters evolution direction associated with lmPar. */
  private double[] lmDir;

  /** Positive input variable used in determining the initial step bound. */
  private double initialStepBoundFactor;

  /** Desired relative error in the sum of squares. */
  private double costRelativeTolerance;

  /**  Desired relative error in the approximate solution parameters. */
  private double parRelativeTolerance;

  /** Desired max cosine on the orthogonality between the function vector
   * and the columns of the jacobian. */
  private double orthoTolerance;

  /** Serializable version identifier */
  private static final long serialVersionUID = -5705952631533171019L;

}