Esempio n. 1
0
	void mexFunction(int nlhs, mxArray* plhs[],
					 int nhrs, const mxArray* prhs[])
	{
		if (nhrs != 5 || nlhs > 3 || nlhs < 2 )
                  DYN_MEX_FUNC_ERR_MSG_TXT("Gensylv: Must have exactly 5 input args and either 2 or 3 output args.");

		int order = (int)mxGetScalar(prhs[0]);
		const mxArray* const A = prhs[1];
		const mxArray* const B = prhs[2];
		const mxArray* const C = prhs[3];
		const mxArray* const D = prhs[4];
		const mwSize* const Adims = mxGetDimensions(A);
		const mwSize* const Bdims = mxGetDimensions(B);
		const mwSize* const Cdims = mxGetDimensions(C);
		const mwSize* const Ddims = mxGetDimensions(D);

                if (Adims[0] != Adims[1])
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix A must be a square matrix.");
                if (Adims[0] != Bdims[0])
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix A and matrix B must have the same number of rows.");
                if (Adims[0] != Ddims[0])
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix A and matrix B must have the same number of rows.");
                if (Cdims[0] != Cdims[1])
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix C must be square.");
                if (Bdims[0] < Bdims[1])
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix B must not have more columns than rows.");
                if (Ddims[1] != (mwSize) power(Cdims[0], order))
                  DYN_MEX_FUNC_ERR_MSG_TXT("Matrix D has wrong number of columns.");

		int n = Adims[0];
		int m = Cdims[0];
		int zero_cols = Bdims[0] - Bdims[1];
		mxArray* X = mxCreateDoubleMatrix(Ddims[0], Ddims[1], mxREAL);
		// copy D to X
		Vector Xvec((double*)mxGetPr(X), power(m, order)*n);
		ConstVector Dvec((double*)mxGetPr(D), power(m, order)*n);
		Xvec = Dvec;
		// solve (or solve and check)
                try
                  {
                    if (nlhs == 2) {
                      gen_sylv_solve(order, n, m, zero_cols,
						   mxGetPr(A), mxGetPr(B), mxGetPr(C),
						   mxGetPr(X));
                    } else if (nlhs == 3) {
                      gen_sylv_solve_and_check(order, n, m, zero_cols,
									 mxGetPr(A), mxGetPr(B), mxGetPr(C),
									 mxGetPr(D), mxGetPr(X), plhs[2]);
                    }
                  }
                catch (const SylvException& e)
                  {
                    char mes[1000];
                    e.printMessage(mes, 999);
                    DYN_MEX_FUNC_ERR_MSG_TXT(mes);
                  }
                plhs[1] = X;
                plhs[0] = mxCreateDoubleScalar(0);
	}
Esempio n. 2
0
/** Perform curve fitting via Levenberg-Marquardt method with optional bounds.
  * \param fxnIn Function to fit.
  * \param Xvals_ target X values (ordinates, M)
  * \param Yvals_ target Y values (coordinates, M)
  * \param ParamVec input parameters(N); at finish contains best esimate of fit parameters
  * \param boundsIn true if parameter has bounds (N)
  * \param lboundIn contain lower bounds for parameter (N)
  * \param uboundIn contain upper bounds for parameter (N)
  * \param weightsIn contain weights for Y values (M)
  * \param tolerance Fit tolerance
  * \param maxIterations Number of iterations to try.
  */
