SINFONI Pipeline Reference Manual  2.6.0
sinfo_fit.c
1 /*
2  * This file is part of the ESO SINFONI Pipeline
3  * Copyright (C) 2004,2005 European Southern Observatory
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, 51 Franklin St, Fifth Floor, Boston, MA 02111-1307 USA
18  */
19 #ifdef HAVE_CONFIG_H
20 # include <config.h>
21 #endif
22 #include <sinfo_fit.h>
23 #include <sinfo_msg.h>
24 #include <sinfo_utils_wrappers.h>
25 #include <sinfo_error.h>
26 #include <stdio.h>
27 #include <math.h>
28 #include <cpl.h>
36 /* function prototypes */
37 /*
38  static int
39  sinfo_stat_rectangle(cpl_image* img,
40  const int kappa,
41  const int nclip,
42  double *mean,
43  double *stdev);
44  */
45 static void
46 sinfo_fit_amoeba_get_psum(int ndim, int mpts, double** p, double* psum);
47 
48 
49 static double
50 sinfo_fit_amotry(double** p,
51  double y[],
52  double psum[],
53  int ndim,
54  double (*funk)(double[]),
55  int ihi,
56  double fac);
57 
58 
59 
60 static double
61 sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
62  double *sppp, int n);
63 
64 static double
65 sinfo_get_chisq(int N, int D,
66  int (*f)(const double x[], const double a[], double *result),
67  const double *a,
68  const double *x,
69  const double *y,
70  const double *sigma);
71 
72 static int sinfo_get_candidate(const double *a, const int ia[],
73  int M, int N, int D,
74  double lambda,
75  int (*f)(const double x[], const double a[],
76  double *result),
77  int (*dfda)(const double x[], const double a[],
78  double result[]),
79  const double *x,
80  const double *y,
81  const double *sigma,
82  double *partials,
83  cpl_matrix *alpha,
84  cpl_matrix *beta,
85  double *a_da);
86 
87 
88 #define SINFO_FIT_AMOEBA_NMAX 5000
89 
90 /*-------------------------------------------------------------------------*/
91 
103 static void
104 sinfo_fit_amoeba_get_psum(int ndim, int mpts, double** p, double* psum)
105 {
106  int i=0;
107  int j=0;
108  double sum=0;
109  for (j=0;j<ndim;j++) {
110  for (sum=0.0,i=0;i<mpts;i++) {
111  sum += p[i][j];
112  }
113  psum[j]=sum;
114  }
115 
116 }
117 
118 
119 #define SINFO_FIT_AMOEBA_SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
120 
121 
122 
149 void
150 sinfo_fit_amoeba(
151  double**p,
152  double y[],
153  int ndim,
154  double ftol,
155  double (*funk)(double[]),
156  int* nfunk)
157 {
158 
159 
160  int i=0;
161  int ihi=0;
162  int ilo=0;
163  int inhi=0;
164  int j=0;
165  int mpts=ndim+1;
166  double rtol=0;
167  double swap=0;
168  double ysave=0;
169  double ytry=0;
170  cpl_vector* sum=NULL;
171  double* psum=NULL;
172 
173  sum=cpl_vector_new(ndim);
174  psum=cpl_vector_get_data(sum);
175  *nfunk=0;
176 
177  sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);
178 
179  for(;;) {
180  ilo=0;
181  /*
182  First we must determine which point is the highest (worst),
183  next-highest, and lowest (best), by looping over the points
184  in the simplex
185  */
186  ihi=y[0]>y[1] ? (inhi=1,0) : (inhi=0,1);
187 
188  for (i=0;i< mpts;i++) {
189  if (y[i] <= y[ilo]) ilo=i;
190  if (y[i] > y[ihi]) {
191  inhi=ihi;
192  ihi=i;
193  } else if (y[i] > y[inhi] && i != ihi) inhi=i;
194  }
195  rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo]));
196 
197  /*
198  compute the fractional range from highest to lowest and return if
199  satisfactory
200  */
201  if(rtol < ftol) { // if returning, but best point and value is in slot 1
202  SINFO_FIT_AMOEBA_SWAP(y[0],y[ilo]);
203  for (i=0;i<ndim;i++) SINFO_FIT_AMOEBA_SWAP(p[1][i],p[ilo][i]);
204  break;
205  }
206  if (*nfunk >= SINFO_FIT_AMOEBA_NMAX) {
207  sinfo_msg_error("NMAX exceeded");
208  SINFO_FIT_AMOEBA_SWAP(y[0],y[ilo]);
209  for (i=0;i<ndim;i++) SINFO_FIT_AMOEBA_SWAP(p[1][i],p[ilo][i]);
210  for (i=0;i<ndim;i++) {
211  sinfo_msg("p[1][i]=%g p[ilo][i]=%g ilo=%d",p[1][i],p[ilo][i],ilo);
212  }
213 
214  assure(*nfunk >= SINFO_FIT_AMOEBA_NMAX,CPL_ERROR_UNSPECIFIED,
215  "NMAX exceeded");
216  break;
217 
218  }
219  *nfunk +=2;
220  /*
221  Begin a new iteration. First extrapolate by a Factor -1 through the face
222  of the simplex across the high point, i.e. reflect the simplex from the
223  high point
224  */
225  ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,-1.0);
226  if(ytry <= y[ilo]) {
227  /*
228  Gives a result better than the best point, so try an additional
229  extrapolation by a factor 2
230  */
231  ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,2.0);
232  } else if (ytry >= y[inhi]) {
233 
234  /*
235  The reflected point is worse than the second highest, so look for an
236  intermediate lower point, i.e. do a one-dimensional contraction
237  */
238  ysave=y[ihi];
239  ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,0.5);
240  if(ytry >= ysave) {
241  /*
242  Can't seem to get rid of that high point.
243  Better contract around the lowest (best) point
244  */
245  for(i=0;i<mpts;i++) {
246  if(i != ilo) {
247  for( j=0;j<ndim;j++) {
248  p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
249  }
250  y[i]=(*funk)(psum);
251  }
252  }
253  *nfunk += ndim; /* Keep track of function evaluations */
254  sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);/* Recomputes psum */
255  }
256  } else {
257  --(*nfunk);
258  /* Go back for the test of doneness and the next iteration */
259  }
260  }
261  cleanup:
262  cpl_vector_delete(sum);
263 }
264 
265 
266 static double
267 sinfo_fit_amotry(double** p, double y[], double psum[], int ndim,
268  double (*funk)(double[]),int ihi, double fac)
269 {
270  int j;
271  double fac1=0;
272  double fac2=0;
273  double ytry=0;
274  cpl_vector * vtry=NULL;
275  double *ptry=NULL;
276 
277  vtry=cpl_vector_new(ndim);
278  ptry=cpl_vector_get_data(vtry);
279 
280  fac1=(1.0-fac)/ndim;
281  fac2=fac1-fac;
282 
283  for (j=0;j<ndim;j++) {
284  ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
285  }
286  ytry=(*funk)(ptry);
287  if (ytry < y[ihi]) {
288  y[ihi]=ytry;
289  for (j=0;j<ndim;j++) {
290  psum[j] += ptry[j]-p[ihi][j];
291  p[ihi][j]=ptry[j];
292  }
293  }
294  sinfo_free_my_vector(&vtry);
295  return ytry;
296 }
297 
298 
299 
314 double
315 sinfo_amsub(double d0[], double d1[], double d2[], double value[],
316  double range[], double tol, int ivorf[], int ncon, int nref, double
317  (*ftbm)(double[], int ncon))
318 
319 {
320  double alpha = 1.0, loc_gamma = 1.5;
321  double sf, bsave, temp, sum, cval, ccval, beta;
322  int idone, nvar, nvec, nrefl, i, j, k, kd1, kval, isign, jvar, imin, imax,
323  i2max, it1, it2, itemp;
324  idone = 0;
325  isign = 1;
326  // we require that nvec=nvar+1, define nvar
327  nvar = 0;
328  for (i = 0; i < ncon; ++i) {
329  if (ivorf[i] == 1) {
330  nvar = nvar + 1;
331  }
332  }
333  nvec = nvar + 1;
334  // sf is the 'shrink' factor
335  sf = 1e5 * nvec;
336  nrefl = 0;
337  value[0] = (*ftbm)(d2, ncon);
338  // sinfo_msg("value[0] = %lg",value[0]);
339  // initial and shrink calculation of the d1 array, uses range
340  // set d2 in the first position -- using Fortran convention of 1st
341  // array element moving first
342  cont20: kd1 = -1;
343  for (i = 0; i < ncon; ++i) {
344  if (ivorf[i] == 1) {
345  kd1 = kd1 + 1;
346  d1[kd1] = d2[i];
347  }
348  }
349  // now for the next nvar values
350  kval = 0;
351  for (jvar = 0; jvar < ncon; ++jvar) {
352  if (ivorf[jvar] == 1) {
353  kval = kval + 1;
354  bsave = d2[jvar];
355  isign = -isign;
356  d2[jvar] = d2[jvar] + isign * range[jvar];
357  value[kval] = (*ftbm)(d2, ncon);
358  for (i = 0; i < ncon; ++i) {
359  if (ivorf[i] == 1) {
360  kd1 = kd1 + 1;
361  d1[kd1] = d2[i];
362  }
363  }
364  d2[jvar] = bsave;
365  }
366  }
367  // sinfo_msg(" d1 ");
368  // for (j=0;j<nvec;++j) {
369  // for(i=0;i<nvar;++i) {
370  // sinfo_msg("%12.4lg ",d1[i+j*nvar]);
371  // }
372  // }
373  /* find highest, second highest, and minimum values
374  imax points to the vector with the largest value
375  i2max points to the vector with the second largest value
376  imin points to the vector with the smallest value */
377  cont40: imin = 1;
378  if (value[0] > value[1]) {
379  imax = 0;
380  i2max = 1;
381  }
382  else {
383  imax = 1;
384  i2max = 0;
385  }
386  for (i = 0; i < nvec; ++i) {
387  if (value[i] < value[imin])
388  imin = i;
389  if (value[i] > value[imax]) {
390  i2max = imax;
391  imax = i;
392  }
393  else if ((value[i] > value[i2max]) && (i != imax)) {
394  i2max = i;
395  }
396  }
397  // sinfo_msg(" values after sorting ");
398  // for(i=0;i<nvec;++i)sinfo_msg("%12.4lg ",value[i]);
399  // sinfo_msg("imin %d,i2max %d,imax %d",imin,i2max,imax);
400  // scanf("%d",&itest);
401 
402  // check if done
403 
404  if (nrefl >= nref) {
405  sinfo_msg(" maximum number of reflection reached");
406  idone = 1;
407  goto cont400;
408  }
409  if (value[imin] != 0.0) {
410  temp = (value[imax] - value[imin]) / value[imin];
411  if (fabs(temp) <= tol) {
412  sinfo_msg(" reached tolerance %lg temp %lg tol", temp, tol);
413  idone = 1;
414  goto cont400;
415  }
416  }
417  if (value[imax] - value[imin] <= tol) {
418  sinfo_msg("value[max]-value[min]<=tol");
419  idone = 1;
420  goto cont400;
421  }
422 
423  // *** form d0 the average of all but imax
424  for (j = 0; j < nvar; ++j) {
425  sum = 0.0;
426  for (i = 0; i < nvec; ++i) {
427  if (i != imax)
428  sum = sum + d1[i * nvar + j];
429  }
430  d0[j] = sum / (nvec - 1);
431  }
432  // sinfo_msg(" D0 values ");
433  // for(i=0;i<nvar;++i)sinfo_msg("%12.4lg ",d0[i]);
434  // scanf("%d",&itest);
435  // reflection
436 
437  nrefl = nrefl + 1;
438  k = -1;
439  for (j = 0; j < ncon; ++j) {
440  if (ivorf[j] == 1) {
441  k = k + 1;
442  it1 = imax * nvar + k;
443  d2[j] = (1 + alpha) * d0[k] - alpha * d1[it1];
444  }
445  }
446 
447  // sinfo_msg(" refl d2 ");
448  // for(i=0;i<nvar;++i) sinfo_msg("%12.4lg ",d2[i]);
449 
450  cval = (*ftbm)(d2, ncon);
451  // sinfo_msg("refl ftbm %lg",cval);
452 
453  // value is higher than i2max so do contraction
454  if (cval >= value[i2max])
455  goto cont200;
456 
457  // value is less than i2max - normal - update d1 and value
458 
459  value[imax] = cval;
460  k = -1;
461  for (j = 0; j < ncon; ++j) {
462  if (ivorf[j] == 1) {
463  k = k + 1;
464  it1 = imax * nvar + k;
465  d1[it1] = d2[j];
466  }
467  }
468 
469  // value is less than imin, try expansion
470  if (cval < value[imin])
471  goto cont300;
472  goto cont40;
473 
474  // contraction
475  cont200:
476  // sinfo_msg(" contraction ");
477  beta = 0.75;
478  for (itemp = 0; itemp < 3; ++itemp) {
479  if (cval <= value[imax]) {
480  value[imax] = cval;
481  k = -1;
482  for (j = 0; j < ncon; ++j) {
483  if (ivorf[j] == 1) {
484  k = k + 1;
485  it1 = imax * nvar + k;
486  d1[it1] = d2[j];
487  }
488  }
489  }
490  k = -1;
491  for (j = 0; j < ncon; ++j) {
492  if (ivorf[j] == 1) {
493  k = k + 1;
494  it1 = imax * nvar + k;
495  d2[j] = beta * d1[it1] + (1. - beta) * d0[k];
496  }
497  }
498  cval = ftbm(d2, ncon);
499 
500  // sinfo_msg(" contraction beta %lg cval %lg ",beta,cval);
501  // value is better
502  if (cval < value[i2max]) {
503  value[imax] = cval;
504  k = -1;
505  for (j = 0; j < ncon; ++j) {
506  if (ivorf[j] == 1) {
507  k = k + 1;
508  it1 = imax * nvar + k;
509  d1[it1] = d2[j];
510  }
511  }
512  if (cval < value[imin])
513  sinfo_msg(" contraction minimum %lg", cval);
514  goto cont40;
515  }
516  beta = beta - 0.25;
517  }
518  sinfo_msg(" contraction failed ==>shrink");
519  // scanf("%d",&itest);
520  // value is worse so shrink it
521 
522  goto cont400;
523 
524  // expansion
525 
526  cont300:
527  sinfo_msg(" reflection min %lg \n", cval);
528  k = -1;
529  for (j = 0; j < ncon; ++j) {
530  if (ivorf[j] == 1) {
531  k = k + 1;
532  d2[j] = loc_gamma * d2[j] + (1. - loc_gamma) * d0[k];
533  }
534  }
535  ccval = (*ftbm)(d2, ncon);
536  // value is higher than reflected value ==> discard
537  if (ccval > cval)
538  goto cont40;
539  // value is better so use it rather than the reflected point
540  sinfo_msg(" expansion minimum %lg \n", ccval);
541  value[imax] = ccval;
542  k = -1;
543  for (j = 0; j < ncon; ++j) {
544  if (ivorf[j] == 1) {
545  k = k + 1;
546  it1 = imax * nvar + k;
547  d1[it1] = d2[j];
548  }
549  }
550  goto cont40;
551 
552  cont400:
553  // sinfo_msg(" following cont400 ");
554  // scanf("%d",&itest);
555  // recalculate d2 and range
556  // the range is the average of dist**2 from d1 with min value
557  k = -1;
558  for (j = 0; j < ncon; ++j) {
559  if (ivorf[j] == 1) {
560  k = k + 1;
561  it1 = imin * nvar + k;
562  d2[j] = d1[it1];
563  sum = 0.0;
564  for (i = 0; i < nvec; ++i) {
565  it1 = i * nvar + k;
566  it2 = imin * nvar + k;
567  sum = sum + (d1[it1] - d1[it2]) * (d1[it1] - d1[it2]);
568  }
569  range[j] = sf * sqrt(sum / (nvec - 1));
570  }
571  }
572  value[1] = value[imin];
573  sf = .75 * sf;
574  if (sf < 0.1)
575  idone = 1;
576  sinfo_msg(" shrink factor %lg ", sf);
577  if (idone != 1)
578  goto cont20;
579  return value[1];
580 
581 }
582 
583 static double
584 sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
585  double *sppp, int n)
586 {
587  double retval = 0;
588  double xm = 0;
589  double xm2 = 0;
590  double xm3 = 0;
591 
592  int i = 0;
593 
594  *sp = 0;
595  *spp = 0;
596  *sppp = 0;
597 
598  for (i = 0; i < n; ++i) {
599  if (ak[i] >= x) {
600  xm = ak[i] - x;
601  xm2 = xm * xm;
602  xm3 = xm * xm2;
603  sinfo_msg("cons=%g", cons[i]);
604  retval += cons[i] * xm3;
605  *sp -= 3 * cons[i] * xm2;
606  *spp += 6 * cons[i] * xm;
607  *sppp -= 6 * cons[i];
608  }
609  }
610  sinfo_msg("1x=%g retval=%g", x, retval);
611  return retval;
612 
613 }
614 
615 double
616 sinfo_ftbm(const double x, double cons[])
617 {
618  double retval = 0;
619  double ak[4] =
620  { -1, -.666666666666666, -.333333333333, 0 };
621  double sm1 = 0;
622  double spm1 = 0;
623  double sppm1 = 0;
624  double spppm1 = 0;
625 
626  int n = 4;
627 
628  sm1 = sinfo_spline(x, cons, ak, &spm1, &sppm1, &spppm1, n) - 1;
629  sinfo_msg("x=%g val=%g", x, sm1 + 1);
630 
631  retval = sm1 * sm1 + spm1 * spm1 + sppm1 * sppm1 + spppm1 * spppm1;
632  sinfo_msg("fitbm: x=%g retval=%g", x, retval);
633 
634  return retval;
635 
636 }
637 
638 /*----------------------------------------------------------------------------*/
659 /*----------------------------------------------------------------------------*/
660 
661 static double
662 sinfo_get_chisq(int N, int D,
663  int (*f)(const double x[], const double a[], double *result),
664  const double *a,
665  const double *x,
666  const double *y,
667  const double *sigma)
668 {
669  double chi_sq; /* Result */
670  int i = 0;
671 
672  /* For efficiency, don't check input in this static function */
673  chi_sq = 0.0;
674  for (i = 0; i < N; i++)
675  {
676  double fx_i;
677  double residual; /* Residual in units of uncertainty */
678  const double *x_i = &(x[0+i*D]);
679 
680  /* Evaluate */
681  cpl_ensure( f(x_i,
682  a,
683  &fx_i) == 0, CPL_ERROR_ILLEGAL_INPUT, -1.0);
684 
685  /* Accumulate */
686  if (sigma == NULL)
687  {
688  residual = (fx_i - y[i]);
689  }
690  else
691  {
692  residual = (fx_i - y[i]) / sigma[i];
693  }
694 
695  chi_sq += residual*residual;
696 
697  }
698 
699  return chi_sq;
700 }
701 
702 
703 /*----------------------------------------------------------------------------*/
737 /*----------------------------------------------------------------------------*/
738 static int
739 sinfo_get_candidate(const double *a, const int ia[],
740  int M, int N, int D,
741  double lambda,
742  int (*f)(const double x[], const double a[], double *result),
743  int (*dfda)(const double x[], const double a[], double result[]),
744  const double *x,
745  const double *y,
746  const double *sigma,
747  double *partials,
748  cpl_matrix *alpha,
749  cpl_matrix *beta,
750  double *a_da)
751 {
752  int Mfit = 0; /* Number of non-constant fit parameters */
753  cpl_matrix *da; /* Solution of alpha * da = beta */
754  double *alpha_data;
755  double *beta_data;
756  double *da_data;
757  int i, imfit = 0;
758  int j, jmfit = 0;
759  int k = 0;
760 
761  /* For efficiency, don't check input in this static function */
762 
763  Mfit = cpl_matrix_get_nrow(alpha);
764 
765  alpha_data = cpl_matrix_get_data(alpha);
766  beta_data = cpl_matrix_get_data(beta);
767 
768  /* Build alpha, beta:
769  *
770  * alpha[i,j] = sum_{k=1,N} (sigma_k)^-2 * df/da_i * df/da_j *
771  * (1 + delta_ij lambda) ,
772  *
773  * beta[i] = sum_{k=1,N} (sigma_k)^-2 * ( y_k - f(x_k) ) * df/da_i
774  *
775  * where (i,j) loop over the non-constant parameters (0 to Mfit-1),
776  * delta is Kronecker's delta, and all df/da are evaluated in x_k
777  */
778 
779  cpl_matrix_fill(alpha, 0.0);
780  cpl_matrix_fill(beta , 0.0);
781 
782  for (k = 0; k < N; k++)
783  {
784  double sm2 = 0.0; /* (sigma_k)^-2 */
785  double fx_k = 0.0; /* f(x_k) */
786  const double *x_k = &(x[0+k*D]); /* x_k */
787 
788  if (sigma == NULL)
789  {
790  sm2 = 1.0;
791  }
792  else
793  {
794  sm2 = 1.0 / (sigma[k] * sigma[k]);
795  }
796 
797  /* Evaluate f(x_k) */
798  cpl_ensure( f(x_k, a, &fx_k) == 0, CPL_ERROR_ILLEGAL_INPUT, -1);
799 
800  /* Evaluate (all) df/da (x_k) */
801  cpl_ensure( dfda(x_k, a, partials) == 0,
802  CPL_ERROR_ILLEGAL_INPUT, -1);
803 
804  for (i = 0, imfit = 0; i < M; i++)
805  {
806  if (ia[i] != 0)
807  {
808  /* Beta */
809  beta_data[imfit] +=
810  sm2 * (y[k] - fx_k) * partials[i];
811 
812  /* Alpha is symmetrical, so compute
813  only lower-left part */
814  for (j = 0, jmfit = 0; j < i; j++)
815  {
816  if (ia[j] != 0)
817  {
818  alpha_data[jmfit + imfit*Mfit] +=
819  sm2 * partials[i] *
820  partials[j];
821 
822  jmfit += 1;
823  }
824  }
825 
826  /* Alpha, diagonal terms */
827  j = i;
828  jmfit = imfit;
829 
830  alpha_data[jmfit + imfit*Mfit] +=
831  sm2 * partials[i] *
832  partials[j] * (1 + lambda);
833 
834  imfit += 1;
835  }
836  }
837 
838  cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
839  }
840 
841  /* Create upper-right part of alpha */
842  for (i = 0, imfit = 0; i < M; i++) {
843  if (ia[i] != 0) {
844  for (j = i+1, jmfit = imfit+1; j < M; j++) {
845  if (ia[j] != 0) {
846  alpha_data[jmfit+imfit*Mfit] = alpha_data[imfit+jmfit*Mfit];
847  jmfit += 1;
848  }
849  }
850  cpl_ensure( jmfit == Mfit,CPL_ERROR_ILLEGAL_INPUT,-1 );
851  imfit += 1;
852  }
853  }
854  cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
855 
856  da = cpl_matrix_solve(alpha, beta);
857 
858  cpl_ensure(da != NULL, cpl_error_get_code(), -1);
859 
860  /* Create a+da vector by adding a and da */
861  da_data = cpl_matrix_get_data(da);
862 
863  for (i = 0, imfit = 0; i < M; i++)
864  {
865  if (ia[i] != 0)
866  {
867  a_da[i] = a[i] + da_data[0 + imfit*1];
868 
869  imfit += 1;
870  }
871  else
872  {
873  a_da[i] = a[i];
874  }
875  }
876 
877  cpl_ensure( imfit == Mfit ,CPL_ERROR_ILLEGAL_INPUT,-1);
878 
879  cpl_matrix_delete(da);
880 
881  return 0;
882 }
883 
884 
885 #ifndef CPL_VECTOR_FIT_MAXITER
886 #define CPL_VECTOR_FIT_MAXITER 1000
887 #endif
888 /*----------------------------------------------------------------------------*/
955 /*----------------------------------------------------------------------------*/
956 cpl_error_code
957 sinfo_fit_lm(const cpl_matrix *x,
958  const cpl_matrix *sigma_x,
959  const cpl_vector *y,
960  const cpl_vector *sigma_y,
961  cpl_vector *a,
962  const int ia[],
963  int (*f)(const double x[],
964  const double a[],
965  double *result),
966  int (*dfda)(const double x[],
967  const double a[],
968  double result[]),
969  double *mse,
970  double *red_chisq,
971  cpl_matrix **covariance)
972 {
973  const double *x_data = NULL; /* Pointer to input data */
974  const double *y_data = NULL; /* Pointer to input data */
975  const double *sigma_data = NULL; /* Pointer to input data */
976  int N = 0; /* Number of data points */
977  int D = 0; /* Dimension of x-points */
978  int M = 0; /* Number of fit parameters */
979  int Mfit = 0; /* Number of non-constant fit
980  parameters */
981 
982  double lambda = 0.0; /* Lambda in L-M algorithm */
983  double MAXLAMBDA = 10e40; /* Parameter to control the graceful exit
984  if steepest descent unexpectedly fails */
985  double chi_sq = 0.0; /* Current chi^2 */
986  int count = 0; /* Number of successive small improvements
987  in chi^2 */
988  int iterations = 0;
989 
990  cpl_matrix *alpha = NULL; /* The MxM ~curvature matrix used in L-M */
991  cpl_matrix *beta = NULL; /* Mx1 matrix = -.5 grad(chi^2) */
992  double *a_data = NULL; /* Parameters, a */
993  double *a_da = NULL; /* Candidate position a+da */
994  double *part = NULL; /* The partial derivatives df/da */
995  int *ia_local = NULL; /* non-NULL version of ia */
996 
997  /* If covariance computation is requested, then either
998  * return the covariance matrix or return NULL.
999  */
1000  if (covariance != NULL) *covariance = NULL;
1001 
1002  /* Validate input */
1003  cpl_ensure_code(x != NULL, CPL_ERROR_NULL_INPUT);
1004  cpl_ensure_code(sigma_x == NULL, CPL_ERROR_UNSUPPORTED_MODE);
1005  cpl_ensure_code(y != NULL, CPL_ERROR_NULL_INPUT);
1006  cpl_ensure_code(a != NULL, CPL_ERROR_NULL_INPUT);
1007  /* ia may be NULL */
1008  cpl_ensure_code(f != NULL, CPL_ERROR_NULL_INPUT);
1009  cpl_ensure_code(dfda != NULL, CPL_ERROR_NULL_INPUT);
1010 
1011  /* Chi^2 and covariance computations require sigmas to be known */
1012  cpl_ensure_code( sigma_y != NULL ||
1013  (red_chisq == NULL && covariance == NULL),
1014  CPL_ERROR_INCOMPATIBLE_INPUT);
1015 
1016  D = cpl_matrix_get_ncol(x);
1017  N = cpl_matrix_get_nrow(x);
1018  M = cpl_vector_get_size(a);
1019  cpl_ensure_code(N > 0 && D > 0 && M > 0, CPL_ERROR_ILLEGAL_INPUT);
1020 
1021  cpl_ensure_code( cpl_vector_get_size(y) == N,
1022  CPL_ERROR_INCOMPATIBLE_INPUT);
1023 
1024  x_data = cpl_matrix_get_data_const(x);
1025  y_data = cpl_vector_get_data_const(y);
1026  a_data = cpl_vector_get_data(a);
1027 
1028  if (sigma_y != NULL)
1029  {
1030  cpl_ensure_code( cpl_vector_get_size(sigma_y) == N,
1031  CPL_ERROR_INCOMPATIBLE_INPUT);
1032  /* Sigmas must be positive */
1033  cpl_ensure_code( cpl_vector_get_min (sigma_y) > 0,
1034  CPL_ERROR_ILLEGAL_INPUT);
1035  sigma_data = cpl_vector_get_data_const(sigma_y);
1036  }
1037 
1038  ia_local = cpl_malloc(M * sizeof(int));
1039  cpl_ensure_code(ia_local != NULL, CPL_ERROR_ILLEGAL_OUTPUT);
1040 
1041  /* Count non-constant fit parameters, copy ia */
1042  if (ia != NULL)
1043  {
1044  int i;
1045 
1046  Mfit = 0;
1047  for (i = 0; i < M; i++)
1048  {
1049  ia_local[i] = ia[i];
1050 
1051  if (ia[i] != 0)
1052  {
1053  Mfit += 1;
1054  }
1055  }
1056 
1057  if (! (Mfit > 0))
1058  {
1059  cpl_free(ia_local);
1060  cpl_ensure_code( CPL_FALSE,
1061  CPL_ERROR_ILLEGAL_INPUT);
1062  }
1063  }
1064  else
1065  {
1066  /* All parameters participate */
1067  int i;
1068 
1069  Mfit = M;
1070 
1071  for (i = 0; i < M; i++)
1072  {
1073  ia_local[i] = 1;
1074  }
1075  }
1076 
1077  /* To compute reduced chi^2, we need N > Mfit */
1078  if (! ( red_chisq == NULL || N > Mfit ) )
1079  {
1080  cpl_free(ia_local);
1081  cpl_ensure_code(
1082  CPL_FALSE,
1083  CPL_ERROR_ILLEGAL_INPUT);
1084  }
1085 
1086  /* Create alpha, beta, a_da, part work space */
1087  alpha = cpl_matrix_new(Mfit, Mfit);
1088  if (alpha == NULL)
1089  {
1090  cpl_free(ia_local);
1091  cpl_ensure_code(
1092  CPL_FALSE,
1093  CPL_ERROR_ILLEGAL_OUTPUT);
1094  }
1095 
1096  beta = cpl_matrix_new(Mfit, 1);
1097  if (beta == NULL)
1098  {
1099  cpl_free(ia_local);
1100  cpl_matrix_delete(alpha);
1101  cpl_ensure_code(
1102  CPL_FALSE,
1103  CPL_ERROR_ILLEGAL_OUTPUT);
1104  }
1105 
1106  a_da = cpl_malloc(M * sizeof(double));
1107  if (a_da == NULL)
1108  {
1109  cpl_free(ia_local);
1110  cpl_matrix_delete(alpha);
1111  cpl_matrix_delete(beta);
1112  cpl_ensure_code(
1113  CPL_FALSE,
1114  CPL_ERROR_ILLEGAL_OUTPUT);
1115  }
1116 
1117  part = cpl_malloc(M * sizeof(double));
1118  if (part == NULL)
1119  {
1120  cpl_free(ia_local);
1121  cpl_matrix_delete(alpha);
1122  cpl_matrix_delete(beta);
1123  cpl_free(a_da);
1124  cpl_ensure_code(
1125  CPL_FALSE,
1126  CPL_ERROR_ILLEGAL_OUTPUT);
1127  }
1128 
1129  /* Initialize loop variables */
1130  lambda = 0.001;
1131  count = 0;
1132  iterations = 0;
1133  if( (chi_sq = sinfo_get_chisq(N, D, f, a_data, x_data, y_data, sigma_data)) < 0)
1134  {
1135  cpl_free(ia_local);
1136  cpl_matrix_delete(alpha);
1137  cpl_matrix_delete(beta);
1138  cpl_free(a_da);
1139  cpl_free(part);
1140  cpl_ensure_code(
1141  CPL_FALSE,
1142  cpl_error_get_code());
1143  }
1144 
1145  /* uves_msg_debug("Initial chi^2 = %f", chi_sq); */
1146 
1147  /* Iterate until chi^2 didn't improve substantially many (say, 5)
1148  times in a row */
1149  while (count < 5 &&
1150  lambda < MAXLAMBDA &&
1151  iterations < CPL_VECTOR_FIT_MAXITER)
1152  {
1153  /* In each iteration lambda increases, or chi^2 decreases or
1154  count increases. Because chi^2 is bounded from below
1155  (and lambda and count from above), the loop will terminate */
1156 
1157  double chi_sq_candidate = 0.0;
1158  int returncode = 0;
1159 
1160  /* Get candidate position in parameter space = a+da,
1161  * where alpha * da = beta .
1162  * Increase lambda until alpha is non-singular
1163  */
1164 
1165  while( (returncode = sinfo_get_candidate(a_data, ia_local,
1166  M, N, D,
1167  lambda, f, dfda,
1168  x_data, y_data, sigma_data,
1169  part, alpha, beta, a_da)
1170  ) != 0
1171  && cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX
1172  && lambda < MAXLAMBDA)
1173  {
1174  /* uves_msg_debug("Singular matrix. lambda = %e", lambda); */
1175  cpl_error_reset();
1176  lambda *= 9.0;
1177  }
1178 
1179  /* Set error if lambda diverged */
1180  if ( !( lambda < MAXLAMBDA ) )
1181  {
1182  cpl_free(ia_local);
1183  cpl_matrix_delete(alpha);
1184  cpl_matrix_delete(beta);
1185  cpl_free(a_da);
1186  cpl_free(part);
1187  cpl_ensure_code(
1188  CPL_FALSE,
1189  CPL_ERROR_CONTINUE);
1190  }
1191 
1192  if (returncode != 0)
1193  {
1194  cpl_free(ia_local);
1195  cpl_matrix_delete(alpha);
1196  cpl_matrix_delete(beta);
1197  cpl_free(a_da);
1198  cpl_free(part);
1199  cpl_ensure_code(
1200  CPL_FALSE,
1201  cpl_error_get_code());
1202  }
1203 
1204  /* Get chi^2(a+da) */
1205  if ((chi_sq_candidate = sinfo_get_chisq(N, D, f, a_da,
1206  x_data, y_data, sigma_data)) < 0)
1207  {
1208  cpl_free(ia_local);
1209  cpl_matrix_delete(alpha);
1210  cpl_matrix_delete(beta);
1211  cpl_free(a_da);
1212  cpl_free(part);
1213  cpl_ensure_code(
1214  CPL_FALSE,
1215  cpl_error_get_code());
1216  }
1217 
1218  /* uves_msg_debug("Chi^2 = %f Candidate = %f Lambda = %e",
1219  chi_sq, chi_sq_candidate, lambda); */
1220 
1221  if (chi_sq_candidate > chi_sq)
1222  {
1223  /* Move towards steepest descent */
1224  lambda *= 9.0;
1225  }
1226  else
1227  {
1228  /* Move towards Newton's algorithm */
1229  lambda /= 10.0;
1230 
1231  /* Count the number of successive improvements in chi^2 of
1232  less than 0.01 relative */
1233  if ( chi_sq == 0 ||
1234  (chi_sq - chi_sq_candidate)/chi_sq < .01)
1235  {
1236  count += 1;
1237  }
1238  else
1239  {
1240  /* Chi^2 improved by a significant amount,
1241  reset counter */
1242  count = 0;
1243  }
1244 
1245  /* chi^2 improved, update a and chi^2 */
1246  {
1247  int i;
1248  for (i = 0; i < M; i++) a_data[i] = a_da[i];
1249  }
1250  chi_sq = chi_sq_candidate;
1251  }
1252  iterations++;
1253  }
1254 
1255  /* Set error if we didn't converge */
1256  if ( !( lambda < MAXLAMBDA && iterations < CPL_VECTOR_FIT_MAXITER ) )
1257  {
1258  cpl_free(ia_local);
1259  cpl_matrix_delete(alpha);
1260  cpl_matrix_delete(beta);
1261  cpl_free(a_da);
1262  cpl_free(part);
1263  cpl_ensure_code(
1264  CPL_FALSE,
1265  CPL_ERROR_CONTINUE);
1266  }
1267 
1268  /* Compute mse if requested */
1269  if (mse != NULL)
1270  {
1271  int i;
1272 
1273  *mse = 0.0;
1274 
1275  for(i = 0; i < N; i++)
1276  {
1277  double fx_i = 0.0;
1278  double residual = 0.0;
1279 
1280  /* Evaluate f(x_i) at the best fit parameters */
1281  if( f(&(x_data[i*D]),
1282  a_data,
1283  &fx_i) != 0)
1284  {
1285  cpl_free(ia_local);
1286  cpl_matrix_delete(alpha);
1287  cpl_matrix_delete(beta);
1288  cpl_free(a_da);
1289  cpl_free(part);
1290  cpl_ensure_code(
1291  CPL_FALSE,
1292  CPL_ERROR_ILLEGAL_INPUT);
1293  }
1294 
1295  residual = y_data[i] - fx_i;
1296  *mse += residual * residual;
1297  }
1298  *mse /= N;
1299  }
1300 
1301  /* Compute reduced chi^2 if requested */
1302  if (red_chisq != NULL)
1303  {
1304  /* We already know the optimal chi^2 (and that N > Mfit)*/
1305  *red_chisq = chi_sq / (N-Mfit);
1306  }
1307 
1308  /* Compute covariance matrix if requested
1309  * cov = alpha(lambda=0)^-1
1310  */
1311  if (covariance != NULL)
1312  {
1313  cpl_matrix *cov;
1314 
1315  if( sinfo_get_candidate(a_data, ia_local,
1316  M, N, D, 0.0, f, dfda,
1317  x_data, y_data, sigma_data,
1318  part, alpha, beta, a_da)
1319  != 0)
1320  {
1321  cpl_free(ia_local);
1322  cpl_matrix_delete(alpha);
1323  cpl_matrix_delete(beta);
1324  cpl_free(a_da);
1325  cpl_free(part);
1326  cpl_ensure_code(
1327  CPL_FALSE,
1328  cpl_error_get_code());
1329  }
1330 
1331  cov = cpl_matrix_invert_create(alpha);
1332  if (cov == NULL)
1333  {
1334  cpl_free(ia_local);
1335  cpl_matrix_delete(alpha);
1336  cpl_matrix_delete(beta);
1337  cpl_free(a_da);
1338  cpl_free(part);
1339  cpl_ensure_code(
1340  CPL_FALSE,
1341  cpl_error_get_code());
1342  }
1343 
1344  /* Make sure that variances are positive */
1345  {
1346  int i;
1347  for (i = 0; i < Mfit; i++)
1348  {
1349  if ( !(cpl_matrix_get(cov, i, i) > 0) )
1350  {
1351  cpl_free(ia_local);
1352  cpl_matrix_delete(alpha);
1353  cpl_matrix_delete(beta);
1354  cpl_free(a_da);
1355  cpl_free(part);
1356  cpl_matrix_delete(cov);
1357  *covariance = NULL;
1358  cpl_ensure_code(
1359  CPL_FALSE,
1360  CPL_ERROR_SINGULAR_MATRIX);
1361  }
1362  }
1363  }
1364 
1365  /* Expand covariance matrix from Mfit x Mfit
1366  to M x M. Set rows/columns corresponding to fixed
1367  parameters to zero */
1368 
1369  *covariance = cpl_matrix_new(M, M);
1370  if (*covariance == NULL)
1371  {
1372  cpl_free(ia_local);
1373  cpl_matrix_delete(alpha);
1374  cpl_matrix_delete(beta);
1375  cpl_free(a_da);
1376  cpl_free(part);
1377  cpl_matrix_delete(cov);
1378  cpl_ensure_code(
1379  CPL_FALSE,
1380  CPL_ERROR_ILLEGAL_OUTPUT);
1381  }
1382 
1383  {
1384  int j, jmfit;
1385 
1386  for (j = 0, jmfit = 0; j < M; j++)
1387  if (ia_local[j] != 0)
1388  {
1389  int i, imfit;
1390 
1391  for (i = 0, imfit = 0; i < M; i++)
1392  if (ia_local[i] != 0)
1393  {
1394  cpl_matrix_set(*covariance, i, j,
1395  cpl_matrix_get(
1396  cov, imfit, jmfit));
1397  imfit += 1;
1398  }
1399 
1400  cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
1401 
1402  jmfit += 1;
1403  }
1404 
1405  cpl_ensure( jmfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1 );
1406  }
1407 
1408  cpl_matrix_delete(cov);
1409  }
1410 
1411  cpl_free(ia_local);
1412  cpl_matrix_delete(alpha);
1413  cpl_matrix_delete(beta);
1414  cpl_free(a_da);
1415  cpl_free(part);
1416 
1417  return CPL_ERROR_NONE;
1418 }
1419 
1420 
1421 
1422 
#define sinfo_msg_error(...)
Print an error message.
Definition: sinfo_msg.h:69