123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609 |
- /* multiroots/hybridj.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__config.h"
- #include <stddef.h>
- #include <stdlib.h>
- #include <stdio.h>
- #include <math.h>
- #include <float.h>
- #include "gsl_math.h"
- #include "gsl_errno.h"
- #include "gsl_multiroots.h"
- #include "gsl_linalg.h"
- #include "gsl_multiroots__dogleg.c"
- typedef struct
- {
- size_t iter;
- size_t ncfail;
- size_t ncsuc;
- size_t nslow1;
- size_t nslow2;
- double fnorm;
- double delta;
- gsl_matrix *q;
- gsl_matrix *r;
- gsl_vector *tau;
- gsl_vector *diag;
- gsl_vector *qtf;
- gsl_vector *newton;
- gsl_vector *gradient;
- gsl_vector *x_trial;
- gsl_vector *f_trial;
- gsl_vector *df;
- gsl_vector *qtdf;
- gsl_vector *rdx;
- gsl_vector *w;
- gsl_vector *v;
- }
- hybridj_state_t;
- static int hybridj_alloc (void *vstate, size_t n);
- static int hybridj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
- static int hybridsj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
- static int set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale);
- static int hybridj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
- static void hybridj_free (void *vstate);
- static int iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale);
- static int
- hybridj_alloc (void *vstate, size_t n)
- {
- hybridj_state_t *state = (hybridj_state_t *) vstate;
- gsl_matrix *q, *r;
- gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
- *df, *qtdf, *rdx, *w, *v;
- q = gsl_matrix_calloc (n, n);
- if (q == 0)
- {
- GSL_ERROR ("failed to allocate space for q", GSL_ENOMEM);
- }
- state->q = q;
- r = gsl_matrix_calloc (n, n);
- if (r == 0)
- {
- gsl_matrix_free (q);
- GSL_ERROR ("failed to allocate space for r", GSL_ENOMEM);
- }
- state->r = r;
- tau = gsl_vector_calloc (n);
- if (tau == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- GSL_ERROR ("failed to allocate space for tau", GSL_ENOMEM);
- }
- state->tau = tau;
- diag = gsl_vector_calloc (n);
- if (diag == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- GSL_ERROR ("failed to allocate space for diag", GSL_ENOMEM);
- }
- state->diag = diag;
- qtf = gsl_vector_calloc (n);
- if (qtf == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- GSL_ERROR ("failed to allocate space for qtf", GSL_ENOMEM);
- }
- state->qtf = qtf;
- newton = gsl_vector_calloc (n);
- if (newton == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- GSL_ERROR ("failed to allocate space for newton", GSL_ENOMEM);
- }
- state->newton = newton;
- gradient = gsl_vector_calloc (n);
- if (gradient == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- GSL_ERROR ("failed to allocate space for gradient", GSL_ENOMEM);
- }
- state->gradient = gradient;
- x_trial = gsl_vector_calloc (n);
- if (x_trial == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
- }
- state->x_trial = x_trial;
- f_trial = gsl_vector_calloc (n);
- if (f_trial == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- GSL_ERROR ("failed to allocate space for f_trial", GSL_ENOMEM);
- }
- state->f_trial = f_trial;
- df = gsl_vector_calloc (n);
- if (df == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- gsl_vector_free (f_trial);
- GSL_ERROR ("failed to allocate space for df", GSL_ENOMEM);
- }
- state->df = df;
- qtdf = gsl_vector_calloc (n);
- if (qtdf == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- gsl_vector_free (f_trial);
- gsl_vector_free (df);
- GSL_ERROR ("failed to allocate space for qtdf", GSL_ENOMEM);
- }
- state->qtdf = qtdf;
- rdx = gsl_vector_calloc (n);
- if (rdx == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- gsl_vector_free (f_trial);
- gsl_vector_free (df);
- gsl_vector_free (qtdf);
- GSL_ERROR ("failed to allocate space for rdx", GSL_ENOMEM);
- }
- state->rdx = rdx;
- w = gsl_vector_calloc (n);
- if (w == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- gsl_vector_free (f_trial);
- gsl_vector_free (df);
- gsl_vector_free (qtdf);
- gsl_vector_free (rdx);
- GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
- }
- state->w = w;
- v = gsl_vector_calloc (n);
- if (v == 0)
- {
- gsl_matrix_free (q);
- gsl_matrix_free (r);
- gsl_vector_free (tau);
- gsl_vector_free (diag);
- gsl_vector_free (qtf);
- gsl_vector_free (newton);
- gsl_vector_free (gradient);
- gsl_vector_free (x_trial);
- gsl_vector_free (f_trial);
- gsl_vector_free (df);
- gsl_vector_free (qtdf);
- gsl_vector_free (rdx);
- gsl_vector_free (w);
- GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
- }
- state->v = v;
- return GSL_SUCCESS;
- }
- static int
- hybridj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
- {
- int status = set (vstate, fdf, x, f, J, dx, 0);
- return status ;
- }
- static int
- hybridsj_set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
- {
- int status = set (vstate, fdf, x, f, J, dx, 1);
- return status ;
- }
- static int
- set (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
- {
- hybridj_state_t *state = (hybridj_state_t *) vstate;
- gsl_matrix *q = state->q;
- gsl_matrix *r = state->r;
- gsl_vector *tau = state->tau;
- gsl_vector *diag = state->diag;
- GSL_MULTIROOT_FN_EVAL_F_DF (fdf, x, f, J);
- state->iter = 1;
- state->fnorm = enorm (f);
- state->ncfail = 0;
- state->ncsuc = 0;
- state->nslow1 = 0;
- state->nslow2 = 0;
- gsl_vector_set_all (dx, 0.0);
- /* Store column norms in diag */
- if (scale)
- compute_diag (J, diag);
- else
- gsl_vector_set_all (diag, 1.0);
- /* Set delta to factor |D x| or to factor if |D x| is zero */
- state->delta = compute_delta (diag, x);
- /* Factorize J into QR decomposition */
- gsl_linalg_QR_decomp (J, tau);
- gsl_linalg_QR_unpack (J, tau, q, r);
- return GSL_SUCCESS;
- }
- static int
- hybridj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
- {
- int status = iterate (vstate, fdf, x, f, J, dx, 0);
- return status;
- }
- static int
- hybridsj_iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
- {
- int status = iterate (vstate, fdf, x, f, J, dx, 1);
- return status;
- }
- static int
- iterate (void *vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx, int scale)
- {
- hybridj_state_t *state = (hybridj_state_t *) vstate;
- const double fnorm = state->fnorm;
- gsl_matrix *q = state->q;
- gsl_matrix *r = state->r;
- gsl_vector *tau = state->tau;
- gsl_vector *diag = state->diag;
- gsl_vector *qtf = state->qtf;
- gsl_vector *x_trial = state->x_trial;
- gsl_vector *f_trial = state->f_trial;
- gsl_vector *df = state->df;
- gsl_vector *qtdf = state->qtdf;
- gsl_vector *rdx = state->rdx;
- gsl_vector *w = state->w;
- gsl_vector *v = state->v;
- double prered, actred;
- double pnorm, fnorm1, fnorm1p;
- double ratio;
- double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
- /* Compute qtf = Q^T f */
- compute_qtf (q, f, qtf);
- /* Compute dogleg step */
- dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
- /* Take a trial step */
- compute_trial_step (x, dx, state->x_trial);
- pnorm = scaled_enorm (diag, dx);
- if (state->iter == 1)
- {
- if (pnorm < state->delta)
- {
- state->delta = pnorm;
- }
- }
- /* Evaluate function at x + p */
- {
- int status = GSL_MULTIROOT_FN_EVAL_F (fdf, x_trial, f_trial);
- if (status != GSL_SUCCESS)
- {
- return GSL_EBADFUNC;
- }
- }
- /* Set df = f_trial - f */
- compute_df (f_trial, f, df);
- /* Compute the scaled actual reduction */
- fnorm1 = enorm (f_trial);
- actred = compute_actual_reduction (fnorm, fnorm1);
- /* Compute rdx = R dx */
- compute_rdx (r, dx, rdx);
- /* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
- fnorm1p = enorm_sum (qtf, rdx);
- prered = compute_predicted_reduction (fnorm, fnorm1p);
- /* Compute the ratio of the actual to predicted reduction */
- if (prered > 0)
- {
- ratio = actred / prered;
- }
- else
- {
- ratio = 0;
- }
- /* Update the step bound */
- if (ratio < p1)
- {
- state->ncsuc = 0;
- state->ncfail++;
- state->delta *= p5;
- }
- else
- {
- state->ncfail = 0;
- state->ncsuc++;
- if (ratio >= p5 || state->ncsuc > 1)
- state->delta = GSL_MAX (state->delta, pnorm / p5);
- if (fabs (ratio - 1) <= p1)
- state->delta = pnorm / p5;
- }
- /* Test for successful iteration */
- if (ratio >= p0001)
- {
- gsl_vector_memcpy (x, x_trial);
- gsl_vector_memcpy (f, f_trial);
- state->fnorm = fnorm1;
- state->iter++;
- }
- /* Determine the progress of the iteration */
- state->nslow1++;
- if (actred >= p001)
- state->nslow1 = 0;
- if (actred >= p1)
- state->nslow2 = 0;
- if (state->ncfail == 2)
- {
- {
- int status = GSL_MULTIROOT_FN_EVAL_DF (fdf, x, J);
-
- if (status != GSL_SUCCESS)
- {
- return GSL_EBADFUNC;
- }
- }
- state->nslow2++;
- if (state->iter == 1)
- {
- if (scale)
- compute_diag (J, diag);
- state->delta = compute_delta (diag, x);
- }
- else
- {
- if (scale)
- update_diag (J, diag);
- }
- /* Factorize J into QR decomposition */
- gsl_linalg_QR_decomp (J, tau);
- gsl_linalg_QR_unpack (J, tau, q, r);
- return GSL_SUCCESS;
- }
- /* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|, v = D^2 dx/|dx| */
- compute_qtf (q, df, qtdf);
- compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
- /* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
- gsl_linalg_QR_update (q, r, w, v);
- /* No progress as measured by jacobian evaluations */
- if (state->nslow2 == 5)
- {
- return GSL_ENOPROGJ;
- }
- /* No progress as measured by function evaluations */
- if (state->nslow1 == 10)
- {
- return GSL_ENOPROG;
- }
- return GSL_SUCCESS;
- }
- static void
- hybridj_free (void *vstate)
- {
- hybridj_state_t *state = (hybridj_state_t *) vstate;
- gsl_vector_free (state->v);
- gsl_vector_free (state->w);
- gsl_vector_free (state->rdx);
- gsl_vector_free (state->qtdf);
- gsl_vector_free (state->df);
- gsl_vector_free (state->f_trial);
- gsl_vector_free (state->x_trial);
- gsl_vector_free (state->gradient);
- gsl_vector_free (state->newton);
- gsl_vector_free (state->qtf);
- gsl_vector_free (state->diag);
- gsl_vector_free (state->tau);
- gsl_matrix_free (state->r);
- gsl_matrix_free (state->q);
- }
- static const gsl_multiroot_fdfsolver_type hybridj_type =
- {
- "hybridj", /* name */
- sizeof (hybridj_state_t),
- &hybridj_alloc,
- &hybridj_set,
- &hybridj_iterate,
- &hybridj_free
- };
- static const gsl_multiroot_fdfsolver_type hybridsj_type =
- {
- "hybridsj", /* name */
- sizeof (hybridj_state_t),
- &hybridj_alloc,
- &hybridsj_set,
- &hybridsj_iterate,
- &hybridj_free
- };
- const gsl_multiroot_fdfsolver_type *gsl_multiroot_fdfsolver_hybridj = &hybridj_type;
- const gsl_multiroot_fdfsolver_type *gsl_multiroot_fdfsolver_hybridsj = &hybridsj_type;
|