// Evaluate the function at the starting point and calculate its norm.
evaluationCounter.incrementCount();
//value will be reassigned in the loop
Evaluation current = problem.evaluate(new ArrayRealVector(currentPoint));
double[] currentResiduals = current.getResiduals().toArray();
double currentCost = current.getCost();
// Outer loop.
boolean firstIteration = true;
while (true) {
iterationCounter.incrementCount();
final Evaluation previous = current;
// QR decomposition of the jacobian matrix
final InternalData internalData
= qrDecomposition(current.getJacobian(), solvedCols);
final double[][] weightedJacobian = internalData.weightedJacobian;
final int[] permutation = internalData.permutation;
final double[] diagR = internalData.diagR;
final double[] jacNorm = internalData.jacNorm;
//residuals already have weights applied
double[] weightedResidual = currentResiduals;
for (int i = 0; i < nR; i++) {
qtf[i] = weightedResidual[i];
}
// compute Qt.res
qTy(qtf, internalData);
// now we don't need Q anymore,
// so let jacobian contain the R matrix with its diagonal elements
for (int k = 0; k < solvedCols; ++k) {
int pk = permutation[k];
weightedJacobian[k][pk] = diagR[pk];
}
if (firstIteration) {
// scale the point according to the norms of the columns
// of the initial jacobian
xNorm = 0;
for (int k = 0; k < nC; ++k) {
double dk = jacNorm[k];
if (dk == 0) {
dk = 1.0;
}
double xk = dk * currentPoint[k];
xNorm += xk * xk;
diag[k] = dk;
}
xNorm = FastMath.sqrt(xNorm);
// initialize the step bound delta
delta = (xNorm == 0) ? initialStepBoundFactor : (initialStepBoundFactor * xNorm);
}
// check orthogonality between function vector and jacobian columns
double maxCosine = 0;
if (currentCost != 0) {
for (int j = 0; j < solvedCols; ++j) {
int pj = permutation[j];
double s = jacNorm[pj];
if (s != 0) {
double sum = 0;
for (int i = 0; i <= j; ++i) {
sum += weightedJacobian[i][pj] * qtf[i];
}
maxCosine = FastMath.max(maxCosine, FastMath.abs(sum) / (s * currentCost));
}
}
}
if (maxCosine <= orthoTolerance) {
// Convergence has been reached.
return new OptimumImpl(
current,
evaluationCounter.getCount(),
iterationCounter.getCount());
}
// rescale if necessary
for (int j = 0; j < nC; ++j) {
diag[j] = FastMath.max(diag[j], jacNorm[j]);
}
// Inner loop.
for (double ratio = 0; ratio < 1.0e-4;) {
// save the state
for (int j = 0; j < solvedCols; ++j) {
int pj = permutation[j];
oldX[pj] = currentPoint[pj];
}
final double previousCost = currentCost;
double[] tmpVec = weightedResidual;
weightedResidual = oldRes;
oldRes = tmpVec;
// determine the Levenberg-Marquardt parameter
lmPar = determineLMParameter(qtf, delta, diag,
internalData, solvedCols,
work1, work2, work3, lmDir, lmPar);
// compute the new point and the norm of the evolution direction
double lmNorm = 0;
for (int j = 0; j < solvedCols; ++j) {
int pj = permutation[j];
lmDir[pj] = -lmDir[pj];
currentPoint[pj] = oldX[pj] + lmDir[pj];
double s = diag[pj] * lmDir[pj];
lmNorm += s * s;
}
lmNorm = FastMath.sqrt(lmNorm);
// on the first iteration, adjust the initial step bound.
if (firstIteration) {
delta = FastMath.min(delta, lmNorm);
}
// Evaluate the function at x + p and calculate its norm.
evaluationCounter.incrementCount();
current = problem.evaluate(new ArrayRealVector(currentPoint));
currentResiduals = current.getResiduals().toArray();
currentCost = current.getCost();
// compute the scaled actual reduction
double actRed = -1.0;