22 #include <sinfo_fit.h>
23 #include <sinfo_msg.h>
24 #include <sinfo_utils_wrappers.h>
25 #include <sinfo_error.h>
46 sinfo_fit_amoeba_get_psum(
int ndim,
int mpts,
double** p,
double* psum);
50 sinfo_fit_amotry(
double** p,
54 double (*funk)(
double[]),
61 sinfo_spline(
double x,
double cons[],
double ak[],
double *sp,
double *spp,
65 sinfo_get_chisq(
int N,
int D,
66 int (*f)(
const double x[],
const double a[],
double *result),
72 static int sinfo_get_candidate(
const double *a,
const int ia[],
75 int (*f)(
const double x[],
const double a[],
77 int (*dfda)(
const double x[],
const double a[],
88 #define SINFO_FIT_AMOEBA_NMAX 5000
104 sinfo_fit_amoeba_get_psum(
int ndim,
int mpts,
double** p,
double* psum)
109 for (j=0;j<ndim;j++) {
110 for (sum=0.0,i=0;i<mpts;i++) {
119 #define SINFO_FIT_AMOEBA_SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
155 double (*funk)(
double[]),
170 cpl_vector* sum=NULL;
173 sum=cpl_vector_new(ndim);
174 psum=cpl_vector_get_data(sum);
177 sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);
186 ihi=y[0]>y[1] ? (inhi=1,0) : (inhi=0,1);
188 for (i=0;i< mpts;i++) {
189 if (y[i] <= y[ilo]) ilo=i;
193 }
else if (y[i] > y[inhi] && i != ihi) inhi=i;
195 rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo]));
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]);
206 if (*nfunk >= SINFO_FIT_AMOEBA_NMAX) {
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);
214 assure(*nfunk >= SINFO_FIT_AMOEBA_NMAX,CPL_ERROR_UNSPECIFIED,
225 ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,-1.0);
231 ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,2.0);
232 }
else if (ytry >= y[inhi]) {
239 ytry=sinfo_fit_amotry(p,y,psum,ndim,funk,ihi,0.5);
245 for(i=0;i<mpts;i++) {
247 for( j=0;j<ndim;j++) {
248 p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
254 sinfo_fit_amoeba_get_psum(ndim,mpts,p,psum);
262 cpl_vector_delete(sum);
267 sinfo_fit_amotry(
double** p,
double y[],
double psum[],
int ndim,
268 double (*funk)(
double[]),
int ihi,
double fac)
274 cpl_vector * vtry=NULL;
277 vtry=cpl_vector_new(ndim);
278 ptry=cpl_vector_get_data(vtry);
283 for (j=0;j<ndim;j++) {
284 ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
289 for (j=0;j<ndim;j++) {
290 psum[j] += ptry[j]-p[ihi][j];
294 sinfo_free_my_vector(&vtry);
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))
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;
328 for (i = 0; i < ncon; ++i) {
337 value[0] = (*ftbm)(d2, ncon);
343 for (i = 0; i < ncon; ++i) {
351 for (jvar = 0; jvar < ncon; ++jvar) {
352 if (ivorf[jvar] == 1) {
356 d2[jvar] = d2[jvar] + isign * range[jvar];
357 value[kval] = (*ftbm)(d2, ncon);
358 for (i = 0; i < ncon; ++i) {
378 if (value[0] > value[1]) {
386 for (i = 0; i < nvec; ++i) {
387 if (value[i] < value[imin])
389 if (value[i] > value[imax]) {
393 else if ((value[i] > value[i2max]) && (i != imax)) {
405 sinfo_msg(
" maximum number of reflection reached");
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);
417 if (value[imax] - value[imin] <= tol) {
418 sinfo_msg(
"value[max]-value[min]<=tol");
424 for (j = 0; j < nvar; ++j) {
426 for (i = 0; i < nvec; ++i) {
428 sum = sum + d1[i * nvar + j];
430 d0[j] = sum / (nvec - 1);
439 for (j = 0; j < ncon; ++j) {
442 it1 = imax * nvar + k;
443 d2[j] = (1 + alpha) * d0[k] - alpha * d1[it1];
450 cval = (*ftbm)(d2, ncon);
454 if (cval >= value[i2max])
461 for (j = 0; j < ncon; ++j) {
464 it1 = imax * nvar + k;
470 if (cval < value[imin])
478 for (itemp = 0; itemp < 3; ++itemp) {
479 if (cval <= value[imax]) {
482 for (j = 0; j < ncon; ++j) {
485 it1 = imax * nvar + k;
491 for (j = 0; j < ncon; ++j) {
494 it1 = imax * nvar + k;
495 d2[j] = beta * d1[it1] + (1. - beta) * d0[k];
498 cval = ftbm(d2, ncon);
502 if (cval < value[i2max]) {
505 for (j = 0; j < ncon; ++j) {
508 it1 = imax * nvar + k;
512 if (cval < value[imin])
513 sinfo_msg(
" contraction minimum %lg", cval);
518 sinfo_msg(
" contraction failed ==>shrink");
527 sinfo_msg(
" reflection min %lg \n", cval);
529 for (j = 0; j < ncon; ++j) {
532 d2[j] = loc_gamma * d2[j] + (1. - loc_gamma) * d0[k];
535 ccval = (*ftbm)(d2, ncon);
540 sinfo_msg(
" expansion minimum %lg \n", ccval);
543 for (j = 0; j < ncon; ++j) {
546 it1 = imax * nvar + k;
558 for (j = 0; j < ncon; ++j) {
561 it1 = imin * nvar + k;
564 for (i = 0; i < nvec; ++i) {
566 it2 = imin * nvar + k;
567 sum = sum + (d1[it1] - d1[it2]) * (d1[it1] - d1[it2]);
569 range[j] = sf * sqrt(sum / (nvec - 1));
572 value[1] = value[imin];
576 sinfo_msg(
" shrink factor %lg ", sf);
584 sinfo_spline(
double x,
double cons[],
double ak[],
double *sp,
double *spp,
598 for (i = 0; i < n; ++i) {
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];
610 sinfo_msg(
"1x=%g retval=%g", x, retval);
616 sinfo_ftbm(
const double x,
double cons[])
620 { -1, -.666666666666666, -.333333333333, 0 };
628 sm1 = sinfo_spline(x, cons, ak, &spm1, &sppm1, &spppm1, n) - 1;
629 sinfo_msg(
"x=%g val=%g", x, sm1 + 1);
631 retval = sm1 * sm1 + spm1 * spm1 + sppm1 * sppm1 + spppm1 * spppm1;
632 sinfo_msg(
"fitbm: x=%g retval=%g", x, retval);
662 sinfo_get_chisq(
int N,
int D,
663 int (*f)(
const double x[],
const double a[],
double *result),
674 for (i = 0; i < N; i++)
678 const double *x_i = &(x[0+i*D]);
683 &fx_i) == 0, CPL_ERROR_ILLEGAL_INPUT, -1.0);
688 residual = (fx_i - y[i]);
692 residual = (fx_i - y[i]) / sigma[i];
695 chi_sq += residual*residual;
739 sinfo_get_candidate(
const double *a,
const int ia[],
742 int (*f)(
const double x[],
const double a[],
double *result),
743 int (*dfda)(
const double x[],
const double a[],
double result[]),
763 Mfit = cpl_matrix_get_nrow(alpha);
765 alpha_data = cpl_matrix_get_data(alpha);
766 beta_data = cpl_matrix_get_data(beta);
779 cpl_matrix_fill(alpha, 0.0);
780 cpl_matrix_fill(beta , 0.0);
782 for (k = 0; k < N; k++)
786 const double *x_k = &(x[0+k*D]);
794 sm2 = 1.0 / (sigma[k] * sigma[k]);
798 cpl_ensure( f(x_k, a, &fx_k) == 0, CPL_ERROR_ILLEGAL_INPUT, -1);
801 cpl_ensure( dfda(x_k, a, partials) == 0,
802 CPL_ERROR_ILLEGAL_INPUT, -1);
804 for (i = 0, imfit = 0; i < M; i++)
810 sm2 * (y[k] - fx_k) * partials[i];
814 for (j = 0, jmfit = 0; j < i; j++)
818 alpha_data[jmfit + imfit*Mfit] +=
830 alpha_data[jmfit + imfit*Mfit] +=
832 partials[j] * (1 + lambda);
838 cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
842 for (i = 0, imfit = 0; i < M; i++) {
844 for (j = i+1, jmfit = imfit+1; j < M; j++) {
846 alpha_data[jmfit+imfit*Mfit] = alpha_data[imfit+jmfit*Mfit];
850 cpl_ensure( jmfit == Mfit,CPL_ERROR_ILLEGAL_INPUT,-1 );
854 cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
856 da = cpl_matrix_solve(alpha, beta);
858 cpl_ensure(da != NULL, cpl_error_get_code(), -1);
861 da_data = cpl_matrix_get_data(da);
863 for (i = 0, imfit = 0; i < M; i++)
867 a_da[i] = a[i] + da_data[0 + imfit*1];
877 cpl_ensure( imfit == Mfit ,CPL_ERROR_ILLEGAL_INPUT,-1);
879 cpl_matrix_delete(da);
885 #ifndef CPL_VECTOR_FIT_MAXITER
886 #define CPL_VECTOR_FIT_MAXITER 1000
957 sinfo_fit_lm(
const cpl_matrix *x,
958 const cpl_matrix *sigma_x,
960 const cpl_vector *sigma_y,
963 int (*f)(
const double x[],
966 int (*dfda)(
const double x[],
971 cpl_matrix **covariance)
973 const double *x_data = NULL;
974 const double *y_data = NULL;
975 const double *sigma_data = NULL;
983 double MAXLAMBDA = 10e40;
990 cpl_matrix *alpha = NULL;
991 cpl_matrix *beta = NULL;
992 double *a_data = NULL;
995 int *ia_local = NULL;
1000 if (covariance != NULL) *covariance = NULL;
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);
1008 cpl_ensure_code(f != NULL, CPL_ERROR_NULL_INPUT);
1009 cpl_ensure_code(dfda != NULL, CPL_ERROR_NULL_INPUT);
1012 cpl_ensure_code( sigma_y != NULL ||
1013 (red_chisq == NULL && covariance == NULL),
1014 CPL_ERROR_INCOMPATIBLE_INPUT);
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);
1021 cpl_ensure_code( cpl_vector_get_size(y) == N,
1022 CPL_ERROR_INCOMPATIBLE_INPUT);
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);
1028 if (sigma_y != NULL)
1030 cpl_ensure_code( cpl_vector_get_size(sigma_y) == N,
1031 CPL_ERROR_INCOMPATIBLE_INPUT);
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);
1038 ia_local = cpl_malloc(M *
sizeof(
int));
1039 cpl_ensure_code(ia_local != NULL, CPL_ERROR_ILLEGAL_OUTPUT);
1047 for (i = 0; i < M; i++)
1049 ia_local[i] = ia[i];
1060 cpl_ensure_code( CPL_FALSE,
1061 CPL_ERROR_ILLEGAL_INPUT);
1071 for (i = 0; i < M; i++)
1078 if (! ( red_chisq == NULL || N > Mfit ) )
1083 CPL_ERROR_ILLEGAL_INPUT);
1087 alpha = cpl_matrix_new(Mfit, Mfit);
1093 CPL_ERROR_ILLEGAL_OUTPUT);
1096 beta = cpl_matrix_new(Mfit, 1);
1100 cpl_matrix_delete(alpha);
1103 CPL_ERROR_ILLEGAL_OUTPUT);
1106 a_da = cpl_malloc(M *
sizeof(
double));
1110 cpl_matrix_delete(alpha);
1111 cpl_matrix_delete(beta);
1114 CPL_ERROR_ILLEGAL_OUTPUT);
1117 part = cpl_malloc(M *
sizeof(
double));
1121 cpl_matrix_delete(alpha);
1122 cpl_matrix_delete(beta);
1126 CPL_ERROR_ILLEGAL_OUTPUT);
1133 if( (chi_sq = sinfo_get_chisq(N, D, f, a_data, x_data, y_data, sigma_data)) < 0)
1136 cpl_matrix_delete(alpha);
1137 cpl_matrix_delete(beta);
1142 cpl_error_get_code());
1150 lambda < MAXLAMBDA &&
1151 iterations < CPL_VECTOR_FIT_MAXITER)
1157 double chi_sq_candidate = 0.0;
1165 while( (returncode = sinfo_get_candidate(a_data, ia_local,
1168 x_data, y_data, sigma_data,
1169 part, alpha, beta, a_da)
1171 && cpl_error_get_code() == CPL_ERROR_SINGULAR_MATRIX
1172 && lambda < MAXLAMBDA)
1180 if ( !( lambda < MAXLAMBDA ) )
1183 cpl_matrix_delete(alpha);
1184 cpl_matrix_delete(beta);
1189 CPL_ERROR_CONTINUE);
1192 if (returncode != 0)
1195 cpl_matrix_delete(alpha);
1196 cpl_matrix_delete(beta);
1201 cpl_error_get_code());
1205 if ((chi_sq_candidate = sinfo_get_chisq(N, D, f, a_da,
1206 x_data, y_data, sigma_data)) < 0)
1209 cpl_matrix_delete(alpha);
1210 cpl_matrix_delete(beta);
1215 cpl_error_get_code());
1221 if (chi_sq_candidate > chi_sq)
1234 (chi_sq - chi_sq_candidate)/chi_sq < .01)
1248 for (i = 0; i < M; i++) a_data[i] = a_da[i];
1250 chi_sq = chi_sq_candidate;
1256 if ( !( lambda < MAXLAMBDA && iterations < CPL_VECTOR_FIT_MAXITER ) )
1259 cpl_matrix_delete(alpha);
1260 cpl_matrix_delete(beta);
1265 CPL_ERROR_CONTINUE);
1275 for(i = 0; i < N; i++)
1278 double residual = 0.0;
1281 if( f(&(x_data[i*D]),
1286 cpl_matrix_delete(alpha);
1287 cpl_matrix_delete(beta);
1292 CPL_ERROR_ILLEGAL_INPUT);
1295 residual = y_data[i] - fx_i;
1296 *mse += residual * residual;
1302 if (red_chisq != NULL)
1305 *red_chisq = chi_sq / (N-Mfit);
1311 if (covariance != NULL)
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)
1322 cpl_matrix_delete(alpha);
1323 cpl_matrix_delete(beta);
1328 cpl_error_get_code());
1331 cov = cpl_matrix_invert_create(alpha);
1335 cpl_matrix_delete(alpha);
1336 cpl_matrix_delete(beta);
1341 cpl_error_get_code());
1347 for (i = 0; i < Mfit; i++)
1349 if ( !(cpl_matrix_get(cov, i, i) > 0) )
1352 cpl_matrix_delete(alpha);
1353 cpl_matrix_delete(beta);
1356 cpl_matrix_delete(cov);
1360 CPL_ERROR_SINGULAR_MATRIX);
1369 *covariance = cpl_matrix_new(M, M);
1370 if (*covariance == NULL)
1373 cpl_matrix_delete(alpha);
1374 cpl_matrix_delete(beta);
1377 cpl_matrix_delete(cov);
1380 CPL_ERROR_ILLEGAL_OUTPUT);
1386 for (j = 0, jmfit = 0; j < M; j++)
1387 if (ia_local[j] != 0)
1391 for (i = 0, imfit = 0; i < M; i++)
1392 if (ia_local[i] != 0)
1394 cpl_matrix_set(*covariance, i, j,
1396 cov, imfit, jmfit));
1400 cpl_ensure( imfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1);
1405 cpl_ensure( jmfit == Mfit, CPL_ERROR_ILLEGAL_INPUT,-1 );
1408 cpl_matrix_delete(cov);
1412 cpl_matrix_delete(alpha);
1413 cpl_matrix_delete(beta);
1417 return CPL_ERROR_NONE;
#define sinfo_msg_error(...)
Print an error message.