Example #1
0
 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);
Example #2
0
        /*****************************************************************************/
        /*  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. ***/