#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "nrutil.h"
#define NR_END 1
#define FREE_ARG char*
#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
char Buffer[1024];
typedef void (*MYFUNCTYPE)(float, float [], float *, float [], int);
void funcs(float x, float a[], float *y, float dyda[], int na);
void covsrt(float **covar, int ma, int ia[], int mfit);
void free_vector(float *v, long nl, long nh);
float *vector(long nl, long nh);
int *ivector(long nl, long nh);
void free_ivector(int *v, long nl, long nh);
void free_matrix(float **m, long nrl, long nrh, long ncl, long nch);
float **matrix(long nrl, long nrh, long ncl, long nch);
void nrerror(char error_text[]);
void gaussj(float **a, int n, float **b, int m);
void mrqmin(float x[], float y[], float sig[], int ndata, float a[], int
ia[],int ma, float **covar, float **alpha, float *chisq,MYFUNCTYPE funcs,
float *alamda);
int main()
{
int i;
int ma=9,mfit=0; // number of parameters
float *a; // parameters array
int *ia;
float **alpha; // curvature matrix
float *beta; // beta vector
float *chisq,ochisq;
float *lam;
float *x,*y,*sig,*yfit;
float **covar;
int N=100;
// Start the process
a = (float*)malloc(sizeof(float)*ma);
ia = (int*)malloc(sizeof(int)*ma);
beta = (float*)malloc(sizeof(float)*ma);
x = (float*)malloc(sizeof(float)*N);
y = (float*)malloc(sizeof(float)*N);
yfit = (float*)malloc(sizeof(float)*N);
sig = (float*)malloc(sizeof(float)*N);
alpha = (float**)malloc(sizeof(float)*ma);
lam = (float*)malloc(sizeof(float)*ma);
//covar = (float**)malloc(sizeof(float)*ma*ma);
covar = new float*[ma];
//covar[0][0]=(float)1.2;
//sprintf(Buffer, " covar[1][1]=%f\n ", covar[0][0]);
// printf(Buffer);
for (i=0;i<N;i++)
{
x=(float)rand();
y=x*sin(x);
sig=1;
sprintf(Buffer, " x_in=%f y_in=%f\n", x,y);
printf(Buffer);
}
for (i=0;i<ma;i++)
{
a=0;
ia=1;
beta=0;
}
*lam=(float)0.01;
chisq=0;
covsrt(covar,ma,ia,mfit);
mrqmin(x,y,sig,N,a,ia,ma,covar,alpha,chisq,funcs,lam);
for (i=0;i<N;i++)
{
sprintf(Buffer, " x=%f y=%f ", x,y);
printf(Buffer);
}
ochisq=0;
}
void covsrt(float **covar, int ma, int ia[], int mfit)
{
int i,j,k;
float swap;
covar = new float*[ma];
for (i=mfit+1;i<=ma;i++)
covar = new float[ma];
covar[1]=(float)0;
exit(0);
for (j=1;j<=i;j++) {covar[j]=covar[j]=0.0;}
k=mfit;
for (j=ma;j>=1;j--) {
if (ia[j]) {
for (i=1;i<=ma;i++) SWAP(covar[k],covar[j])
for (i=1;i<=ma;i++) SWAP(covar[k],covar[j])
k--;
}
}
}
void funcs(float x, float a[], float *y, float dyda[], int na)
{
float fac,Mcos,Msin;
*y=0.0;
for(int i=1;i<na-1;i+=3)
{
Mcos=cos(x+a[i+2]);
Msin=sin(x+a[i+2]);
fac=-a[i+1]*sin(x+a[i+2]);
*y+=a+(a[i+1]*Mcos);
dyda=1;
dyda[i+1]=Mcos-(a[i+1]*Msin);
dyda[i+2]=-a[i+1]*Msin;
}
}
typedef void (*MYFUNCTYPE)(float, float [], float *, float [], int);
void mrqmin(float x[], float y[], float sig[], int ndata, float a[], int
ia[],
int ma, float **covar, float **alpha, float *chisq,MYFUNCTYPE funcs, float
*alamda)
{
//void covsrt(float **covar, int ma, int ia[], int mfit);
//void gaussj(float **a, int n, float **b, int m);
void mrqcof(float x[], float y[], float sig[], int ndata, float a[],int
ia[], int ma, float **alpha, float beta[], float *chisq,MYFUNCTYPE funcs);
int j,k,l;
static int mfit;
static float ochisq,*atry,*beta,*da,**oneda;
if (*alamda < 0.0) {
atry=vector(1,ma);
beta=vector(1,ma);
da=vector(1,ma);
for (mfit=0,j=1;j<=ma;j++)
if (ia[j]) mfit++;
oneda=matrix(1,mfit,1,1);
*alamda=(float)0.001;
mrqcof(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
ochisq=(*chisq);
for (j=1;j<=ma;j++) atry[j]=a[j];
}
for (j=1;j<=mfit;j++) {
for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
covar[j][j]=(float)(alpha[j][j]*(1.0+(*alamda)));
oneda[j][1]=beta[j];
}
gaussj(covar,mfit,oneda,1);
for (j=1;j<=mfit;j++) da[j]=oneda[j][1];
if (*alamda == 0.0) {
covsrt(covar,ma,ia,mfit);
covsrt(alpha,ma,ia,mfit);
free_matrix(oneda,1,mfit,1,1);
free_vector(da,1,ma);
free_vector(beta,1,ma);
free_vector(atry,1,ma);
return;
}
for (j=0,l=1;l<=ma;l++)
if (ia[l]) atry[l]=a[l]+da[++j];
mrqcof(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
if (*chisq < ochisq) {
*alamda *= (float)0.1;
ochisq=(*chisq);
for (j=1;j<=mfit;j++) {
for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
beta[j]=da[j];
}
for (l=1;l<=ma;l++) a[l]=atry[l];
} else {
*alamda *= 10.0;
*chisq=ochisq;
}
}
float *vector(long nl, long nh)
/* allocate a float vector with subscript range v[nl..nh] */
{
float *v;
v=(float *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float)));
if (!v) nrerror("allocation failure in vector()");
return v-nl+NR_END;
}
void nrerror(char error_text[])
/* Numerical Recipes standard error handler */
{
fprintf(stderr,"Numerical Recipes run-time error...\n");
fprintf(stderr,"%s\n",error_text);
fprintf(stderr,"...now exiting to system...\n");
exit(1);
}
float **matrix(long nrl, long nrh, long ncl, long nch)
/* allocate a float matrix with subscript range m[nrl..nrh][ncl..nch] */
{
long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
float **m;
/* allocate pointers to rows */
m=(float **) malloc((size_t)((nrow+NR_END)*sizeof(float*)));
if (!m) nrerror("allocation failure 1 in matrix()");
m += NR_END;
m -= nrl;
/* allocate rows and set pointers to them */
m[nrl]=(float *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(float)));
if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
m[nrl] += NR_END;
m[nrl] -= ncl;
for(i=nrl+1;i<=nrh;i++) m=m[i-1]+ncol;
/* return pointer to array of pointers to rows */
return m;
}
void free_matrix(float **m, long nrl, long nrh, long ncl, long nch)
/* free a float matrix allocated by matrix() */
{
free((FREE_ARG) (m[nrl]+ncl-NR_END));
free((FREE_ARG) (m+nrl-NR_END));
}
void free_vector(float *v, long nl, long nh)
/* free a float vector allocated with vector() */
{
free((FREE_ARG) (v+nl-NR_END));
}
void free_ivector(int *v, long nl, long nh)
/* free an int vector allocated with ivector() */
{
free((FREE_ARG) (v+nl-NR_END));
}
int *ivector(long nl, long nh)
/* allocate an int vector with subscript range v[nl..nh] */
{
int *v;
v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
if (!v) nrerror("allocation failure in ivector()");
return v-nl+NR_END;
}
void mrqcof(float x[], float y[], float sig[], int ndata, float a[], int
ia[],int ma, float **alpha, float beta[], float *chisq,MYFUNCTYPE funcs)
{
int i,j,k,l,m,mfit=0;
float ymod,wt,sig2i,dy,*dyda;
dyda=vector(1,ma);
for (j=1;j<=ma;j++)
if (ia[j]) mfit++;
for (j=1;j<=mfit;j++) {
for (k=1;k<=j;k++) alpha[j][k]=0.0;
beta[j]=0.0;
}
*chisq=0.0;
for (i=1;i<=ndata;i++) {
(*funcs)(x,a,&ymod,dyda,ma);
sig2i=(float)(1.0/(sig*sig));
dy=y-ymod;
for (j=0,l=1;l<=ma;l++) {
if (ia[l]) {
wt=dyda[l]*sig2i;
for (j++,k=0,m=1;m<=l;m++)
if (ia[m]) alpha[j][++k] += wt*dyda[m];
beta[j] += dy*wt;
}
}
*chisq += dy*dy*sig2i;
}
for (j=2;j<=mfit;j++)
for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
free_vector(dyda,1,ma);
}
void gaussj(float **a, int n, float **b, int m)
{
int *indxc,*indxr,*ipiv;
int i,icol,irow,j,k,l,ll;
float big,dum,pivinv;//,temp;
float swap;
indxc=ivector(1,n);
indxr=ivector(1,n);
ipiv=ivector(1,n);
for (j=1;j<=n;j++) ipiv[j]=0;
for (i=1;i<=n;i++) {
big=0.0;
for (j=1;j<=n;j++)
if (ipiv[j] != 1)
for (k=1;k<=n;k++) {
if (ipiv[k] == 0) {
if (fabs(a[j][k]) >= big) {
big=(float)(fabs(a[j][k]));
irow=j;
icol=k;
}
} else if (ipiv[k] > 1) nrerror("gaussj: Singular Matrix-1");
}
++(ipiv[icol]);
if (irow != icol) {
for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
}
indxr=irow;
indxc=icol;
if (a[icol][icol] == 0.0) nrerror("gaussj: Singular Matrix-2");
pivinv=(float)(1.0/a[icol][icol]);
a[icol][icol]=1.0;
for (l=1;l<=n;l++) a[icol][l] *= pivinv;
for (l=1;l<=m;l++) b[icol][l] *= pivinv;
for (ll=1;ll<=n;ll++)
if (ll != icol) {
dum=a[ll][icol];
a[ll][icol]=0.0;
for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
}
}
for (l=n;l>=1;l--) {
if (indxr[l] != indxc[l])
for (k=1;k<=n;k++)
SWAP(a[k][indxr[l]],a[k][indxc[l]]);
}
free_ivector(ipiv,1,n);
free_ivector(indxr,1,n);
free_ivector(indxc,1,n);
}
#undef SWAP