<commit_before>/**
 * Copyright (C) 2009 - 2010 by OpenGamma Inc.
 *
 * Please see distribution for license.
 */
package com.opengamma.math.statistics.leastsquare;

import com.opengamma.math.ConvergenceException;
import com.opengamma.math.UtilFunctions;
import com.opengamma.math.differentiation.ScalarFieldFirstOrderDifferentiator;
import com.opengamma.math.function.ParameterizedFunction;
import com.opengamma.math.linearalgebra.Decomposition;
import com.opengamma.math.linearalgebra.DecompositionResult;
import com.opengamma.math.linearalgebra.LUDecompositionCommons;
import com.opengamma.math.matrix.DoubleMatrix1D;
import com.opengamma.math.matrix.DoubleMatrix2D;
import com.opengamma.math.matrix.DoubleMatrixUtils;
import com.opengamma.math.matrix.MatrixAlgebra;
import com.opengamma.math.matrix.OGMatrixAlgebra;
import com.opengamma.util.ArgumentChecker;

/**
 * 
 */
public class NonLinearLeastSquare {
  private final double[] _x;
  private final double[] _y;
  private final double[] _invSigmaSq;
  private final int _n;
  private final double _eps;
  private final Decomposition<?> _decomposition;
  private final MatrixAlgebra _algebra;

  public NonLinearLeastSquare(final double[] x, final double[] y, final double[] sigma) {
    ArgumentChecker.notNull(x, "x");
    ArgumentChecker.notNull(y, "y");
    ArgumentChecker.notNull(x, "sigma");
    ArgumentChecker.notEmpty(x, "x");
    ArgumentChecker.notEmpty(y, "y");
    ArgumentChecker.notEmpty(sigma, "sigma");

    int n = x.length;
    if (y.length != n) {
      throw new IllegalArgumentException("y wrong length");
    }
    if (sigma.length != n) {
      throw new IllegalArgumentException("sigma wrong length");
    }
    _n = n;
    _x = x;
    _y = y;
    _invSigmaSq = new double[n];
    for (int i = 0; i < n; i++) {
      if (sigma[i] <= 0.0) {
        throw new IllegalArgumentException("invalide sigma");
      }
      _invSigmaSq[i] = 1 / sigma[i] / sigma[i];
    }
    _decomposition = new LUDecompositionCommons();
    _algebra = new OGMatrixAlgebra();
    _eps = 1e-8;
  }

  public LeastSquareResults solve(final ParameterizedFunction<Double, DoubleMatrix1D, Double> func,
      final DoubleMatrix1D startPos) {
    final ScalarFieldFirstOrderDifferentiator diff = new ScalarFieldFirstOrderDifferentiator();

    //if a gradient with respect to the parameters is not supplied, calculate it with finite difference 
    ParameterizedFunction<Double, DoubleMatrix1D, DoubleMatrix1D> grad = new ParameterizedFunction<Double, DoubleMatrix1D, DoubleMatrix1D>() {
      @Override
      public DoubleMatrix1D evaluate(Double x, DoubleMatrix1D parameters) {
        return diff.derivative(func.asFunctionOfParameters(x)).evaluate(parameters);
      }
    };
    return solve(func, grad, startPos);
  }

