|
Packit |
67cb25 |
/* multifit_nlinear/lm.c
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* Copyright (C) 2014, 2015, 2016 Patrick Alken
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is free software; you can redistribute it and/or modify
|
|
Packit |
67cb25 |
* it under the terms of the GNU General Public License as published by
|
|
Packit |
67cb25 |
* the Free Software Foundation; either version 3 of the License, or (at
|
|
Packit |
67cb25 |
* your option) any later version.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* This program is distributed in the hope that it will be useful, but
|
|
Packit |
67cb25 |
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
Packit |
67cb25 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
Packit |
67cb25 |
* General Public License for more details.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* You should have received a copy of the GNU General Public License
|
|
Packit |
67cb25 |
* along with this program; if not, write to the Free Software
|
|
Packit |
67cb25 |
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#include <config.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_math.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_vector.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_matrix.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_linalg.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_multifit_nlinear.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_errno.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_blas.h>
|
|
Packit |
67cb25 |
#include <gsl/gsl_permutation.h>
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
* This module contains an implementation of the Levenberg-Marquardt
|
|
Packit |
67cb25 |
* algorithm for nonlinear optimization problems. This implementation
|
|
Packit |
67cb25 |
* closely follows the following works:
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
|
|
Packit |
67cb25 |
* Data Fitting, Informatics and Mathematical Modeling,
|
|
Packit |
67cb25 |
* Technical University of Denmark (DTU), 2010.
|
|
Packit |
67cb25 |
*
|
|
Packit |
67cb25 |
* [2] J. J. More, The Levenberg-Marquardt Algorithm: Implementation
|
|
Packit |
67cb25 |
* and Theory, Lecture Notes in Mathematics, v630, 1978.
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
typedef struct
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
size_t n; /* number of observations */
|
|
Packit |
67cb25 |
size_t p; /* number of parameters */
|
|
Packit |
67cb25 |
gsl_vector *fvv; /* D_v^2 f(x), size n */
|
|
Packit |
67cb25 |
gsl_vector *vel; /* geodesic velocity (standard LM step), size p */
|
|
Packit |
67cb25 |
gsl_vector *acc; /* geodesic acceleration, size p */
|
|
Packit |
67cb25 |
gsl_vector *workp; /* workspace, length p */
|
|
Packit |
67cb25 |
gsl_vector *workn; /* workspace, length n */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
int accel; /* use geodesic acceleration? */
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* tunable parameters */
|
|
Packit |
67cb25 |
gsl_multifit_nlinear_parameters params;
|
|
Packit |
67cb25 |
} lm_state_t;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
#include "common.c"
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void *lm_alloc (const int accel, const void * params, const size_t n, const size_t p);
|
|
Packit |
67cb25 |
static void *lm_alloc_noaccel (const void * params, const size_t n, const size_t p);
|
|
Packit |
67cb25 |
static void *lm_alloc_accel (const void * params, const size_t n, const size_t p);
|
|
Packit |
67cb25 |
static void lm_free(void *vstate);
|
|
Packit |
67cb25 |
static int lm_init(const void *vtrust_state, void *vstate);
|
|
Packit |
67cb25 |
static int lm_preloop(const void * vtrust_state, void * vstate);
|
|
Packit |
67cb25 |
static int lm_step(const void * vtrust_state, const double delta,
|
|
Packit |
67cb25 |
gsl_vector * dx, void * vstate);
|
|
Packit |
67cb25 |
static int lm_preduction(const void * vtrust_state, const gsl_vector * dx,
|
|
Packit |
67cb25 |
double * pred, void * vstate);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void *
|
|
Packit |
67cb25 |
lm_alloc (const int accel, const void * params, const size_t n, const size_t p)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_parameters *mparams = (const gsl_multifit_nlinear_parameters *) params;
|
|
Packit |
67cb25 |
lm_state_t *state;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state = calloc(1, sizeof(lm_state_t));
|
|
Packit |
67cb25 |
if (state == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate lm state", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->workp = gsl_vector_alloc(p);
|
|
Packit |
67cb25 |
if (state->workp == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->workn = gsl_vector_alloc(n);
|
|
Packit |
67cb25 |
if (state->workn == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->fvv = gsl_vector_alloc(n);
|
|
Packit |
67cb25 |
if (state->fvv == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate space for fvv", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->vel = gsl_vector_alloc(p);
|
|
Packit |
67cb25 |
if (state->vel == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate space for vel", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->acc = gsl_vector_alloc(p);
|
|
Packit |
67cb25 |
if (state->acc == NULL)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
GSL_ERROR_NULL ("failed to allocate space for acc", GSL_ENOMEM);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
state->n = n;
|
|
Packit |
67cb25 |
state->p = p;
|
|
Packit |
67cb25 |
state->params = *mparams;
|
|
Packit |
67cb25 |
state->accel = accel;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return state;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void *
|
|
Packit |
67cb25 |
lm_alloc_noaccel (const void * params, const size_t n, const size_t p)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return lm_alloc(0, params, n, p);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void *
|
|
Packit |
67cb25 |
lm_alloc_accel (const void * params, const size_t n, const size_t p)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
return lm_alloc(1, params, n, p);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static void
|
|
Packit |
67cb25 |
lm_free(void *vstate)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
lm_state_t *state = (lm_state_t *) vstate;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->workp)
|
|
Packit |
67cb25 |
gsl_vector_free(state->workp);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->workn)
|
|
Packit |
67cb25 |
gsl_vector_free(state->workn);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->fvv)
|
|
Packit |
67cb25 |
gsl_vector_free(state->fvv);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->vel)
|
|
Packit |
67cb25 |
gsl_vector_free(state->vel);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->acc)
|
|
Packit |
67cb25 |
gsl_vector_free(state->acc);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
free(state);
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
lm_init()
|
|
Packit |
67cb25 |
Initialize LM solver
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Inputs: vtrust_state - trust state
|
|
Packit |
67cb25 |
vstate - workspace
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
Return: success/error
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
lm_init(const void *vtrust_state, void *vstate)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trust_state *trust_state =
|
|
Packit |
67cb25 |
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
|
|
Packit |
67cb25 |
lm_state_t *state = (lm_state_t *) vstate;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
gsl_vector_set_zero(state->vel);
|
|
Packit |
67cb25 |
gsl_vector_set_zero(state->acc);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
*(trust_state->avratio) = 0.0;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
lm_preloop()
|
|
Packit |
67cb25 |
Initialize LM method for new Jacobian matrix
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
lm_preloop(const void * vtrust_state, void * vstate)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
int status;
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trust_state *trust_state =
|
|
Packit |
67cb25 |
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_parameters *params = trust_state->params;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
(void)vstate;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* initialize linear least squares solver */
|
|
Packit |
67cb25 |
status = (params->solver->init)(trust_state, trust_state->solver_state);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
lm_step()
|
|
Packit |
67cb25 |
Calculate a new step vector by solving the linear
|
|
Packit |
67cb25 |
least squares system:
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
[ J ] v = - [ f ]
|
|
Packit |
67cb25 |
[ sqrt(mu) D ] [ 0 ]
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
lm_step(const void * vtrust_state, const double delta,
|
|
Packit |
67cb25 |
gsl_vector * dx, void * vstate)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
int status;
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trust_state *trust_state =
|
|
Packit |
67cb25 |
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
|
|
Packit |
67cb25 |
lm_state_t *state = (lm_state_t *) vstate;
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_parameters *params = trust_state->params;
|
|
Packit |
67cb25 |
const double mu = *(trust_state->mu);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
(void)delta;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* prepare the linear solver with current LM parameter mu */
|
|
Packit |
67cb25 |
status = (params->solver->presolve)(mu, trust_state, trust_state->solver_state);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
* solve: [ J ] v = - [ f ]
|
|
Packit |
67cb25 |
* [ sqrt(mu)*D ] [ 0 ]
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
status = (params->solver->solve)(trust_state->f,
|
|
Packit |
67cb25 |
state->vel,
|
|
Packit |
67cb25 |
trust_state,
|
|
Packit |
67cb25 |
trust_state->solver_state);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
if (state->accel)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
double anorm, vnorm;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute geodesic acceleration */
|
|
Packit |
67cb25 |
status = gsl_multifit_nlinear_eval_fvv(params->h_fvv,
|
|
Packit |
67cb25 |
trust_state->x,
|
|
Packit |
67cb25 |
state->vel,
|
|
Packit |
67cb25 |
trust_state->f,
|
|
Packit |
67cb25 |
trust_state->J,
|
|
Packit |
67cb25 |
trust_state->sqrt_wts,
|
|
Packit |
67cb25 |
trust_state->fdf,
|
|
Packit |
67cb25 |
state->fvv,
|
|
Packit |
67cb25 |
state->workp);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
* solve: [ J ] a = - [ fvv ]
|
|
Packit |
67cb25 |
* [ sqrt(mu)*D ] [ 0 ]
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
status = (params->solver->solve)(state->fvv,
|
|
Packit |
67cb25 |
state->acc,
|
|
Packit |
67cb25 |
trust_state,
|
|
Packit |
67cb25 |
trust_state->solver_state);
|
|
Packit |
67cb25 |
if (status)
|
|
Packit |
67cb25 |
return status;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
anorm = gsl_blas_dnrm2(state->acc);
|
|
Packit |
67cb25 |
vnorm = gsl_blas_dnrm2(state->vel);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* store |a| / |v| */
|
|
Packit |
67cb25 |
*(trust_state->avratio) = anorm / vnorm;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute step dx = v + 1/2 a */
|
|
Packit |
67cb25 |
scaled_addition(1.0, state->vel, 0.5, state->acc, dx);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/*
|
|
Packit |
67cb25 |
lm_preduction()
|
|
Packit |
67cb25 |
Compute predicted reduction using Eq 4.4 of More 1978
|
|
Packit |
67cb25 |
*/
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static int
|
|
Packit |
67cb25 |
lm_preduction(const void * vtrust_state, const gsl_vector * dx,
|
|
Packit |
67cb25 |
double * pred, void * vstate)
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trust_state *trust_state =
|
|
Packit |
67cb25 |
(const gsl_multifit_nlinear_trust_state *) vtrust_state;
|
|
Packit |
67cb25 |
lm_state_t *state = (lm_state_t *) vstate;
|
|
Packit |
67cb25 |
const gsl_vector *diag = trust_state->diag;
|
|
Packit |
67cb25 |
const gsl_vector *p = state->vel;
|
|
Packit |
67cb25 |
const double norm_Dp = scaled_enorm(diag, p);
|
|
Packit |
67cb25 |
const double normf = gsl_blas_dnrm2(trust_state->f);
|
|
Packit |
67cb25 |
const double mu = *(trust_state->mu);
|
|
Packit |
67cb25 |
double norm_Jp;
|
|
Packit |
67cb25 |
double u, v;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
(void)dx;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute work = J*p */
|
|
Packit |
67cb25 |
gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, p, 0.0, state->workn);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
/* compute ||J*p|| */
|
|
Packit |
67cb25 |
norm_Jp = gsl_blas_dnrm2(state->workn);
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
u = norm_Jp / normf;
|
|
Packit |
67cb25 |
v = norm_Dp / normf;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
*pred = u * u + 2.0 * mu * v * v;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
return GSL_SUCCESS;
|
|
Packit |
67cb25 |
}
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static const gsl_multifit_nlinear_trs lm_type =
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
"levenberg-marquardt",
|
|
Packit |
67cb25 |
lm_alloc_noaccel,
|
|
Packit |
67cb25 |
lm_init,
|
|
Packit |
67cb25 |
lm_preloop,
|
|
Packit |
67cb25 |
lm_step,
|
|
Packit |
67cb25 |
lm_preduction,
|
|
Packit |
67cb25 |
lm_free
|
|
Packit |
67cb25 |
};
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_lm = &lm_type;
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
static const gsl_multifit_nlinear_trs lmaccel_type =
|
|
Packit |
67cb25 |
{
|
|
Packit |
67cb25 |
"levenberg-marquardt+accel",
|
|
Packit |
67cb25 |
lm_alloc_accel,
|
|
Packit |
67cb25 |
lm_init,
|
|
Packit |
67cb25 |
lm_preloop,
|
|
Packit |
67cb25 |
lm_step,
|
|
Packit |
67cb25 |
lm_preduction,
|
|
Packit |
67cb25 |
lm_free
|
|
Packit |
67cb25 |
};
|
|
Packit |
67cb25 |
|
|
Packit |
67cb25 |
const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_lmaccel = &lmaccel_type;
|