/*LINTLIBRARY*/
/* gauss.c
This code provides gaussian fitting routines.
Copyright (C) 1997 Karl Glazebrook and Alison Offer Real code
Note it is not clear to me that this code is fully debugged. The reason
I say that is because I tried using the linear eqn solving routines called
elsewhere and they were giving erroneous results. So steal from this
code with caution! However it does give good fits to reasonable looking
gaussians and tests show correct parameters.
KGB 29/Oct/2002
*/
#include <stdio.h>
#include <math.h>
#define NPAR 3
#define MAXITER 1000
/* Malloc 2D ptr array e.g. a[nx][ny] */
static double **malloc2D (int nx, int ny)
{
int i;
double **p;
p = (double**) malloc( nx*sizeof(double*) ); /* 1D array of ptrs p[i] */
if (p==NULL)
return NULL;
for (i=0;i<nx;i++) {
p[i] = (double*) malloc( ny*sizeof(double) );
if (p[i] == NULL) {
free(p);
return(NULL);
}
}
return (double**) p;
}
/* Free a 2D ptr array */
static void free2D (double **p, int nx, int ny)
{
int i;
for (i=0;i<nx;i++)
free(p[i]);
free(p);
}
/* ======================================================================== */
static void seta (int npar, int npoints, double a[NPAR][NPAR], double **d,
double *sig)
{
int i,j,k;
for(i=0; i<npar; i++)
{
for (j=0; j<npar; j++)
{
a[i][j] = 0.0;
for(k=0; k<npoints; k++)
a[i][j] += d[k][i] * d[k][j] / sig[k];
}
}
}
static void setb (int npar, int npoints, double b[NPAR], double **d,
double *y, double *yfit, double *sig)
{
int i,k;
for(i=0; i<npar; i++) {
b[i]=0.0;
for(k=0; k<npoints; k++)
b[i] += d[k][i] * (y[k] - yfit[k]) / sig[k];
}
}
/* ========================================================================
returns the fitted function in yfit and the partial derivatives in d
here f = b exp ( - ((x - c)/a)^2)
*/
static void funct (int npoints, int npar, double *x, double *yfit,
double **d, double par[NPAR])
{
int i;
double a,b,c,arg;
a=par[0]; b=par[1]; c=par[2];
for(i=0; i<npoints; i++) {
arg = (x[i]-c) /a; arg *= arg;
yfit[i] = b * exp(-arg);
d[i][0] = 2.0 * arg * yfit[i] / a;
d[i][1] = yfit[i] / b;
d[i][2] = 2.0 * arg * yfit[i] / (x[i]-c);
}
}
/* These have been checked */
/*
IMPORTANT NOTE: I tried using decomp()+leneq() routines
elsewhere and they were giving erroneous results.
So steal from this code with caution!
KGB 29/Oct/2002
*/
/* =======================================================================
LU decomposition of matrix x using Crouts algotithm with partial
pivoting.
Returns decomposition in x and permutation of rows in iorder
*/
static void decomp (int n, int ndim, double x[NPAR][NPAR], int iorder[NPAR])
{
int icol, irow, isum, iexc, ipivot;
double store, xmax, sum, eps=1e-16;
/* initialise iorder */
for(iexc=1; iexc<=n; iexc++)
iorder[iexc-1] = iexc;
/* loop over columns */
for (icol = 1; icol<=n; icol++) {
/* U - matrix */
if (icol>1) {
for (irow=1; irow<=icol-1; irow++) {
sum = x[irow-1][icol-1];
for (isum=1; isum<=irow-1; isum++)
sum -= x[irow-1][isum-1]*x[isum-1][irow-1];
x[irow-1][icol-1] = sum;
}
}
/* L - matrix plus diagonal element of U matrix */
xmax = 0; ipivot = icol;
for (irow=icol; irow<=n; irow++) {
sum = x[irow-1][icol-1];
if (icol>1) {
for(isum=1; isum<=icol-1; isum++)
sum -= x[irow-1][isum-1] * x[isum-1][icol-1];
}
if (fabs(sum)>xmax) {
xmax = sum;
ipivot = irow;
}
x[irow-1][icol-1] = sum;
}
/* if xmax is very small replace by epsilon to avoid
dividing by zero */
if (fabs(xmax)<eps)
x[ipivot-1][icol-1] = eps;
/* Partial Pivoting - look for maximum x(i,j<=i) and interchange
rows if necessary to put this on the diagonal */
if (ipivot!=icol) {
iexc = iorder[ipivot-1];
iorder[ipivot-1] = iorder[icol-1];
iorder[icol-1] = iexc;
for(iexc=1; iexc<=n; iexc++) {
store = x[ipivot-1][iexc-1];
x[ipivot-1][iexc-1] = x[icol-1][iexc-1];
x[icol-1][iexc-1] = store;
}
}
/* divide new L-matrix elements by new diagonal element */
if (icol<n) {
for (irow=icol+1; irow<=n; irow++)
x[irow-1][icol-1] /= x[icol-1][icol-1];
}
}
}
/*
IMPORTANT NOTE: I tried using decomp()+leneq() routines
elsewhere and they were giving erroneous results.
So steal from this code with caution!
KGB 29/Oct/2002
*/
/* ========================================================================
x[i<=j[]j] is U-matrix from LU-decomposition
x[i>j][j] is L-matrix (diagonal elements are unity)
iorder is the permutation of the rows
b is the input vector, d is the solution vector
*/
static void lineq (int n, int ndim, double x[NPAR][NPAR], double b[NPAR],
double d[NPAR], int iorder[NPAR])
{
int i,isum;
double sum;
/* solving X.b = d ==> (L.U).b = d
or L.(U.b) = d
first re-order the vector */
for (i=1; i<=n; i++)
d[i-1] = b[iorder[i-1]-1];
/* first find (U.b) */
for(i=2; i<=n; i++) {
sum = d[i-1];
for (isum=1; isum<=i-1; isum++)
sum -= x[i-1][isum-1] * d[isum-1];
d[i-1] = sum;
}
/* Now fill out d (solution of X.b) by back substitution */
d[n-1] /= x[n-1][n-1];
for (i=n-1; i>=1; i--){
sum = d[i-1];
for (isum=i+1; isum<=n; isum++)
sum -= x[i-1][isum-1] * d[isum-1];
d[i-1] = sum / x[i-1][i-1];
}
}
/* ========================================================================
My C version of Alison's subroutine to fit a non-linear functions using the
Levenberg-Marquardt algorithm
input: npoints = number of data points
npar = number of parameters in fit
par = initial estimates of parameters
sigma = errors on data (sigma^2)
output: par = output parameters
r = residuals (y(i) - yfit(i))
a = estimated covariance matrix of std errs
in fitted params.
*/
static int marquardt (int npoints, int npar, double*x, double *y,
double* sig, double par[NPAR], double* r,
double a[NPAR][NPAR])
{
int i,k, done, decrease, niter;
int iorder[NPAR];
double *yfit, **d, **d2, tmp;
double par2[NPAR], delta[NPAR], b[NPAR], aprime[NPAR][NPAR];
double lambda, chisq, chisq2, eps=0.001, lamfac=2.0;
/* Memory allocation */
yfit = (double*) malloc( npoints*sizeof(double));
if (yfit==NULL)
return(1);
d = malloc2D( npoints, NPAR);
if (d==NULL) {
free(yfit); return(1);
}
d2 = malloc2D( npoints, NPAR);
if (d2==NULL) {
free(yfit); free2D(d,npoints,NPAR); return(1);
}
/* Not enough points */
if (npoints < npar) {
free(yfit); free2D(d,npoints,NPAR); free2D(d2,npoints,NPAR);
return(2);
}
lambda = 0.001; done = 0; decrease = 0; niter = 1;
/* Get the value for the initial fit and the value of the derivatives
for the current estimate of the parameters */
funct(npoints, npar, x, yfit, d, par);
/* Calculate chi^2 */
chisq = 0;
for (k=0; k<npoints; k++) {
tmp = y[k] - yfit[k];
chisq += tmp*tmp / sig[k];
}
/* Set up initial matrices; curvature matrix a and b */
seta(npar,npoints,a,d,sig);
setb(npar,npoints,b,d,y,yfit,sig);
/* Main Loop */
while(!done) {
/* Set up a' */
for (k=0; k<npar; k++) {
for (i=0; i<npar; i++) {
aprime[i][k] = a[i][k];
}
aprime[k][k] = a[k][k] * (1.0 + lambda);
}
/* Solve Matrix Equation: a'.delta = b */
/*
IMPORTANT NOTE: I tried using decomp()+leneq() routines
elsewhere and they were giving erroneous results.
So steal from this code with caution!
KGB 29/Oct/2002
*/
decomp(npar ,npar, aprime, iorder);
lineq(npar, npar, aprime, b, delta, iorder);
/* Increment parameters in work space and evaluate new chisq */
for (i=0; i<NPAR; i++){
if (i<npar)
par2[i] = par[i] + delta[i];
else
par2[i] = par[i];
}
funct(npoints, npar, x, r, d2, par2);
chisq2 = 0.0;
for (k=0; k<npoints; k++) {
tmp = y[k] - r[k];
chisq2 += tmp*tmp / sig[k];
}
/* SUccess ? */
/* printf ("diag: %lf %lf %lf\n", chisq, chisq2,
fabs(((chisq - chisq2)/chisq))); */
if (chisq2<=chisq) {
/* Test convergence */
if (chisq == chisq2)
done = 1;
if (fabs(((chisq - chisq2)/chisq)) < eps && decrease )
done = 1;
for (k=0; k<npoints; k++)
yfit[k] = r[k];
for (i=0; i<NPAR; i++) {
par[i] = par2[i];
for (k=0; k<npoints; k++) {
d[k][i] = d2[k][i];
}
}
if (!done) {
seta(npar,npoints,a,d,sig); setb(npar,npoints,b,d,y,yfit,sig);
chisq = chisq2; lambda = lambda / lamfac; decrease = 1;
}
}
else {
decrease = 0;
lambda *= lamfac;
}
niter++;
/* Failure to converge after numerous iterations */
if (niter>MAXITER) {
free(yfit); free2D(d,npoints,NPAR); free2D(d2,npoints,NPAR);
return(3);
}
}
/* Success!!! - compute residual and covariance matrix then return
first calculating inverse of aprime */
for (i=0; i<npar; i++)
delta[i] = 0.0;
for (i=0; i<npar; i++) {
delta[i] = 1.0;
lineq (npar, NPAR, aprime, delta, b, iorder);
for (k=0; k<npar; k++)
a[k][i] = b[k];
}
for (k=0; k<npoints; k++)
r[k] = y[k] - yfit[k];
free(yfit); free2D(d,npoints,NPAR); free2D(d2,npoints,NPAR);
return(0);
}
/* qsort doubles */
static void lqsortD (double* xx, int a, int b)
{
int i,j;
double t, median;
i = a; j = b;
median = xx[(i+j) / 2];
do {
while (xx[i] < median)
i++;
while (median < xx[j])
j--;
if (i <= j) {
t = xx[i]; xx[i] = xx[j]; xx[j] = t;
i++; j--;
}
} while (i <= j);
if (a < j)
lqsortD(xx,a,j);
if (i < b)
lqsortD(xx,i,b);
}