  public LeastSquareResults solve(final ParameterizedFunction<Double, DoubleMatrix1D, Double> func,
      final ParameterizedFunction<Double, DoubleMatrix1D, DoubleMatrix1D> grad, final DoubleMatrix1D startPos) {

    DoubleMatrix1D theta = startPos;

    double lambda = 0.0; //if the model is linear, it will be solved in 1 step
    double newChiSqr, oldChiSqr;
    double[] modelValues = getModelValues(func, theta);
    double[] newModelValues;
    DoubleMatrix1D[] modelGrads = getModelGradients(grad, theta);

    oldChiSqr = getChiSqr(modelValues);
    DoubleMatrix1D beta = getChiSqrGrad(modelValues, modelGrads);
    double g0 = _algebra.getNorm2(beta);

    for (int count = 0; count < 100; count++) {
      DoubleMatrix2D alpha = getModifiedCurvatureMatrix(modelGrads, lambda);
      DecompositionResult decmp = _decomposition.evaluate(alpha);
      DoubleMatrix1D deltaTheta = decmp.solve(beta);
      DoubleMatrix1D newTheta = (DoubleMatrix1D) _algebra.add(theta, deltaTheta);
      newModelValues = getModelValues(func, newTheta);
      newChiSqr = getChiSqr(newModelValues);
      if (newChiSqr < oldChiSqr) {
        lambda /= 10;
        theta = newTheta;
        modelValues = newModelValues;
        modelGrads = getModelGradients(grad, theta);
        beta = getChiSqrGrad(modelValues, modelGrads);

        //check for convergence
        if (_algebra.getNorm2(beta) < _eps * g0) {
          alpha = getModifiedCurvatureMatrix(modelGrads, 0.0);
          decmp = _decomposition.evaluate(alpha);
          DoubleMatrix2D covariance = decmp.solve(DoubleMatrixUtils.getIdentityMatrix2D(alpha.getNumberOfRows()));
          return new LeastSquareResults(newChiSqr, newTheta, covariance);
        }
        oldChiSqr = newChiSqr;
      } else {
        if (lambda == 0.0) { //this will happen the fist time a full quadratic step fails 
          lambda = 0.01;
        }
        lambda *= 10;
      }
    }
    throw new ConvergenceException("failed to converge");
  }

  private double[] getModelValues(final ParameterizedFunction<Double, DoubleMatrix1D, Double> func,
      final DoubleMatrix1D theta) {
    double[] res = new double[_n];
    for (int i = 0; i < _n; i++) {
      res[i] = func.evaluate(_x[i], theta);
    }
    return res;
  }

  private DoubleMatrix1D[] getModelGradients(final ParameterizedFunction<Double, DoubleMatrix1D, DoubleMatrix1D> grad,
      final DoubleMatrix1D theta) {
    DoubleMatrix1D[] res = new DoubleMatrix1D[_n];
    for (int i = 0; i < _n; i++) {
      res[i] = grad.evaluate(_x[i], theta);
    }
    return res;
  }

  private double getChiSqr(final double[] modelValues) {
    double res = 0.0;
    for (int k = 0; k < _n; k++) {
      res += _invSigmaSq[k] * UtilFunctions.square(_y[k] - modelValues[k]);
    }
    return res;
  }

  private DoubleMatrix1D getChiSqrGrad(final double[] modelValues, final DoubleMatrix1D[] modelGrads) {
    int size = modelGrads[0].getNumberOfElements();

    double[] beta = new double[size];

    for (int i = 0; i < size; i++) {
      double sum = 0.0;
      for (int k = 0; k < _n; k++) {
        sum += _invSigmaSq[k] * (_y[k] - modelValues[k]) * modelGrads[k].getEntry(i);
      }
      beta[i] = sum;
    }
    return new DoubleMatrix1D(beta);
  }

  private DoubleMatrix2D getModifiedCurvatureMatrix(final DoubleMatrix1D[] modelGrads, final double lambda) {
    int size = modelGrads[0].getNumberOfElements();
    DoubleMatrix2D res = new DoubleMatrix2D(size, size);
    double[][] alpha = res.getData();

    for (int i = 0; i < size; i++) {
      double sum = 0.0;
      for (int k = 0; k < _n; k++) {
        sum += _invSigmaSq[k] * UtilFunctions.square(modelGrads[k].getEntry(i));
      }
      alpha[i][i] = (1 + lambda) * sum;

      for (int j = i + 1; j < size; j++) {
        sum = 0.0;
        for (int k = 0; k < _n; k++) {
          sum += _invSigmaSq[k] * modelGrads[k].getEntry(i) * modelGrads[k].getEntry(j);
        }
        alpha[i][j] = sum;
      }
    }
    //alpha is symmetric 
    for (int i = 0; i < size; i++) {
      for (int j = 0; j < i; j++) {
        alpha[i][j] = alpha[j][i];
      }
    }
    return res;
  }

}<commit_msg>[ANA-233]<commit_after>/**
 * Copyright (C) 2009 - 2010 by OpenGamma Inc.
 *
 * Please see distribution for license.
 */
package com.opengamma.math.statistics.leastsquare;

import org.apache.commons.lang.Validate;

