1 static int
iterate(void * vstate,const gsl_vector * swts,gsl_multifit_function_fdf * fdf,gsl_vector * x,gsl_vector * f,gsl_vector * dx,int scale)2 iterate (void *vstate, const gsl_vector * swts,
3 gsl_multifit_function_fdf * fdf, gsl_vector * x,
4 gsl_vector * f, gsl_vector * dx, int scale)
5 {
6 lmder_state_t *state = (lmder_state_t *) vstate;
7
8 gsl_matrix *r = state->r;
9 gsl_vector *tau = state->tau;
10 gsl_vector *diag = state->diag;
11 gsl_vector *qtf = state->qtf;
12 gsl_vector *x_trial = state->x_trial;
13 gsl_vector *f_trial = state->f_trial;
14 gsl_vector *rptdx = state->rptdx;
15 gsl_vector *newton = state->newton;
16 gsl_vector *gradient = state->gradient;
17 gsl_vector *sdiag = state->sdiag;
18 gsl_vector *w = state->w;
19 gsl_vector *work1 = state->work1;
20 gsl_permutation *perm = state->perm;
21
22 double prered, actred;
23 double pnorm, fnorm1, fnorm1p, gnorm;
24 double ratio;
25 double dirder;
26
27 int iter = 0;
28
29 double p1 = 0.1, p25 = 0.25, p5 = 0.5, p75 = 0.75, p0001 = 0.0001;
30
31 if (state->fnorm == 0.0)
32 {
33 return GSL_SUCCESS;
34 }
35
36 /* Compute norm of scaled gradient */
37
38 compute_gradient_direction (r, perm, qtf, diag, gradient);
39
40 {
41 size_t iamax = gsl_blas_idamax (gradient);
42
43 gnorm = fabs(gsl_vector_get (gradient, iamax) / state->fnorm);
44 }
45
46 /* Determine the Levenberg-Marquardt parameter */
47
48 lm_iteration:
49
50 iter++ ;
51
52 {
53 int status = lmpar (r, perm, qtf, diag, state->delta, &(state->par), newton, gradient, sdiag, dx, w);
54 if (status)
55 return status;
56 }
57
58 /* Take a trial step */
59
60 gsl_vector_scale (dx, -1.0); /* reverse the step to go downhill */
61
62 compute_trial_step (x, dx, state->x_trial);
63
64 pnorm = scaled_enorm (diag, dx);
65
66 if (state->iter == 1)
67 {
68 if (pnorm < state->delta)
69 {
70 #ifdef DEBUG
71 printf("set delta = pnorm = %g\n" , pnorm);
72 #endif
73 state->delta = pnorm;
74 }
75 }
76
77 /* Evaluate function at x + p */
78 /* return immediately if evaluation raised error */
79 {
80 int status = gsl_multifit_eval_wf (fdf, x_trial, swts, f_trial);
81 if (status)
82 return status;
83 }
84
85 fnorm1 = enorm (f_trial);
86
87 /* Compute the scaled actual reduction */
88
89 actred = compute_actual_reduction (state->fnorm, fnorm1);
90
91 #ifdef DEBUG
92 printf("lmiterate: fnorm = %g fnorm1 = %g actred = %g\n", state->fnorm, fnorm1, actred);
93 printf("r = "); gsl_matrix_fprintf(stdout, r, "%g");
94 printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");
95 printf("dx = "); gsl_vector_fprintf(stdout, dx, "%g");
96 #endif
97
98 /* Compute rptdx = R P^T dx, noting that |J dx| = |R P^T dx| */
99
100 compute_rptdx (r, perm, dx, rptdx);
101
102 #ifdef DEBUG
103 printf("rptdx = "); gsl_vector_fprintf(stdout, rptdx, "%g");
104 #endif
105
106 fnorm1p = enorm (rptdx);
107
108 /* Compute the scaled predicted reduction = |J dx|^2 + 2 par |D dx|^2 */
109
110 {
111 double t1 = fnorm1p / state->fnorm;
112 double t2 = (sqrt(state->par) * pnorm) / state->fnorm;
113
114 prered = t1 * t1 + t2 * t2 / p5;
115 dirder = -(t1 * t1 + t2 * t2);
116 }
117
118 /* compute the ratio of the actual to predicted reduction */
119
120 if (prered > 0)
121 {
122 ratio = actred / prered;
123 }
124 else
125 {
126 ratio = 0;
127 }
128
129 #ifdef DEBUG
130 printf("lmiterate: prered = %g dirder = %g ratio = %g\n", prered, dirder,ratio);
131 #endif
132
133
134 /* update the step bound */
135
136 if (ratio > p25)
137 {
138 #ifdef DEBUG
139 printf("ratio > p25\n");
140 #endif
141 if (state->par == 0 || ratio >= p75)
142 {
143 state->delta = pnorm / p5;
144 state->par *= p5;
145 #ifdef DEBUG
146 printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
147 #endif
148 }
149 }
150 else
151 {
152 double temp = (actred >= 0) ? p5 : p5*dirder / (dirder + p5 * actred);
153
154 #ifdef DEBUG
155 printf("ratio < p25\n");
156 #endif
157
158 if (p1 * fnorm1 >= state->fnorm || temp < p1 )
159 {
160 temp = p1;
161 }
162
163 state->delta = temp * GSL_MIN_DBL (state->delta, pnorm/p1);
164
165 state->par /= temp;
166 #ifdef DEBUG
167 printf("updated step bounds: delta = %g, par = %g\n", state->delta, state->par);
168 #endif
169 }
170
171
172 /* test for successful iteration, termination and stringent tolerances */
173
174 if (ratio >= p0001)
175 {
176 gsl_vector_memcpy (x, x_trial);
177 gsl_vector_memcpy (f, f_trial);
178
179 /* return immediately if evaluation raised error */
180 {
181 int status;
182
183 /* compute Jacobian at new x and store in state->r */
184 if (fdf->df)
185 status = gsl_multifit_eval_wdf (fdf, x_trial, swts, r);
186 else
187 status = gsl_multifit_fdfsolver_dif_df(x_trial, swts, fdf, f_trial, r);
188
189 if (status)
190 return status;
191 }
192
193 /* wa2_j = diag_j * x_j */
194 state->xnorm = scaled_enorm(diag, x);
195 state->fnorm = fnorm1;
196 state->iter++;
197
198 /* Rescale if necessary */
199
200 if (scale)
201 {
202 update_diag (r, diag);
203 }
204
205 /* compute J = Q R P^T and qtf = Q^T f */
206 {
207 int signum;
208
209 gsl_matrix_memcpy(state->J, r);
210 gsl_linalg_QRPT_decomp (r, tau, perm, &signum, work1);
211 gsl_vector_memcpy (qtf, f);
212 gsl_linalg_QR_QTvec (r, tau, qtf);
213 }
214
215 return GSL_SUCCESS;
216 }
217 else if (fabs(actred) <= GSL_DBL_EPSILON && prered <= GSL_DBL_EPSILON
218 && p5 * ratio <= 1.0)
219 {
220 return GSL_ETOLF ;
221 }
222 else if (state->delta <= GSL_DBL_EPSILON * state->xnorm)
223 {
224 return GSL_ETOLX;
225 }
226 else if (gnorm <= GSL_DBL_EPSILON)
227 {
228 return GSL_ETOLG;
229 }
230 else if (iter < 10)
231 {
232 /* Repeat inner loop if unsuccessful */
233 goto lm_iteration;
234 }
235
236 return GSL_ENOPROG;
237 }
238