initializeEstimate(problem);
// work matrices
double[] grad = new double[parameters.length];
RealMatrixImpl bDecrement = new RealMatrixImpl(parameters.length, 1);
double[][] bDecrementData = bDecrement.getDataRef();
RealMatrixImpl wGradGradT = new RealMatrixImpl(parameters.length, parameters.length);
double[][] wggData = wGradGradT.getDataRef();
// iterate until convergence is reached
double previous = Double.POSITIVE_INFINITY;
do {
// build the linear problem
incrementJacobianEvaluationsCounter();
RealMatrix b = new RealMatrixImpl(parameters.length, 1);
RealMatrix a = new RealMatrixImpl(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][0] = weight * residual * grad[j];
}
// build the contribution matrix for measurement i
for (int k = 0; k < parameters.length; ++k) {
double[] wggRow = wggData[k];
double gk = grad[k];
for (int l = 0; l < parameters.length; ++l) {
wggRow[l] = weight * gk * grad[l];
}
}
// update the matrices
a = a.add(wGradGradT);
b = b.add(bDecrement);
}
}
try {
// solve the linearized least squares problem
RealMatrix dX = a.solve(b);
// update the estimated parameters
for (int i = 0; i < parameters.length; ++i) {
parameters[i].setEstimate(parameters[i].getEstimate() + dX.getEntry(i, 0));
}