import com.opengamma.math.ConvergenceException;
import com.opengamma.math.UtilFunctions;
import com.opengamma.math.differentiation.VectorFieldFirstOrderDifferentiator;
import com.opengamma.math.function.Function1D;
import com.opengamma.math.function.ParameterizedFunction;
import com.opengamma.math.linearalgebra.Decomposition;
import com.opengamma.math.linearalgebra.DecompositionResult;
import com.opengamma.math.linearalgebra.LUDecompositionCommons;
import com.opengamma.math.matrix.DoubleMatrix1D;
import com.opengamma.math.matrix.DoubleMatrix2D;
import com.opengamma.math.matrix.DoubleMatrixUtils;
import com.opengamma.math.matrix.MatrixAlgebra;
import com.opengamma.math.matrix.OGMatrixAlgebra;
import com.opengamma.util.ArgumentChecker;

/**
 * 
 */
public class NonLinearLeastSquare {

  private final double _eps;
  private final Decomposition<?> _decomposition;
  private final MatrixAlgebra _algebra;

  public NonLinearLeastSquare() {
    _decomposition = new LUDecompositionCommons();
    _algebra = new OGMatrixAlgebra();
    _eps = 1e-8;
  }

  /**
   * Use this when the model is in the ParameterizedFunction form and analytic parameter sensitivity is not available 
   * @param x Set of measurement points 
   * @param y Set of measurement values
   * @param sigma y Set of measurement errors 
   * @param func The model in ParameterizedFunction form (i.e. takes a measurement points and a set of parameters and returns a model value)
   * @param startPos Initial value of the parameters 
   * @return A LeastSquareResults object
   */
  public LeastSquareResults solve(final DoubleMatrix1D x, final DoubleMatrix1D y, final DoubleMatrix1D sigma,
      final ParameterizedFunction<Double, DoubleMatrix1D, Double> func, final DoubleMatrix1D startPos) {
    ArgumentChecker.notNull(x, "x");
    ArgumentChecker.notNull(y, "y");
    ArgumentChecker.notNull(x, "sigma");

    int n = x.getNumberOfElements();
    if (y.getNumberOfElements() != n) {
      throw new IllegalArgumentException("y wrong length");
    }
    if (sigma.getNumberOfElements() != n) {
      throw new IllegalArgumentException("sigma wrong length");
    }

    Function1D<DoubleMatrix1D, DoubleMatrix1D> func1D = new Function1D<DoubleMatrix1D, DoubleMatrix1D>() {

      @Override
      public DoubleMatrix1D evaluate(DoubleMatrix1D theta) {
        int m = x.getNumberOfElements();
        double[] res = new double[m];
        for (int i = 0; i < m; i++) {
          res[i] = func.evaluate(x.getEntry(i), theta);
        }
        return new DoubleMatrix1D(res);
      }
    };

    return solve(y, sigma, func1D, startPos);
  }

  /**
   *  Use this when the model is in the ParameterizedFunction form and analytic parameter sensitivity is available 
   * @param x Set of measurement points 
   * @param y Set of measurement values
   * @param sigma Set of measurement errors 
   * @param func The model in ParameterizedFunction form (i.e. takes a measurement points and a set of parameters and returns a model value)
   * @param grad The model parameter sensitivities in  ParameterizedFunction form (i.e. takes a measurement points and a set of parameters and returns a model parameter sensitivities)
   * @param startPos Initial value of the parameters 
   * @return Initial value of the parameters 
   */
  public LeastSquareResults solve(final DoubleMatrix1D x, final DoubleMatrix1D y, final DoubleMatrix1D sigma,
      final ParameterizedFunction<Double, DoubleMatrix1D, Double> func,
      final ParameterizedFunction<Double, DoubleMatrix1D, DoubleMatrix1D> grad, final DoubleMatrix1D startPos) {
    ArgumentChecker.notNull(x, "x");
    ArgumentChecker.notNull(y, "y");
    ArgumentChecker.notNull(x, "sigma");

    int n = x.getNumberOfElements();
    if (y.getNumberOfElements() != n) {
      throw new IllegalArgumentException("y wrong length");
    }
    if (sigma.getNumberOfElements() != n) {
      throw new IllegalArgumentException("sigma wrong length");
    }

    Function1D<DoubleMatrix1D, DoubleMatrix1D> func1D = new Function1D<DoubleMatrix1D, DoubleMatrix1D>() {

      @Override
      public DoubleMatrix1D evaluate(DoubleMatrix1D theta) {
        int m = x.getNumberOfElements();
        double[] res = new double[m];
        for (int i = 0; i < m; i++) {
          res[i] = func.evaluate(x.getEntry(i), theta);
        }
        return new DoubleMatrix1D(res);
      }
    };

    Function1D<DoubleMatrix1D, DoubleMatrix2D> jac = new Function1D<DoubleMatrix1D, DoubleMatrix2D>() {

      @Override
      public DoubleMatrix2D evaluate(DoubleMatrix1D theta) {
        int m = x.getNumberOfElements();
        double[][] res = new double[m][];
        for (int i = 0; i < m; i++) {
          DoubleMatrix1D temp = grad.evaluate(x.getEntry(i), theta);
          res[i] = temp.getData();
        }
        return new DoubleMatrix2D(res);
      }
    };

    return solve(y, sigma, func1D, jac, startPos);
  }

