updateResidualsAndCost();
// outer loop
lmPar = 0;
boolean firstIteration = true;
VectorialPointValuePair current = new VectorialPointValuePair(point, objective);
while (true) {
for (int i=0;i<rows;i++) {
qtf[i]=wresiduals[i];
}
incrementIterationsCounter();
// compute the Q.R. decomposition of the jacobian matrix
VectorialPointValuePair previous = current;
updateJacobian();
qrDecomposition();
// compute Qt.res
qTy(qtf);
// 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];
wjacobian[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 < cols; ++k) {
double dk = jacNorm[k];
if (dk == 0) {
dk = 1.0;
}
double xk = dk * point[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 (cost != 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 += wjacobian[i][pj] * qtf[i];
}
maxCosine = FastMath.max(maxCosine, FastMath.abs(sum) / (s * cost));
}
}
}
if (maxCosine <= orthoTolerance) {
// convergence has been reached
updateResidualsAndCost();
current = new VectorialPointValuePair(point, objective);
return current;
}
// rescale if necessary
for (int j = 0; j < cols; ++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] = point[pj];
}
double previousCost = cost;
double[] tmpVec = residuals;
residuals = oldRes;
oldRes = tmpVec;
tmpVec = objective;
objective = oldObj;
oldObj = tmpVec;
// determine the Levenberg-Marquardt parameter
determineLMParameter(qtf, delta, diag, work1, work2, work3);
// 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];
point[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
updateResidualsAndCost();
// compute the scaled actual reduction
double actRed = -1.0;
if (0.1 * cost < previousCost) {
double r = cost / previousCost;
actRed = 1.0 - r * r;
}
// compute the scaled predicted reduction
// and the scaled directional derivative
for (int j = 0; j < solvedCols; ++j) {
int pj = permutation[j];
double dirJ = lmDir[pj];
work1[j] = 0;
for (int i = 0; i <= j; ++i) {
work1[i] += wjacobian[i][pj] * dirJ;
}
}
double coeff1 = 0;
for (int j = 0; j < solvedCols; ++j) {
coeff1 += work1[j] * work1[j];
}
double pc2 = previousCost * previousCost;
coeff1 = coeff1 / pc2;
double coeff2 = lmPar * lmNorm * lmNorm / pc2;
double preRed = coeff1 + 2 * coeff2;
double dirDer = -(coeff1 + coeff2);
// ratio of the actual to the predicted reduction
ratio = (preRed == 0) ? 0 : (actRed / preRed);
// update the step bound
if (ratio <= 0.25) {
double tmp =
(actRed < 0) ? (0.5 * dirDer / (dirDer + 0.5 * actRed)) : 0.5;
if ((0.1 * cost >= previousCost) || (tmp < 0.1)) {
tmp = 0.1;
}
delta = tmp * FastMath.min(delta, 10.0 * lmNorm);
lmPar /= tmp;
} else if ((lmPar == 0) || (ratio >= 0.75)) {
delta = 2 * lmNorm;
lmPar *= 0.5;
}
// test for successful iteration.
if (ratio >= 1.0e-4) {
// successful iteration, update the norm
firstIteration = false;
xNorm = 0;
for (int k = 0; k < cols; ++k) {
double xK = diag[k] * point[k];
xNorm += xK * xK;
}
xNorm = FastMath.sqrt(xNorm);
current = new VectorialPointValuePair(point, objective);
// tests for convergence.
if (checker != null) {
// we use the vectorial convergence checker
if (checker.converged(getIterations(), previous, current)) {