123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500 |
- /* multifit/lmpar.c
- *
- * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
- */
- #include "gsl_permute_vector_double.h"
- #include "gsl_multifit__qrsolv.c"
- static size_t
- count_nsing (const gsl_matrix * r)
- {
- /* Count the number of nonsingular entries. Returns the index of the
- first entry which is singular. */
- size_t n = r->size2;
- size_t i;
- for (i = 0; i < n; i++)
- {
- double rii = gsl_matrix_get (r, i, i);
- if (rii == 0)
- {
- break;
- }
- }
- return i;
- }
- static void
- compute_newton_direction (const gsl_matrix * r, const gsl_permutation * perm,
- const gsl_vector * qtf, gsl_vector * x)
- {
- /* Compute and store in x the Gauss-Newton direction. If the
- Jacobian is rank-deficient then obtain a least squares
- solution. */
- const size_t n = r->size2;
- size_t i, j, nsing;
- for (i = 0 ; i < n ; i++)
- {
- double qtfi = gsl_vector_get (qtf, i);
- gsl_vector_set (x, i, qtfi);
- }
- nsing = count_nsing (r);
- #ifdef DEBUG
- printf("nsing = %d\n", nsing);
- printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("\n");
- printf("qtf = "); gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
- #endif
- for (i = nsing; i < n; i++)
- {
- gsl_vector_set (x, i, 0.0);
- }
- if (nsing > 0)
- {
- for (j = nsing; j > 0 && j--;)
- {
- double rjj = gsl_matrix_get (r, j, j);
- double temp = gsl_vector_get (x, j) / rjj;
-
- gsl_vector_set (x, j, temp);
-
- for (i = 0; i < j; i++)
- {
- double rij = gsl_matrix_get (r, i, j);
- double xi = gsl_vector_get (x, i);
- gsl_vector_set (x, i, xi - rij * temp);
- }
- }
- }
- gsl_permute_vector_inverse (perm, x);
- }
- static void
- compute_newton_correction (const gsl_matrix * r, const gsl_vector * sdiag,
- const gsl_permutation * p, gsl_vector * x,
- double dxnorm,
- const gsl_vector * diag, gsl_vector * w)
- {
- size_t n = r->size2;
- size_t i, j;
- for (i = 0; i < n; i++)
- {
- size_t pi = gsl_permutation_get (p, i);
- double dpi = gsl_vector_get (diag, pi);
- double xpi = gsl_vector_get (x, pi);
- gsl_vector_set (w, i, dpi * (dpi * xpi) / dxnorm);
- }
- for (j = 0; j < n; j++)
- {
- double sj = gsl_vector_get (sdiag, j);
- double wj = gsl_vector_get (w, j);
- double tj = wj / sj;
- gsl_vector_set (w, j, tj);
- for (i = j + 1; i < n; i++)
- {
- double rij = gsl_matrix_get (r, i, j);
- double wi = gsl_vector_get (w, i);
- gsl_vector_set (w, i, wi - rij * tj);
- }
- }
- }
- static void
- compute_newton_bound (const gsl_matrix * r, const gsl_vector * x,
- double dxnorm, const gsl_permutation * perm,
- const gsl_vector * diag, gsl_vector * w)
- {
- /* If the jacobian is not rank-deficient then the Newton step
- provides a lower bound for the zero of the function. Otherwise
- set this bound to zero. */
- size_t n = r->size2;
- size_t i, j;
- size_t nsing = count_nsing (r);
- if (nsing < n)
- {
- gsl_vector_set_zero (w);
- return;
- }
- for (i = 0; i < n; i++)
- {
- size_t pi = gsl_permutation_get (perm, i);
- double dpi = gsl_vector_get (diag, pi);
- double xpi = gsl_vector_get (x, pi);
- gsl_vector_set (w, i, dpi * (dpi * xpi / dxnorm));
- }
- for (j = 0; j < n; j++)
- {
- double sum = 0;
- for (i = 0; i < j; i++)
- {
- sum += gsl_matrix_get (r, i, j) * gsl_vector_get (w, i);
- }
- {
- double rjj = gsl_matrix_get (r, j, j);
- double wj = gsl_vector_get (w, j);
- gsl_vector_set (w, j, (wj - sum) / rjj);
- }
- }
- }
- static void
- compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p,
- const gsl_vector * qtf, const gsl_vector * diag,
- gsl_vector * g)
- {
- const size_t n = r->size2;
- size_t i, j;
- for (j = 0; j < n; j++)
- {
- double sum = 0;
- for (i = 0; i <= j; i++)
- {
- sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
- }
- {
- size_t pj = gsl_permutation_get (p, j);
- double dpj = gsl_vector_get (diag, pj);
- gsl_vector_set (g, j, sum / dpj);
- }
- }
- }
- static int
- lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf,
- const gsl_vector * diag, double delta, double * par_inout,
- gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag,
- gsl_vector * x, gsl_vector * w)
- {
- double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c;
- double par = *par_inout;
- size_t iter = 0;
- #ifdef DEBUG
- printf("ENTERING lmpar\n");
- #endif
- compute_newton_direction (r, perm, qtf, newton);
- #ifdef DEBUG
- printf ("newton = ");
- gsl_vector_fprintf (stdout, newton, "%g");
- printf ("\n");
- printf ("diag = ");
- gsl_vector_fprintf (stdout, diag, "%g");
- printf ("\n");
- #endif
- /* Evaluate the function at the origin and test for acceptance of
- the Gauss-Newton direction. */
- dxnorm = scaled_enorm (diag, newton);
- fp = dxnorm - delta;
- #ifdef DEBUG
- printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
- #endif
- if (fp <= 0.1 * delta)
- {
- gsl_vector_memcpy (x, newton);
- #ifdef DEBUG
- printf ("took newton (fp = %g, delta = %g)\n", fp, delta);
- #endif
- *par_inout = 0;
- return GSL_SUCCESS;
- }
- #ifdef DEBUG
- printf ("r = ");
- gsl_matrix_fprintf (stdout, r, "%g");
- printf ("\n");
- printf ("newton = ");
- gsl_vector_fprintf (stdout, newton, "%g");
- printf ("\n");
- printf ("dxnorm = %g\n", dxnorm);
- #endif
- compute_newton_bound (r, newton, dxnorm, perm, diag, w);
- #ifdef DEBUG
- printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
- printf ("diag = ");
- gsl_vector_fprintf (stdout, diag, "%g");
- printf ("\n");
- printf ("w = ");
- gsl_vector_fprintf (stdout, w, "%g");
- printf ("\n");
- #endif
- {
- double wnorm = enorm (w);
- double phider = wnorm * wnorm;
- /* w == zero if r rank-deficient,
- then set lower bound to zero form MINPACK, lmder.f
- Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */
- if ( wnorm > 0 )
- par_lower = fp / (delta * phider);
- else
- par_lower = 0.0;
- }
- #ifdef DEBUG
- printf("par = %g\n", par );
- printf("par_lower = %g\n", par_lower);
- #endif
- compute_gradient_direction (r, perm, qtf, diag, gradient);
- gnorm = enorm (gradient);
- #ifdef DEBUG
- printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
- printf("gnorm = %g\n", gnorm);
- #endif
- par_upper = gnorm / delta;
- if (par_upper == 0)
- {
- par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1);
- }
- #ifdef DEBUG
- printf("par_upper = %g\n", par_upper);
- #endif
- if (par > par_upper)
- {
- #ifdef DEBUG
- printf("set par to par_upper\n");
- #endif
- par = par_upper;
- }
- else if (par < par_lower)
- {
- #ifdef DEBUG
- printf("set par to par_lower\n");
- #endif
- par = par_lower;
- }
- if (par == 0)
- {
- par = gnorm / dxnorm;
- #ifdef DEBUG
- printf("set par to gnorm/dxnorm = %g\n", par);
- #endif
- }
- /* Beginning of iteration */
- iteration:
- iter++;
- #ifdef DEBUG
- printf("lmpar iteration = %d\n", iter);
- #endif
- #ifdef BRIANSFIX
- /* Seems like this is described in the paper but not in the MINPACK code */
- if (par < par_lower || par > par_upper)
- {
- par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper));
- }
- #endif
- /* Evaluate the function at the current value of par */
- if (par == 0)
- {
- par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN);
- #ifdef DEBUG
- printf("par = 0, set par to = %g\n", par);
- #endif
- }
- /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x]
- for A = Q R P^T */
- #ifdef DEBUG
- printf ("calling qrsolv with par = %g\n", par);
- #endif
- {
- double sqrt_par = sqrt(par);
- qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w);
- }
- dxnorm = scaled_enorm (diag, x);
- fp_old = fp;
- fp = dxnorm - delta;
- #ifdef DEBUG
- printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
- printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n");
- printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
- printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n");
- #endif
- /* If the function is small enough, accept the current value of par */
- if (fabs (fp) <= 0.1 * delta)
- goto line220;
- if (par_lower == 0 && fp <= fp_old && fp_old < 0)
- goto line220;
- /* Check for maximum number of iterations */
- if (iter == 10)
- goto line220;
- /* Compute the Newton correction */
- compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w);
- #ifdef DEBUG
- printf ("newton_correction = ");
- gsl_vector_fprintf(stdout, w, "%g"); printf("\n");
- #endif
- {
- double wnorm = enorm (w);
- par_c = fp / (delta * wnorm * wnorm);
- }
- #ifdef DEBUG
- printf("fp = %g\n", fp);
- printf("par_lower = %g\n", par_lower);
- printf("par_upper = %g\n", par_upper);
- printf("par_c = %g\n", par_c);
- #endif
- /* Depending on the sign of the function, update par_lower or par_upper */
- if (fp > 0)
- {
- if (par > par_lower)
- {
- par_lower = par;
- #ifdef DEBUG
- printf("fp > 0: set par_lower = par = %g\n", par);
- #endif
- }
- }
- else if (fp < 0)
- {
- if (par < par_upper)
- {
- #ifdef DEBUG
- printf("fp < 0: set par_upper = par = %g\n", par);
- #endif
- par_upper = par;
- }
- }
- /* Compute an improved estimate for par */
- #ifdef DEBUG
- printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c);
- #endif
- par = GSL_MAX_DBL (par_lower, par + par_c);
- #ifdef DEBUG
- printf("improved estimate par = %g \n", par);
- #endif
- goto iteration;
- line220:
- #ifdef DEBUG
- printf("LEAVING lmpar, par = %g\n", par);
- #endif
- *par_inout = par;
- return GSL_SUCCESS;
- }
|