internal static extern void lmmin(
int n_par, IntPtr par, int m_dat, IntPtr data, LMDelegate evaluate,
ref LMControlStruct control, ref LMStatusStruct status,
AllocatorDelegate arrayAllocator, DeallocatorDelegate arrayDeallocator);
/*****************************************************************************/
/* lmmin (main minimization routine) */
/*****************************************************************************/
/*
* This routine contains the core algorithm of our library.
*
* It minimizes the sum of the squares of m nonlinear functions
* in n variables by a modified Levenberg-Marquardt algorithm.
* The function evaluation is done by the user-provided routine 'evaluate'.
* The Jacobian is then calculated by a forward-difference approximation.
*
* Parameters:
*
* n is the number of variables (INPUT, positive integer).
*
* x is the solution vector (INPUT/OUTPUT, array of length n).
* On input it must be set to an estimated solution.
* On output it yields the final estimate of the solution.
*
* m is the number of functions to be minimized (INPUT, positive integer).
* It must fulfill m>=n.
*
* data is a pointer that is ignored by lmmin; it is however forwarded
* to the user-supplied functions evaluate and printout.
* In a typical application, it contains experimental data to be fitted.
*
* evaluate is a user-supplied function that calculates the m functions.
* Parameters:
* n, x, m, data as above.
* fvec is an array of length m; on OUTPUT, it must contain the
* m function values for the parameter vector x.
* userbreak is an integer pointer. When *userbreak is set to a
* nonzero value, lmmin will terminate.
*
* control contains INPUT variables that control the fit algorithm,
* as declared and explained in lmstruct.h
*
* status contains OUTPUT variables that inform about the fit result,
* as declared and explained in lmstruct.h
*/
public static void LMMin(int n, double[] x, int m, /*const void* data,*/
Action <double[], double[]> evaluate,
ref Native.LMControlStruct C,
ref Native.LMStatusStruct S)
{
double[] fvec, diag, fjac, qtf, wa1, wa2, wa3, wf;
int[] ipvt;
int j, i;
double actred, dirder, fnorm, fnorm1, gnorm, pnorm, prered, ratio, step, sum, temp, temp1, temp2, temp3;
int maxfev = C.patience * (n + 1);
int outer, inner; /* loop counters, for monitoring */
bool inner_success; /* flag for loop control */
double lmpar = 0; /* Levenberg-Marquardt parameter */
double delta = 0;
double xnorm = 0;
double eps = Math.Sqrt(Math.Max(C.epsilon, LM_MACHEP)); /* for forward differences */
int nout = C.n_maxpri == -1 ? n : Math.Min(C.n_maxpri, n);
/* Default status info; must be set ahead of first return statements */
S.outcome = 0; /* status code */
S.userbreak = 0;
S.nfev = 0; /* function evaluation counter */
/*** Check input parameters for errors. ***/
if (n <= 0)
{
Console.Error.WriteLine($"lmmin: invalid number of parameters {n}");
S.outcome = 10; /* invalid parameter */
return;
}
if (m < n)
{
Console.Error.WriteLine($"lmmin: number of data points {m} smaller than number of parameters {n}");
S.outcome = 10;
return;
}
if (C.ftol < 0.0 || C.xtol < 0.0 || C.gtol < 0.0)
{
Console.Error.WriteLine($"lmmin: negative tolerance (at least one of {C.ftol} {C.xtol} {C.gtol}");
S.outcome = 10;
return;
}
if (maxfev <= 0)
{
Console.Error.WriteLine($"lmmin: nonpositive function evaluations limit {maxfev}");
S.outcome = 10;
return;
}
if (C.stepbound <= 0.0)
{
Console.Error.WriteLine($"lmmin: nonpositive stepbound {C.stepbound}");
S.outcome = 10;
return;
}
if (C.scale_diag != 0 && C.scale_diag != 1)
{
Console.Error.WriteLine($"lmmin: logical variable scale_diag={C.scale_diag} should be 0 or 1");
S.outcome = 10;
return;
}
/*** Allocate work space. ***/
fvec = new double[m];
diag = new double[n];
qtf = new double[n];
fjac = new double[n * m];
wa1 = new double[n];
wa2 = new double[n];
wa3 = new double[n];
wf = new double[m];
ipvt = new int[n];
if (C.scale_diag != 1)
{
for (j = 0; j < n; j++)
{
diag[j] = 1.0;
}
}
/*** Evaluate function at starting point and calculate norm. ***/
evaluate(x, fvec);
S.nfev = 1;
if (S.userbreak != 0)
{
goto terminate;
}
fnorm = EuclideanNorm(m, 0, fvec);
if (C.verbosity > 0)
{
Console.WriteLine("lmmin start ");
PrintPars(nout, x, fnorm);
}
if (fnorm <= LM_DWARF)
{
S.outcome = 0; /* sum of squares almost zero, nothing to do */
goto terminate;
}
/*** The outer loop: compute gradient, then descend. ***/
for (outer = 0; ; ++outer)
{
/*** [outer] Calculate the Jacobian. ***/
for (j = 0; j < n; j++)
{
temp = x[j];
step = Math.Max(eps * eps, eps * Math.Abs(temp));
x[j] += step; /* replace temporarily */
evaluate(x, wf);
++S.nfev;
if (S.userbreak != 0)
{
goto terminate;
}
for (i = 0; i < m; i++)
{
fjac[j * m + i] = (wf[i] - fvec[i]) / step;
}
x[j] = temp; /* restore */
}
/*** [outer] Compute the QR factorization of the Jacobian. ***/
/* fjac is an m by n array. The upper n by n submatrix of fjac
* is made to contain an upper triangular matrix r with diagonal
* elements of nonincreasing magnitude such that
*
* p^T*(jac^T*jac)*p = r^T*r
*
* (NOTE: ^T stands for matrix transposition),
*
* where p is a permutation matrix and jac is the final calculated
* Jacobian. Column j of p is column ipvt(j) of the identity matrix.
* The lower trapezoidal part of fjac contains information generated
* during the computation of r.
*
* ipvt is an integer array of length n. It defines a permutation
* matrix p such that jac*p = q*r, where jac is the final calculated
* Jacobian, q is orthogonal (not stored), and r is upper triangular
* with diagonal elements of nonincreasing magnitude. Column j of p
* is column ipvt(j) of the identity matrix.
*/
QRFactorization(m, n, fjac, ipvt, wa1, wa2, wa3);
/* return values are ipvt, wa1=rdiag, wa2=acnorm */
/*** [outer] Form q^T * fvec and store first n components in qtf. ***/
for (i = 0; i < m; i++)
{
wf[i] = fvec[i];
}
for (j = 0; j < n; j++)
{
temp3 = fjac[j * m + j];
if (temp3 != 0.0)
{
sum = 0;
for (i = j; i < m; i++)
{
sum += fjac[j * m + i] * wf[i];
}
temp = -sum / temp3;
for (i = j; i < m; i++)
{
wf[i] += fjac[j * m + i] * temp;
}
}
fjac[j * m + j] = wa1[j];
qtf[j] = wf[j];
}
/*** [outer] Compute norm of scaled gradient and detect degeneracy. ***/
gnorm = 0;
for (j = 0; j < n; j++)
{
if (wa2[ipvt[j]] == 0)
{
continue;
}
sum = 0.0;
for (i = 0; i <= j; i++)
{
sum += fjac[j * m + i] * qtf[i];
}
gnorm = Math.Max(gnorm, Math.Abs(sum / wa2[ipvt[j]] / fnorm));
}
if (gnorm <= C.gtol)
{
S.outcome = 4;
goto terminate;
}
/*** [outer] Initialize / update diag and delta. ***/
if (outer == 0)
{
/* first iteration only */
if (C.scale_diag != 0)
{
/* diag := norms of the columns of the initial Jacobian */
for (j = 0; j < n; j++)
{
diag[j] = wa2[j] != 0.0 ? wa2[j] : 1;
}
/* xnorm := || D x || */
for (j = 0; j < n; j++)
{
wa3[j] = diag[j] * x[j];
}
xnorm = EuclideanNorm(n, 0, wa3);
if (C.verbosity >= 2)
{
Console.Write("lmmin diag ");
PrintPars(nout, x, xnorm);
}
/* only now print the header for the loop table */
if (C.verbosity >= 3)
{
Console.Write(" o i lmpar prered ratio dirder delta pnorm fnorm");
for (i = 0; i < nout; ++i)
{
Console.Write($" p{i}");
}
Console.WriteLine();
}
}
else
{
xnorm = EuclideanNorm(n, 0, x);
}
/* initialize the step bound delta. */
if (xnorm != 0.0)
{
delta = C.stepbound * xnorm;
}
else
{
delta = C.stepbound;
}
}
else
{
if (C.scale_diag != 0)
{
for (j = 0; j < n; j++)
{
diag[j] = Math.Max(diag[j], wa2[j]);
}
}
}
/*** The inner loop. ***/
inner = 0;
do
{
/*** [inner] Determine the Levenberg-Marquardt parameter. ***/
LMParameter(n, fjac, m, ipvt, diag, qtf, delta, ref lmpar, wa1, wa2, wf, wa3);
/* used return values are fjac (partly), lmpar, wa1=x, wa3=diag*x */
/* predict scaled reduction */
pnorm = EuclideanNorm(n, 0, wa3);
var pdf = pnorm / fnorm;
temp2 = lmpar * pdf * pdf;
for (j = 0; j < n; j++)
{
wa3[j] = 0;
for (i = 0; i <= j; i++)
{
wa3[i] -= fjac[j * m + i] * wa1[ipvt[j]];
}
}
temp1 = Square(EuclideanNorm(n, 0, wa3) / fnorm);
prered = temp1 + 2 * temp2;
dirder = -temp1 + temp2; /* scaled directional derivative */
/* at first call, adjust the initial step bound. */
if (outer == 0 && pnorm < delta)
{
delta = pnorm;
}
/*** [inner] Evaluate the function at x + p. ***/
for (j = 0; j < n; j++)
{
wa2[j] = x[j] - wa1[j];
}
evaluate(wa2, wf);
++S.nfev;
if (S.userbreak != 0)
{
goto terminate;
}
fnorm1 = EuclideanNorm(m, 0, wf);
/*** [inner] Evaluate the scaled reduction. ***/
/* actual scaled reduction */
actred = 1 - Square(fnorm1 / fnorm);
/* ratio of actual to predicted reduction */
ratio = prered != 0.0? actred / prered : 0;
if (C.verbosity == 2)
{
Console.Write($"lmmin ({outer}:{inner}) ");
PrintPars(nout, wa2, fnorm1);
}
else if (C.verbosity >= 3)
{
Console.Write($"{outer} {inner} {lmpar} {prered} {ratio} {dirder} {delta} {pnorm} {fnorm1}");
for (i = 0; i < nout; ++i)
{
Console.Write($" {wa2[i]}");
}
Console.WriteLine();
}
/* update the step bound */
if (ratio <= 0.25)
{
if (actred >= 0)
{
temp = 0.5;
}
else if (actred > -99) /* -99 = 1-1/0.1^2 */
{
temp = Math.Max(dirder / (2 * dirder + actred), 0.1);
}
else
{
temp = 0.1;
}
delta = temp * Math.Min(delta, pnorm / 0.1);
lmpar /= temp;
}
else if (ratio >= 0.75)
{
delta = 2 * pnorm;
lmpar *= 0.5;
}
else if (lmpar == 0.0)
{
delta = 2 * pnorm;
}
/*** [inner] On success, update solution, and test for convergence. ***/
inner_success = ratio >= p0001;
if (inner_success)
{
/* update x, fvec, and their norms */
if (C.scale_diag != 0)
{
for (j = 0; j < n; j++)
{
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
}
else
{
for (j = 0; j < n; j++)
{
x[j] = wa2[j];
}
}
for (i = 0; i < m; i++)
{
fvec[i] = wf[i];
}
xnorm = EuclideanNorm(n, 0, wa2);
fnorm = fnorm1;
}
/* convergence tests */
S.outcome = 0;
if (fnorm <= LM_DWARF)
{
goto terminate; /* success: sum of squares almost zero */
}
/* test two criteria (both may be fulfilled) */
if (Math.Abs(actred) <= C.ftol && prered <= C.ftol && ratio <= 2)
{
S.outcome = 1; /* success: x almost stable */
}
if (delta <= C.xtol * xnorm)
{
S.outcome += 2; /* success: sum of squares almost stable */
}
if (S.outcome != 0)
{
goto terminate;
}
/*** [inner] Tests for termination and stringent tolerances. ***/
if (S.nfev >= maxfev)
{
S.outcome = 5;
goto terminate;
}
if (Math.Abs(actred) <= LM_MACHEP &&
prered <= LM_MACHEP && ratio <= 2)
{
S.outcome = 6;
goto terminate;
}
if (delta <= LM_MACHEP * xnorm)
{
S.outcome = 7;
goto terminate;
}
if (gnorm <= LM_MACHEP)
{
S.outcome = 8;
goto terminate;
}
/*** [inner] End of the loop. Repeat if iteration unsuccessful. ***/
++inner;
} while (!inner_success);
/*** [outer] End of the loop. ***/
}
;
terminate:
S.fnorm = EuclideanNorm(m, 0, fvec);
if (C.verbosity >= 2)
{
Console.WriteLine($"lmmin outcome ({S.outcome}) xnorm {xnorm} ftol {C.ftol} xtol {C.xtol}");
}
if (C.verbosity % 2 != 0)
{
Console.Write("lmmin final ");
PrintPars(nout, x, S.fnorm);
}
if (S.userbreak == 1) /* user-requested break */
{
S.outcome = 11;
}
} /*** lmmin. ***/