/* multifit/lmmisc.c
*
* Copyright (C) 2014 Patrick Alken
*
* 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.
*/
/* compute step dx by solving (J^T J + mu*I) dx = -J^T f */
static int
lmniel_calc_dx(const double mu, const gsl_matrix *A, const gsl_vector *rhs,
gsl_vector *dx, lmniel_state_t *state)
{
int status;
gsl_matrix *A_copy = state->A_copy;
gsl_vector_view diag = gsl_matrix_diagonal(A_copy);
/* make a copy of J^T J matrix */
gsl_matrix_memcpy(A_copy, A);
/* augment normal equations with LM parameter: A -> A + mu*I */
gsl_vector_add_constant(&diag.vector, mu);
status = gsl_linalg_QR_decomp(A_copy, state->work);
if (status)
return status;
status = gsl_linalg_QR_solve(A_copy, state->work, rhs, dx);
if (status)
return status;
return GSL_SUCCESS;
} /* lmniel_calc_dx() */
/* compute x_trial = x + dx */
static void
lmniel_trial_step(const gsl_vector * x, const gsl_vector * dx,
gsl_vector * x_trial)
{
size_t i, N = x->size;
for (i = 0; i < N; i++)
{
double dxi = gsl_vector_get (dx, i);
double xi = gsl_vector_get (x, i);
gsl_vector_set (x_trial, i, xi + dxi);
}
} /* lmniel_trial_step() */
/*
lmniel_calc_dF()
Compute dF = F(x) - F(x + dx) = 1/2 (f - f_new)^T (f + f_new)
*/
static double
lmniel_calc_dF(const gsl_vector *f, const gsl_vector *f_new)
{
const size_t N = f->size;
size_t i;
double dF = 0.0;
for (i = 0; i < N; ++i)
{
double fi = gsl_vector_get(f, i);
double fnewi = gsl_vector_get(f_new, i);
dF += (fi - fnewi) * (fi + fnewi);
}
dF *= 0.5;
return dF;
} /* lmniel_calc_dF() */
/*
lmniel_calc_dL()
Compute dL = L(0) - L(dx) = 1/2 dx^T (mu * D^T D dx - g)
Here, the mg input is -g
*/
static double
lmniel_calc_dL(const double mu, const gsl_vector *diag,
const gsl_vector *dx, const gsl_vector *mg)
{
const size_t p = dx->size;
size_t i;
double dL = 0.0;
for (i = 0; i < p; ++i)
{
double dxi = gsl_vector_get(dx, i);
double di = gsl_vector_get(diag, i);
double mgi = gsl_vector_get(mg, i); /* -g_i */
dL += dxi * (mu * di * di * dxi + mgi);
}
dL *= 0.5;
return dL;
} /* lmniel_calc_dL() */