  /**
   *  Use this when the model is given as a function of its parameters only (i.e. a function that takes a set of parameters and return a set of model values,
   *  so the measurement points are already known to the function), and  analytic parameter sensitivity is not available 
   * @param observedValues Set of measurement values
   * @param sigma Set of measurement errors
   * @param func The model as a function of its parameters only
   * @param startPos  Initial value of the parameters 
   * @return Initial value of the parameters 
   */
  public LeastSquareResults solve(final DoubleMatrix1D observedValues, final DoubleMatrix1D sigma,
      final Function1D<DoubleMatrix1D, DoubleMatrix1D> func, final DoubleMatrix1D startPos) {

    final VectorFieldFirstOrderDifferentiator jac = new VectorFieldFirstOrderDifferentiator();
    return solve(observedValues, sigma, func, jac.derivative(func), startPos);
  }

  /**
   * Use this when the model is given as a function of its parameters only (i.e. a function that takes a set of parameters and return a set of model values,
   * so the measurement points are already known to the function), and  analytic parameter sensitivity is available 
   * @param observedValues Set of measurement values
   * @param sigma Set of measurement errors
   * @param func The model as a function of its parameters only
   * @param jac The model sensitivity to its parameters (i.e. the Jacobian matrix) as a function of its parameters only
   * @param startPos  Initial value of the parameters 
   * @return Initial value of the parameters 
   */
  public LeastSquareResults solve(final DoubleMatrix1D observedValues, final DoubleMatrix1D sigma,
      final Function1D<DoubleMatrix1D, DoubleMatrix1D> func, final Function1D<DoubleMatrix1D, DoubleMatrix2D> jac,
      final DoubleMatrix1D startPos) {

    Validate.notNull(observedValues, "observedValues");
    Validate.notNull(sigma, " sigma");
    Validate.notNull(func, " func");
    Validate.notNull(jac, " jac");
    Validate.notNull(startPos, "startPos");
    int n = observedValues.getNumberOfElements();
    Validate.isTrue(n == sigma.getNumberOfElements(), "observedValues and sigma must be same length");
    Validate.isTrue(n >= startPos.getNumberOfElements(),
        "must have data points greater or equal to number of parameters");

    DoubleMatrix1D theta = startPos;

    double lambda = 0.0; //if the model is linear, it will be solved in 1 step
    double newChiSqr, oldChiSqr;
    DoubleMatrix1D error = getError(func, observedValues, sigma, theta);
    DoubleMatrix1D newError;
    DoubleMatrix2D jacobian = getJacobian(jac, sigma, theta);

    oldChiSqr = getChiSqr(error);
    DoubleMatrix1D beta = getChiSqrGrad(error, jacobian);
    double g0 = _algebra.getNorm2(beta);

    for (int count = 0; count < 100; count++) {
      DoubleMatrix2D alpha = getModifiedCurvatureMatrix(jacobian, lambda);
      DecompositionResult decmp = _decomposition.evaluate(alpha);
      DoubleMatrix1D deltaTheta = decmp.solve(beta);
      DoubleMatrix1D newTheta = (DoubleMatrix1D) _algebra.add(theta, deltaTheta);
      newError = getError(func, observedValues, sigma, newTheta);
      newChiSqr = getChiSqr(newError);
      if (newChiSqr < oldChiSqr) {
        lambda /= 10;
        theta = newTheta;
        error = newError;
        jacobian = getJacobian(jac, sigma, newTheta);
        beta = getChiSqrGrad(error, jacobian);

        //check for convergence
        if (_algebra.getNorm2(beta) < _eps * g0) {
          alpha = getModifiedCurvatureMatrix(jacobian, 0.0);
          decmp = _decomposition.evaluate(alpha);
          DoubleMatrix2D covariance = decmp.solve(DoubleMatrixUtils.getIdentityMatrix2D(alpha.getNumberOfRows()));
          return new LeastSquareResults(newChiSqr, newTheta, covariance);
        }
        oldChiSqr = newChiSqr;
      } else {
        if (lambda == 0.0) { //this will happen the fist time a full quadratic step fails 
          lambda = 0.01;
        }
        lambda *= 10;
      }
    }
    throw new ConvergenceException("failed to converge");
  }

