initializeEstimate(problem);
// work matrices
double[] grad = new double[parameters.length];
ArrayRealVector bDecrement = new ArrayRealVector(parameters.length);
double[] bDecrementData = bDecrement.getDataRef();
RealMatrix wGradGradT = MatrixUtils.createRealMatrix(parameters.length, parameters.length);
// iterate until convergence is reached
double previous = Double.POSITIVE_INFINITY;
do {
// build the linear problem
incrementJacobianEvaluationsCounter();
RealVector b = new ArrayRealVector(parameters.length);
RealMatrix a = MatrixUtils.createRealMatrix(parameters.length, parameters.length);
for (int i = 0; i < measurements.length; ++i) {
if (! measurements [i].isIgnored()) {
double weight = measurements[i].getWeight();
double residual = measurements[i].getResidual();
// compute the normal equation
for (int j = 0; j < parameters.length; ++j) {
grad[j] = measurements[i].getPartial(parameters[j]);
bDecrementData[j] = weight * residual * grad[j];
}
// build the contribution matrix for measurement i
for (int k = 0; k < parameters.length; ++k) {
double gk = grad[k];
for (int l = 0; l < parameters.length; ++l) {
wGradGradT.setEntry(k, l, weight * gk * grad[l]);
}
}
// update the matrices
a = a.add(wGradGradT);
b = b.add(bDecrement);
}
}
try {