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 {
// solve the linearized least squares problem
RealVector dX = new LUDecompositionImpl(a).getSolver().solve(b);
// update the estimated parameters
for (int i = 0; i < parameters.length; ++i) {
parameters[i].setEstimate(parameters[i].getEstimate() + dX.getEntry(i));
}
} catch(InvalidMatrixException e) {
throw new EstimationException("unable to solve: singular problem");
}