Validate.isTrue(nObs == sigma.getNumberOfElements(), "observedValues and sigma must be same length");
ArgumentChecker.isTrue(nObs >= nParms, "must have data points greater or equal to number of parameters. #date points = {}, #parameters = {}", nObs, nParms);
ArgumentChecker.isTrue(constraints.evaluate(startPos), "The inital value of the parameters (startPos) is {} - this is not an allowed value", startPos);
DoubleMatrix2D alpha;
DecompositionResult decmp;
DoubleMatrix1D theta = startPos;
double lambda = 0.0; //TODO debug if the model is linear, it will be solved in 1 step
double newChiSqr, oldChiSqr;
DoubleMatrix1D error = getError(func, observedValues, sigma, theta);
DoubleMatrix1D newError;
DoubleMatrix2D jacobian = getJacobian(jac, sigma, theta);
oldChiSqr = getChiSqr(error);
//If we start at the solution we are done
if (oldChiSqr == 0.0) {
return finish(oldChiSqr, jacobian, theta, sigma);
}
DoubleMatrix1D beta = getChiSqrGrad(error, jacobian);
for (int count = 0; count < MAX_ATTEMPTS; count++) {
alpha = getModifiedCurvatureMatrix(jacobian, lambda);
DoubleMatrix1D deltaTheta;
try {
decmp = _decomposition.evaluate(alpha);
deltaTheta = decmp.solve(beta);
} catch (final Exception e) {
throw new MathException(e);
}
DoubleMatrix1D trialTheta = (DoubleMatrix1D) _algebra.add(theta, deltaTheta);
//if the new value of theta is not in the model domain or the jump is too large, keep increasing lambda until an acceptable step is found
if (!constraints.evaluate(trialTheta) || !allowJump(deltaTheta, maxJumps)) {
lambda = increaseLambda(lambda);
continue;
}
newError = getError(func, observedValues, sigma, trialTheta);
newChiSqr = getChiSqr(newError);
//Check for convergence when no improvement in chiSqr occurs
if (Math.abs(newChiSqr - oldChiSqr) / (1 + oldChiSqr) < _eps) {
final DoubleMatrix2D alpha0 = lambda == 0.0 ? alpha : getModifiedCurvatureMatrix(jacobian, 0.0);
//if the model is an exact fit to the data, then no more improvement is possible
if (newChiSqr < _eps) {
if (lambda > 0.0) {
decmp = _decomposition.evaluate(alpha0);
}
return finish(alpha0, decmp, newChiSqr, jacobian, trialTheta, sigma);
}
final SVDecompositionCommons svd = (SVDecompositionCommons) DecompositionFactory.SV_COMMONS;
//add the second derivative information to the Hessian matrix to check we are not at a local maximum or saddle point
final VectorFieldSecondOrderDifferentiator diff = new VectorFieldSecondOrderDifferentiator();
final Function1D<DoubleMatrix1D, DoubleMatrix2D[]> secDivFunc = diff.differentiate(func, constraints);
final DoubleMatrix2D[] secDiv = secDivFunc.evaluate(trialTheta);
final double[][] temp = new double[nParms][nParms];
for (int i = 0; i < nObs; i++) {
for (int j = 0; j < nParms; j++) {
for (int k = 0; k < nParms; k++) {
temp[j][k] -= newError.getEntry(i) * secDiv[i].getEntry(j, k) / sigma.getEntry(i);
}
}
}
final DoubleMatrix2D newAlpha = (DoubleMatrix2D) _algebra.add(alpha0, new DoubleMatrix2D(temp));
final SVDecompositionResult svdRes = svd.evaluate(newAlpha);
final double[] w = svdRes.getSingularValues();
final DoubleMatrix2D u = svdRes.getU();
final DoubleMatrix2D v = svdRes.getV();
final double[] p = new double[nParms];
boolean saddle = false;
double sum = 0.0;
for (int i = 0; i < nParms; i++) {
double a = 0.0;
for (int j = 0; j < nParms; j++) {
a += u.getEntry(j, i) * v.getEntry(j, i);
}
final int sign = a > 0.0 ? 1 : -1;
if (w[i] * sign < 0.0) {
sum += w[i];
w[i] = -w[i];
saddle = true;
}
}
//if a local maximum or saddle point is found (as indicated by negative eigenvalues), move in a direction that is a weighted
//sum of the eigenvectors corresponding to the negative eigenvalues
if (saddle) {
lambda = increaseLambda(lambda);
for (int i = 0; i < nParms; i++) {
if (w[i] < 0.0) {
final double scale = 0.5 * Math.sqrt(-oldChiSqr * w[i]) / sum;
for (int j = 0; j < nParms; j++) {
p[j] += scale * u.getEntry(j, i);
}
}
}
final DoubleMatrix1D direction = new DoubleMatrix1D(p);
deltaTheta = direction;
trialTheta = (DoubleMatrix1D) _algebra.add(theta, deltaTheta);
int i = 0;
double scale = 1.0;
while (!constraints.evaluate(trialTheta)) {