public double MinimizeOneStep(Matrix parameters)
{
// initial value of the function; callee knows the size of the returned vector
var errorVector = function(parameters);
var error = errorVector.Dot(errorVector);
// Jacobian; callee knows the size of the returned matrix
var J = jacobianFunction(parameters);
// J'*J
var JtJ = new Matrix(parameters.Size, parameters.Size);
//stopWatch.Restart();
//JtJ.MultATA(J, J); // this is the big calculation that could be parallelized
JtJ.MultATAParallel(J, J);
//Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds);
// J'*error
var JtError = new Matrix(parameters.Size, 1);
//stopWatch.Restart();
JtError.MultATA(J, errorVector); // error vector must be a column vector
//Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds);
// allocate some space
var JtJaugmented = new Matrix(parameters.Size, parameters.Size);
var JtJinv = new Matrix(parameters.Size, parameters.Size);
var delta = new Matrix(parameters.Size, 1);
var newParameters = new Matrix(parameters.Size, 1);
// find a value of lambda that reduces error
double lambda = initialLambda;
while (true)
{
// augment J'*J: J'*J += lambda*(diag(J))
JtJaugmented.Copy(JtJ);
for (int i = 0; i < parameters.Size; i++)
{
JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i];
}
//WriteMatrixToFile(errorVector, "errorVector");
//WriteMatrixToFile(J, "J");
//WriteMatrixToFile(JtJaugmented, "JtJaugmented");
//WriteMatrixToFile(JtError, "JtError");
// solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error
JtJinv.Inverse(JtJaugmented);
delta.Mult(JtJinv, JtError);
// new parameters = parameters - delta [why not add?]
newParameters.Sub(parameters, delta);
// evaluate function, compute error
var newErrorVector = function(newParameters);
double newError = newErrorVector.Dot(newErrorVector);
// if error is reduced, divide lambda by 10
bool improvement;
if (newError < error)
{
lambda /= lambdaIncrement;
improvement = true;
}
else // if not, multiply lambda by 10
{
lambda *= lambdaIncrement;
improvement = false;
}
// termination criteria:
// reduction in error is too small
var diff = new Matrix(errorVector.Size, 1);
diff.Sub(errorVector, newErrorVector);
double diffSq = diff.Dot(diff);
double errorDelta = Math.Sqrt(diffSq / error);
if (errorDelta < minimumReduction)
{
state = States.ReductionStepTooSmall;
}
// lambda is too big
if (lambda > maximumLambda)
{
state = States.LambdaTooLarge;
}
// change in parameters is too small [not implemented]
// if we made an improvement, accept the new parameters
if (improvement)
{
parameters.Copy(newParameters);
error = newError;
break;
}
// if we meet termination criteria, break
if (state != States.Running)
{
break;
}
}
rmsError = Math.Sqrt(error / errorVector.Size);
return(rmsError);
}