  private DoubleMatrix1D getError(final Function1D<DoubleMatrix1D, DoubleMatrix1D> func,
      final DoubleMatrix1D observedValues, final DoubleMatrix1D sigma, final DoubleMatrix1D theta) {
    int n = observedValues.getNumberOfElements();
    DoubleMatrix1D modelValues = func.evaluate(theta);
    if (modelValues.getNumberOfElements() != n) {
      throw new IllegalArgumentException("Number of data points different between model and observed");
    }
    double[] res = new double[n];
    for (int i = 0; i < n; i++) {
      res[i] = (observedValues.getEntry(i) - modelValues.getEntry(i)) / sigma.getEntry(i);
    }

    return new DoubleMatrix1D(res);
  }

  private DoubleMatrix2D getJacobian(Function1D<DoubleMatrix1D, DoubleMatrix2D> jac, DoubleMatrix1D sigma,
      final DoubleMatrix1D theta) {
    DoubleMatrix2D res = jac.evaluate(theta);
    double[][] data = res.getData();
    int m = res.getNumberOfRows();
    int n = res.getNumberOfColumns();
    if (theta.getNumberOfElements() != n || sigma.getNumberOfElements() != m) {
      throw new IllegalArgumentException("Jacobian is wrong size");
    }

    for (int i = 0; i < m; i++) {
      for (int j = 0; j < n; j++) {
        data[i][j] /= sigma.getEntry(i);
      }
    }
    return res;
  }

  private double getChiSqr(final DoubleMatrix1D error) {
    return _algebra.getInnerProduct(error, error);
  }

  private DoubleMatrix1D getChiSqrGrad(final DoubleMatrix1D error, final DoubleMatrix2D jacobian) {
    return (DoubleMatrix1D) _algebra.multiply(error, jacobian);
  }

  private DoubleMatrix2D getModifiedCurvatureMatrix(final DoubleMatrix2D jacobian, final double lambda) {
    int n = jacobian.getNumberOfRows();
    int m = jacobian.getNumberOfColumns();

    DoubleMatrix2D res = new DoubleMatrix2D(m, m);
    double[][] alpha = res.getData();

    for (int i = 0; i < m; i++) {
      double sum = 0.0;
      for (int k = 0; k < n; k++) {
        sum += UtilFunctions.square(jacobian.getEntry(k, i));
      }
      alpha[i][i] = (1 + lambda) * sum;

      for (int j = i + 1; j < m; j++) {
        sum = 0.0;
        for (int k = 0; k < n; k++) {
          sum += jacobian.getEntry(k, i) * jacobian.getEntry(k, j);
        }
        alpha[i][j] = sum;
      }
    }
    //alpha is symmetric 
    for (int i = 0; i < m; i++) {
      for (int j = 0; j < i; j++) {
        alpha[i][j] = alpha[j][i];
      }
    }
    return res;
  }

}
{'fraction_non_alphanumeric': 0.10468245425188374, 'fraction_numerical': 0.013240043057050592, 'mean_word_length': 3.3833451285680587, 'pattern_counts': {'":': 0, '<': 57, '<?xml version=': 0, '>': 27, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1892017', 'n_tokens_mistral': 5977, 'n_tokens_neox': 5629, 'n_words': 1591}