int CurveFit::LevenbergMarquardt(FitFunctionType fxnIn, Darray const& Xvals_,
                                 Darray const& Yvals_, Darray& ParamVec,
                                 std::vector<bool> const& boundsIn,
                                 Darray const& lboundIn, Darray const& uboundIn,
                                 Darray const& weightsIn,
                                 double tolerance, int maxIterations)
{
  int info = 0;
  hasBounds_ = boundsIn;
  Ubound_ = uboundIn;
  Lbound_ = lboundIn;
  Weights_ = weightsIn;
  if (ParametersHaveProblems(Xvals_, Yvals_, ParamVec)) {
    DBGPRINT("Error: %s\n", errorMessage_);
    return info;
  }
  // Set initial parameters
  finalY_ = Yvals_;
  Params_= ParamVec;  // Internal parameter vector
  fParms_ = ParamVec; // Parameters for function evaluation
  Pvec_to_Params( ParamVec );

  fxn_ = fxnIn;
  m_ = Xvals_.size();        // Number of values (rows)
  n_ = Params_.size();       // Number of parameters (cols)
  Darray residual_( m_ );    // Contain Y vals at current Params minus original Y
  // Jacobian matrix/R: m rows by n cols, transposed.
  jacobian_.assign( m_ * n_, 0.0 );

  // For holding diagonal elements for scaling // TODO: Rename
  Darray diag( n_, 0.0 );
  // Workspace arrays
  Darray work1( n_, 0.0 );
  Darray work2( n_, 0.0 );
  Darray newResidual( m_, 0.0 );
  // delta specifies an upper bound on the euclidean norm of d*x
  double delta = 0.0;
  // factor is positive input variable used in determining the initial step
  // bound. This bound is set to the product of factor and the euclidean norm
  // of diag*x if nonzero, or else to factor itself. In most cases factor should
  // lie in the interval (.1,100.). 100. is a generally recommended value.
  // TODO: Make input parameter
  const double factor = 100.0;
  // gtol is a nonnegative input variable. Termination occurs when the cosine 
  // of the angle between residual and any column of the Jacobian is at most 
  // gtol in absolute value. Therefore, gtol measures the orthogonality
  // desired between the function vector and the columns of the Jacobian.
  // TODO: Make input parameter
  const double gtol = 0.0;
  // Smallest possible magnitude
  // TODO: Make Constant?
  const double dwarf = DBL_MIN;
  // Levenberg-Marquard parameter
  double LM_par = 0.0;
  //  xtol is a nonnegative input variable. Termination occurs when the 
  //  relative error between two consecutive iterates is at most xtol.
  //  Therefore, xtol measures the relative error desired in the 
  //  approximate solution.
  // TODO: Make input paramter
  double xtol = 0.0;
  // maxfev is the maxiumum number of allowed function evaluations.
  // NOTE: Included here for complete compatibility with eariler code,
  //       though may not be strictly necessary.
  dsize maxfev = (n_ + 1) * (dsize)maxIterations;

  // Evaluate initial function, obtain residual
  EvaluateFxn( Xvals_, Yvals_, Params_, residual_ );

  // Calculate norm of the residual
  double rnorm = VecNorm( residual_ );
  DBGPRINT("Rnorm= %g\n", rnorm);

  dsize nfev = 1; // Will be set to calls to fxn_. 1 already.

  // MAIN LOOP
  int currentIt = 0;
  while ( currentIt < maxIterations ) {
    DBGPRINT("DEBUG: ----- Iteration %i ------------------------------\n", currentIt+1);
    // Calculate the Jacobian using the forward-difference approximation.
    CalcJacobian_ForwardDiff( Xvals_, Yvals_, Params_, residual_, newResidual );
    PrintMatrix( "Jacobian", n_, m_, jacobian_ );
    nfev += n_;

    // -------------------------------------------
    // | BEGIN QRFAC
    // -------------------------------------------
    // Perform QR factorization of Jacobian via Householder transformations
    // with column pivoting:
    //   J * P = Q * R
    // where J is the Jacobian, P is the permutation matrix, Q is an orthogonal
    // matrix, and R is an upper trapezoidal matrix with diagonal elements of
    // nonincreasing magnitude. The Householder transformation for column k,
    // k = 1,2,...,min(m,n), is of the form:
    //   I = (1/u[k])*u*ut
    // where u has zeros in the first k-1 positions. Adapted from routine qrfac_
    // in Grace 5.1.22 lmdif.c, which in turn is derived from an earlier linpack
    // subroutine.
    DBGPRINT("\nQRFAC ITERATION %i\n", currentIt+1);
    // Rdiag will hold the diagonal elements of R.
    Darray Rdiag(n_, 0.0);
    // Jpvt defines the permutation matrix p such that a*p = q*r. Column j of p
    // is column Jpvt[j] of the identity matrix.
    Iarray Jpvt(n_, 0.0);
    // JcolNorm Will hold the norms of the columns of input Jacobian.
    Darray JcolNorm_(n_, 0.0);
    // Compute the initial column norms and initialize arrays.
    for (dsize in = 0; in != n_; in++) {
      JcolNorm_[in] = VecNorm( jacobian_.begin() + (in * m_), m_ ); 
      Rdiag[in] = JcolNorm_[in];
      work1[in] = Rdiag[in];
      Jpvt[in] = in;
      DBGPRINT("%lu: Rdiag= %12.6g    wa= %12.6g    ipvt= %i\n",
             in+1, Rdiag[in], work1[in], Jpvt[in]+1);
    }

    // Reduce Jacobian to R with Householder transformations.
    dsize min_m_n = std::min( m_, n_ );
    for (dsize in = 0; in != min_m_n; in++) {
      // Bring the column of largest norm into the pivot position.
      dsize kmax = in;
      for (dsize k = in; k != n_; k++) {
        DBGPRINT("\tRdiag[%lu]= %g    Rdiag[%lu]= %g\n", k+1, Rdiag[k], kmax+1, Rdiag[kmax]);
        if (Rdiag[k] > Rdiag[kmax])
          kmax = k;
      }
      DBGPRINT("Elt= %lu    Kmax= %lu\n", in+1, kmax+1);
      if (kmax != in) {
        for (dsize i = 0; i != m_; i++) {
          double temp = jacobian_[i + in * m_];
          jacobian_[i + in   * m_] = jacobian_[i + kmax * m_];
          jacobian_[i + kmax * m_] = temp;
          DBGPRINT("DBG: Swap jac[%lu,%lu] with jac[%lu,%lu]\n", i+1, in+1, i+1, kmax+1);
        }
        Rdiag[kmax] = Rdiag[in];
        work1[kmax] = work1[in];
        dsize k = Jpvt[in];
        Jpvt[in] = Jpvt[kmax];
        Jpvt[kmax] = k;
      }

      // Compute the Householder transformation to reduce the j-th column
      // of Jacobian to a multiple of the j-th unit vector.
      double ajnorm = VecNorm( jacobian_.begin() + (in + in * m_), m_ - in );
      DBGPRINT("#Elt= %lu    mat[%lu]= %g    ajnorm= %g\n",
             m_ - in, in + in * m_, jacobian_[in + in * m_], ajnorm);
      if (ajnorm != 0.0) {
        if (jacobian_[in + in * m_] < 0.0)
          ajnorm = -ajnorm;
        for (dsize im = in; im < m_; im++)
          jacobian_[im + in * m_] /= ajnorm;
        jacobian_[in + in * m_] += 1.0;

        // Apply the transfomation to the remaining columns and update the norms
        dsize in1 = in + 1;
        if (in1 < n_) {
          for (dsize k = in1; k < n_; k++) {
            double sum = 0.0;
            for (dsize i = in; i < m_; i++)
              sum += jacobian_[i + in * m_] * jacobian_[i + k * m_];
            double temp = sum / jacobian_[in + in * m_];
            for (dsize i = in; i < m_; i++)
              jacobian_[i + k * m_] -= temp * jacobian_[i + in * m_];
            if (Rdiag[k] != 0.0) {
              temp = jacobian_[in + k * m_] / Rdiag[k];
              Rdiag[k] *= sqrt( std::max( 0.0, 1.0 - temp * temp ) );
              temp = Rdiag[k] / work1[k];
              DBGPRINT("\t\tQRFAC TEST: 0.5 * %g^2 <= %g\n", temp, machine_epsilon);
              if (0.05 * (temp * temp) <= machine_epsilon) {
                DBGPRINT("\t\tTEST PASSED\n");
                Rdiag[k] = VecNorm( jacobian_.begin() + (in1 + k * m_), m_ - in - 1 );
                work1[k] = Rdiag[k];
              }
            }
            DBGPRINT("QRFAC Rdiag[%lu]= %g\n", k+1, Rdiag[k]);
          }
        }
      }
      Rdiag[in] = -ajnorm;
    }      
    // -------------------------------------------
    // | END QRFAC
    // -------------------------------------------

    PrintMatrix("QR", n_, m_, jacobian_);
    for (dsize in = 0; in != n_; in++)
      DBGPRINT("\tRdiag[%lu]= %12.6g    acnorm[%lu]= %12.6g    ipvt[%lu]= %i\n",
             in+1, Rdiag[in], in+1, JcolNorm_[in], in+1, Jpvt[in]+1);

    double xnorm = 0.0;
    if ( currentIt == 0 ) {
      // First iteration. Scale according to the norms of the columns of
      // the initial Jacobian.
      for (dsize in = 0; in != n_; in++) {
        if ( JcolNorm_[in] == 0.0 )
          diag[in] = 1.0;
        else
          diag[in] = JcolNorm_[in];
      }
      PrintVector("diag", diag);
      // Calculate norm of scaled params and init step bound delta
      for (dsize in = 0; in != n_; in++)
        work1[in] = diag[in] * Params_[in];
      xnorm = VecNorm( work1 );
      delta = factor * xnorm;
      if (delta == 0.0)
        delta = factor;
      DBGPRINT("Delta= %g\n", delta);
    }

    // Form Qt * residual and store in Qt_r. Only first n components of
    // Qt*r are needed.
    Darray Qt_r( n_, 0.0 );
    newResidual = residual_;
    for (dsize in = 0; in != n_; in++) {
      double matElt = jacobian_[in + in * m_];
      DBGPRINT("DEBUG: Element %lu = %g\n", in+1, matElt);
      if (matElt != 0.0) {
        double sum = 0.0;
        for (dsize i = in; i < m_; i++)
          sum += jacobian_[i + in *  m_] * newResidual[i];
        double temp = -sum / matElt;
        for (dsize i = in; i < m_; i++)
          newResidual[i] += jacobian_[i + in * m_] * temp;
      }
      jacobian_[in + in * m_] = Rdiag[in]; 
      DBGPRINT("\tRdiag[%lu]= %g\n", in+1, jacobian_[in + in * m_]);
      Qt_r[in] = newResidual[in];
      DBGPRINT("DEBUG: qtf[%lu]= %g\n", in+1, Qt_r[in]);
    }

    // Compute the norm of the scaled gradient.
    double gnorm = 0.0;
    if ( rnorm > 0.0 ) {
      for (dsize in = 0; in != n_; in++) {
        int l = Jpvt[ in ];
        if (JcolNorm_[in] != 0.0) {
          double sum = 0.0;
          for (dsize i2 = 0; i2 <= in; i2++) {
            sum += jacobian_[i2 + in * m_] * (Qt_r[i2] / rnorm);
            DBGPRINT("DEBUG: jacobian[%lu, %lu]= %g\n", i2+1, in+1, jacobian_[i2 + in * m_]);
          }
          // Determine max
          double d1 = fabs( sum / JcolNorm_[l] );
          gnorm = std::max( gnorm, d1 );
        }
      }
    }
    DBGPRINT("gnorm= %g\n", gnorm);
    // Test for convergence of gradient norm.
    if (gnorm <= gtol) {
      DBGPRINT("Gradient norm %g is less than gtol %g, iteration %i\n",
             gnorm, gtol, currentIt+1);
      info = 4;
      break;
    }

    // Rescale if necessary
    for (dsize in = 0; in != n_; in++)
      diag[in] = std::max( diag[in], JcolNorm_[in] );
    PrintVector("RescaleDiag", diag);
    double Ratio = 0.0;
    while (Ratio < 0.0001) {
      // -----------------------------------------
      // | LMPAR BEGIN
      // -----------------------------------------
      // NOTE: Adapted from lmpar_ in lmdif.c from Grace 5.1.22
      DBGPRINT("\nLMPAR ITERATION %i\n", currentIt+1);
      // Determine the Levenberg-Marquardt parameter.
      Darray Xvec(n_, 0.0);  // Will contain solution to A*x=b, sqrt(par)*D*x=0
      // Compute and store in Xvec the Gauss-Newton direction.
      // If the Jacobian is rank-deficient, obtain a least-squares solution.
      dsize rank = n_;
      for (dsize in = 0; in != n_; in++) {
        work1[in] = Qt_r[in];
        DBGPRINT("jac[%lu,%lu]= %12.6g  rank= %4lu", in+1, in+1, jacobian_[in + in * m_], rank);
        if (jacobian_[in + in * m_] == 0.0 && rank == n_)
          rank = in;
        if (rank < n_)
          work1[in] = 0.0;
        DBGPRINT("   wa1= %12.6g\n", work1[in]);
      }
      DBGPRINT("Final rank= %lu\n", rank);
      // Subtract 1 from rank to use as an index.
      long int rm1 = rank - 1;
      if (rm1 >= 0) {
        for (long int k = 0; k <= rm1; k++) {
          long int in = rm1 - k;
          work1[in] /= jacobian_[in + in * (long int)m_];
          DBGPRINT("wa1[%li] /= %g\n", in+1, jacobian_[in + in * (long int)m_]);
          long int in1 = in - 1;
          if (in1 > -1) {
            double temp = work1[in];
            for (long int i = 0; i <= in1; i++) {
              work1[i] -= jacobian_[i + in * (long int)m_] * temp;
              DBGPRINT("  wa1[%li] -= %g\n", i+1, jacobian_[i + in * (long int)m_]);
            }
          }
        }
      }
      PrintVector("work1", work1);
      for (dsize in = 0; in != n_; in++)
        Xvec[ Jpvt[in] ] = work1[in];
  
      // Evaluate the function at the origin, and test for acceptance of
      // the Gauss-Newton direction.
      for (dsize in = 0; in != n_; in++) {
        DBGPRINT("work2[%lu] = %g * %g\n", in+1, diag[in], Xvec[in]);
        work2[in] = diag[in] * Xvec[in];
      }
      double dxnorm = VecNorm( work2 );
      DBGPRINT("dxnorm= %g\n", dxnorm);
      double fp = dxnorm - delta;
      // Initialize counter for searching for LM parameter
      int lmIterations = 0;
      if (fp > 0.1 * delta) {
        // If the Jacobian is not rank deficient, the Newton step provdes a lower
        // bound, parl, for the zero of the function. Otherwise set this bound to
        // zero
        double parl = 0.0;
        if ( rank >= n_ ) {
          for (dsize in = 0; in != n_; in++) {
            int idx = Jpvt[in];
            work1[in] = diag[idx] * (work2[idx] / dxnorm);
          }
          for (dsize in = 0; in != n_; in++) {
            double sum = 0.0;
            long int in1 = (long int)in - 1;
            if (in1 > -1) {
              for (long int i = 0; i <= in1; i++)
                sum += jacobian_[i + in * m_] * work1[i];
            }
            work1[in] = (work1[in] - sum) / jacobian_[in + in * m_];
          }
          double temp = VecNorm(work1);
          parl = fp / delta / temp / temp;
        }
        DBGPRINT("parl= %g\n",parl);
  
        // Calculate an upper bound, paru, for the zero of the function
        for (dsize in = 0; in != n_; in++) {
          double sum = 0.0;
          for (dsize i = 0; i <= in; i++)
            sum += jacobian_[i + in * m_] * Qt_r[i];
          work1[in] = sum / diag[ Jpvt[in] ];
          DBGPRINT("paru work1[%lu]= %g\n", in+1, work1[in]);
        }
        double w1norm = VecNorm( work1 );
        double paru = w1norm / delta;
        DBGPRINT("paru = %g = %g / %g\n", paru, w1norm, delta);
        if (paru == 0.0)
          paru = dwarf / std::min( delta, 0.1 );
        DBGPRINT("paru= %g\n", paru);
        
        // If the current L-M parameter lies outside of the interval (parl,paru),
        // set par to the closer endpoint.
        LM_par = std::max( LM_par, parl );
        LM_par = std::min( LM_par, paru );
        if (LM_par == 0.0)
          LM_par = w1norm / dxnorm;
  
        // Iteration start.
        bool lmLoop = true;
        while (lmLoop) {
          ++lmIterations;
          DBGPRINT("\t[ lmLoop %i  parl= %g  paru= %g   LM_par= %g ]\n",
                 lmIterations, parl, paru, LM_par);
          // Evaluate the function at the current value of LM_par
          if (LM_par == 0.0)
            LM_par = std::max( dwarf, 0.001 * paru );
          double temp = sqrt( LM_par );
          for (dsize in = 0; in != n_; in++) {
            work1[in] = temp * diag[in];
            DBGPRINT("\tLMPAR work1[%lu]= %g = %g * %g\n", in+1, work1[in], temp, diag[in]);
          }
  
          // -----------------------------------------------
          // | BEGIN QRSOLV
          // -----------------------------------------------
          // NOTE: Adapted from qrsolv_ in lmdif.c from Grace 5.1.22 
          DBGPRINT("\nQRSOLV ITERATION %i\n", lmIterations);
          // This array of length n will hold the diagonal elements of the
          // upper triangular matrix s.
          Darray Sdiag(n_, 0.0);

          // Copy R and Qt_r to preserve input and initialize s.
          for (dsize in = 0; in != n_; in++) {
            for (dsize i = in; i != n_; i++)
              jacobian_[i + in * m_] = jacobian_[in + i * m_];
            Xvec[in] = jacobian_[in + in * m_];
            work2[in] = Qt_r[in];
          }

          // Eliminate the diagonal matrix D using a Givens rotation
          for (dsize in = 0; in != n_; in++) {
            // Prepare the row of D to be eliminated, locating the diagonal 
            // element using p from the QR factorization.
            double diagL = work1[ Jpvt[in] ];
            DBGPRINT("diag[%i] = %g\n", Jpvt[in]+1, diagL);
            if (diagL != 0.0) {
              for (dsize k = in; k < n_; k++)
                Sdiag[k] = 0.0;
              Sdiag[in] = diagL;
              // The transformations to eliminate the row of D modify only
              // a single element of Qt_r beyond the first n, which is 
              // initially zero.
              double qtbpj = 0.0;
              for (dsize k = in; k < n_; k++) {
                // Determine a Givens rotation which eliminates the appropriate
                // element in the current row of D.
                if (Sdiag[k] != 0.0) {
                  double d1 = jacobian_[k + k * m_];
                  double cos, sin;
                  if (fabs(d1) < fabs(Sdiag[k])) {
                    double cotan = jacobian_[k + k * m_] / Sdiag[k];
                    sin = 0.5 / sqrt(0.25 + 0.25 * (cotan * cotan));
                    cos = sin * cotan;
                  } else {
                    double tan = Sdiag[k] / jacobian_[k + k *m_];
                    cos = 0.5 / sqrt(0.25 + 0.25 * (tan * tan));
                    sin = cos * tan;
                  }
                  DBGPRINT("DBG QRsolv: %lu, %lu: cos= %12.6g  sin= %12.6g\n",
                         in+1, k+1, cos, sin);
                  // Compute the modified diagonal element of R and the 
                  // modified element of (Qt_r, 0)
                  jacobian_[k + k * m_] = cos * jacobian_[k + k * m_] + sin * Sdiag[k];
                  double temp = cos * work2[k] + sin * qtbpj; 
                  qtbpj = -sin * work2[k] + cos * qtbpj;
                  work2[k] = temp;
                  DBGPRINT("\twork2[%lu]= %g\n", k+1, work2[k]);
                  // Accumulate the transformation in the row of S
                  dsize kp1 = k + 1;
                  if (kp1 <= n_) {
                    for (dsize i = kp1; i < n_; i++) {
                      temp = cos * jacobian_[i + k * m_] + sin * Sdiag[i];
                      Sdiag[i] = -sin * jacobian_[i + k * m_] + cos * Sdiag[i];
                      jacobian_[i + k * m_] = temp;
                    }
                  }
                }
              }
            }
            // Store the diagonal element of s and restore the corresponding
            // diagonal element of r
            Sdiag[in] = jacobian_[in + in * m_];
            DBGPRINT("QRsolv sdiag[%lu]= %g\n", in+1, Sdiag[in]);
            jacobian_[in + in * m_] = Xvec[in];
          }
  
          // Solve the triangular system for z. if the system is singular,
          // then obtain a least-squares solution.
          dsize u_nsing = n_;
          for (dsize in = 0; in != n_; in++) {
            if (Sdiag[in] == 0.0 && u_nsing == n_)
              u_nsing = in;
            if (u_nsing < n_)
              work2[in] = 0.0;
          }
          DBGPRINT("nsing= %lu\n", u_nsing);
          // Subtract 1 from nsing to use as an index.
          long int nsing = (long int)u_nsing - 1;
          if (nsing >= 0) {
            for (long int k = 0; k <= nsing; k++) {
              long int in = nsing - k;
              double sum = 0.0;
              long int in1 = in + 1;
              if (in1 <= nsing) {
                for (long int i = in1; i <= nsing; i++)
                  sum += jacobian_[i + in * (long int)m_] * work2[i];
              }
              work2[in] = (work2[in] - sum) / Sdiag[in];
            }
          }

          // Permute the components of z back to components of x.
          for (dsize in = 0; in != n_; in++)
            Xvec[ Jpvt[in] ] = work2[in];

          PrintVector("QRsolv Xvec", Xvec);
          PrintVector("sdiag", Sdiag);
          // -----------------------------------------------
          // | END QRSOLV
          // -----------------------------------------------

          for (dsize in = 0; in != n_; in++) {
            work2[in] = diag[in] * Xvec[in];
            DBGPRINT("\twork2[%lu]= %g = %g * %g\n", in+1, work2[in], diag[in], Xvec[in]);
          }
  
          dxnorm = VecNorm( work2 );
          temp = fp;
          fp = dxnorm - delta;
          DBGPRINT("DBG LMPAR: fp = %g = %g - %g\n", fp, dxnorm, delta); 
          // If the function is small enough, accept the current value of LM_par.
          // Also test for the exceptional cases where parl is zero of the number
          // of iterations has reached 10.
          if (fabs(fp) <= 0.1 * delta ||
              (parl == 0.0 && fp <= temp && temp < 0.0) ||
              lmIterations == 10)
          {
            lmLoop = false;
            break;
          }
  
          // Compute the Newton correction.
          for (dsize in = 0; in != n_; in++) {
            int idx = Jpvt[ in ];
            work1[in] = diag[idx] * (work2[idx] / dxnorm);
          }
          for (dsize in = 0; in != n_; in++) {
            work1[in] /= Sdiag[in];
            temp = work1[in];
            dsize in1 = in + 1;
            if (in1 <= n_) {
              for (dsize i = in1; i < n_; i++)
                work1[i] -= jacobian_[i + in * m_] * temp;
            }
          }
          temp = VecNorm( work1 );
          double parc = fp / delta / temp / temp;
          DBGPRINT("parc= %g\n", parc);
          
          // Depending on the sign of the function, update parl or paru
          if (fp > 0.0)
            parl = std::max( parl, LM_par );
          if (fp < 0.0)
            paru = std::min( paru, LM_par );

          // Compute an improved estimate for the parameter
          LM_par = std::max( parl, LM_par + parc );
        }
      }
      DBGPRINT("END LMPAR iter= %i  LM_par= %g\n", lmIterations, LM_par); 
      if (lmIterations == 0)
        LM_par = 0.0;
      // -------------------------------------------
      // | LMPAR END
      // -------------------------------------------
      DBGPRINT("DEBUG: LM_par is %g\n", LM_par);
      // Store the direction Xvec and Param + Xvec. Calculate norm of Param.
      PrintVector("DEBUG: hvec", Xvec);
      for (dsize in = 0; in != n_; in++) {
        Xvec[in] = -Xvec[in];
        work1[in] = Params_[in] + Xvec[in];
        work2[in] = diag[in] * Xvec[in];
      }
      double pnorm = VecNorm( work2 );
      DBGPRINT("pnorm= %g\n", pnorm);

      // On first iteration, adjust initial step bound
      if (currentIt == 0)
        delta = std::min( delta, pnorm );
      DBGPRINT("Delta is now %g\n", delta);

      // Evaluate function at Param + Xvec and calculate its norm
      EvaluateFxn( Xvals_, Yvals_, work1, newResidual );
      ++nfev;
      double rnorm1 = VecNorm( newResidual );
      DBGPRINT("rnorm1= %g\n", rnorm1);

      // Compute the scaled actual reduction
      double actual_reduction = -1.0;
      if ( 0.1 * rnorm1 < rnorm) {
        double d1 = rnorm1 / rnorm;
        actual_reduction = 1.0 - d1 * d1;
      }
      DBGPRINT("actualReduction= %g\n", actual_reduction);

      // Compute the scaled predicted reduction and the scaled directional
      // derivative.
      for (dsize in = 0; in != n_; in++)
      {
        work2[in] = 0.0;
        double temp = Xvec[ Jpvt[in] ];
        for (dsize i = 0; i <= in; i++)
          work2[i] += jacobian_[i + in * m_] * temp;
      }
      double temp1 = VecNorm( work2 ) / rnorm;
      double temp2 = sqrt(LM_par) * pnorm / rnorm;
      DBGPRINT("temp1= %g    temp2= %g\n", temp1, temp2);

      double predicted_reduction = temp1 * temp1 + temp2 * temp2 / 0.5;
      double dirder = -(temp1 * temp1 + temp2 * temp2);
      DBGPRINT("predictedReduction = %12.6g    dirder= %12.6g\n", predicted_reduction, dirder);

      // Compute the ratio of the actial to the predicted reduction.
      if (predicted_reduction != 0.0)
        Ratio = actual_reduction / predicted_reduction;
      DBGPRINT("ratio= %g\n", Ratio);

      // Update the step bound
      if (Ratio <= 0.25) {
        DBGPRINT("Ratio <= 0.25\n");
        double temp;
        if (actual_reduction < 0.0)
          temp = 0.5 * dirder / (dirder + 0.5 * actual_reduction);
        else
          temp = 0.5;
        if ( 0.1 * rnorm1 >= rnorm || temp < 0.1 )
          temp = 0.1;
        DBGPRINT("delta = %g * min( %g, %g )\n", temp, delta, pnorm / 0.1); 
        delta = temp * std::min( delta, pnorm / 0.1 );
        LM_par /= temp;
      } else {
        if (LM_par == 0.0 || Ratio >= 0.75) {
          DBGPRINT("Ratio > 0.25 and (LMpar is zero or Ratio >= 0.75\n");
          delta = pnorm / 0.5;
          LM_par *= 0.5;
        }
      }
      DBGPRINT("LM_par= %g    Delta= %g\n", LM_par, delta);

      if (Ratio >= 0.0001) { 
        // Successful iteration. Update Param, residual, and their norms.
        for (dsize in = 0; in != n_; in++) {
          Params_[in] = work1[in];
          work1[in] = diag[in] * Params_[in];
        }
        for (dsize im = 0; im < m_; im++)
          residual_[im] = newResidual[im];
        xnorm = VecNorm( work1 );
        rnorm = rnorm1;
      }
      
      // Tests for convergence
      if (fabs(actual_reduction) <= tolerance &&
          predicted_reduction <= tolerance &&
          0.5 * Ratio <= 1.0)
        info = 1;
      if (delta <= xtol * xnorm)
        info = 2;  
      if (fabs(actual_reduction) <= tolerance &&
          predicted_reduction <= tolerance &&
          0.5 * Ratio <= 1.0 &&
          info == 2)
        info = 3;
      if (info != 0) break;
                 
      // Tests for stringent tolerance
      if (nfev >= maxfev)
        info = 5;
      if (fabs(actual_reduction) <= machine_epsilon &&
          predicted_reduction <= machine_epsilon &&
          0.5 * Ratio <= 1.0)
        info = 6;
      if (delta <= machine_epsilon * xnorm)
        info = 7;
      if (gnorm <= machine_epsilon)
        info = 8;
      if (info != 0) break;

    } // END inner loop
    if (info != 0) break;
    currentIt++;
  }
  // Final parameters and Y at final parameters
  Params_to_Pvec(ParamVec, Params_);
  fxn_(Xvals_, ParamVec, finalY_);
# ifdef DBG_CURVEFIT
  DBGPRINT("%s\n", Message(info));
  DBGPRINT("Exiting with info value = %i\n", info);
  for (dsize in = 0; in != n_; in++)
    DBGPRINT("\tParams[%lu]= %g\n", in, ParamVec[in]);
# endif
  return info;
}