00001
00005 #include <stdio.h>
00006 #include <stdlib.h>
00007 #include <bpm/bpm_nr.h>
00008 #include <bpm/bpm_units.h>
00009 #include <bpm/bpm_messages.h>
00010 #include <bpm/bpm_process.h>
00011
00012 #define FIT_MAX_ITER 5000 // max iteration
00013 #define FIT_WINDOW_FACTOR 3 // number of sigma to fit withing (integer)
00014
00015
00016
00017
00018
00019
00020
00021
00022 void fcnlorjac( double *p, double *ljac, int np, int ns, void *a ) {
00023
00024 int i,j;
00025 double denom;
00026 double f;
00027
00028 for ( i=j=0; i<ns; i++ ) {
00029
00030 f = (((double*)a)[0] + (double)i + 0.5 ) / ((double*)a)[1] * ((double*)a)[2];
00031 denom = SQR( f - p[1] ) + SQR( p[2] / 2. );
00032
00033 if ( denom == 0. ) {
00034 ljac[j++] = 0.;
00035 ljac[j++] = 0.;
00036 ljac[j++] = 0.;
00037 ljac[j++] = 1.;
00038 } else {
00039 ljac[j++] = 1. / denom;
00040 ljac[j++] = 2. * p[0] * (f-p[1]) / SQR( denom );
00041 ljac[j++] = -0.5 * p[0] * p[2] / SQR( denom );
00042 ljac[j++] = 1.;
00043 }
00044
00045 }
00046
00047 return;
00048 }
00049
00050 void fcnlor( double *p, double *lor, int np, int ns, void *a ) {
00051
00052 int i;
00053 double f;
00054
00055
00056
00057
00058
00059
00060
00061 for ( i=0; i<ns; i++ ) {
00062
00063 f = (((double*)a)[0] + (double) i + 0.5 ) / ((double*)a)[1] * ((double*)a)[2];
00064 lor[i] = p[0] / ( SQR( f - p[1] ) + SQR( p[2] / 2. ) ) + p[3];
00065 }
00066
00067 return;
00068 }
00069
00070
00071
00072 int fit_fft_prepare( complexwf_t *ft, int *n1, int *n2,
00073 double *amp, double *freq, double *fwhm ) {
00074
00075 int i, imax=0;
00076 double pw, tmp;
00077
00078 if ( ! ft || ! amp ) {
00079 bpm_error( "Invalid pointers in fit_fft_prepare(...)",
00080 __FILE__, __LINE__ );
00081 return BPM_FAILURE;
00082 }
00083
00084
00085 *amp = 0;
00086 *freq = 20.0 * _MHz__;
00087 *fwhm = 10.0 * _MHz__;
00088 *n1 = 20;
00089 *n2 = 100;
00090
00091
00092 for ( i=0; i<ft->ns/2; i++ ) {
00093 pw = c_abs2( ft->wf[i] );
00094 if ( pw > *amp ) {
00095 *amp = pw; imax = i;
00096 }
00097 }
00098 *freq = (double) imax / (double) ft->ns * ft->fs;
00099
00100
00101 for ( i=imax; i>0; i-- ) {
00102 pw = c_abs2( ft->wf[i] );
00103 if ( pw <= *amp/2. ) break;
00104 }
00105 *n1 = i;
00106
00107 for ( i=imax; i<ft->ns/2 ; i++ ) {
00108 pw = c_abs2( ft->wf[i] );
00109 if ( pw <= *amp/2. ) break;
00110 }
00111 *n2 = i;
00112
00113
00114 *fwhm = ( (double)( *n2 - *n1 ) / (double) ft->ns * ft->fs );
00115
00116
00117 *n1 = imax - FIT_WINDOW_FACTOR * ABS(imax - *n1);
00118 *n2 = imax + FIT_WINDOW_FACTOR * ABS(imax - *n2);
00119
00120
00121 if ( *n1 < 0 ) *n1 = 0;
00122 if ( *n2 > ft->ns/2) *n2=ft->ns/2;
00123
00124 if ( *n2 <= *n1 ) {
00125 bpm_error( "Error in fit range ( n2 <= n1 ) in fit_fft_prepare(...)",
00126 __FILE__, __LINE__ );
00127 return BPM_FAILURE;
00128 }
00129
00130 if ( *n2 - *n1 < 5 ) {
00131 bpm_error( "Error, too few number of samples in fit_fft_prepare(...)",
00132 __FILE__, __LINE__ );
00133 return BPM_FAILURE;
00134 }
00135
00136
00137 #if 0
00138 printf( "fftfit prepare : n1=%d, n2=%d, imax=%d, amp=%f, freq=%f, fwhm=%f\n",
00139 *n1, *n2, imax, *amp, *freq, *fwhm );
00140 fflush( stdout );
00141 #endif
00142
00143 return BPM_SUCCESS;
00144 }
00145
00146
00147
00148 int fit_fft( complexwf_t *ft, double *freq, double *tdecay, double *A, double *C ) {
00149
00150 const int npar = 4;
00151 double *pspec, par[npar];
00152 double opt[4], info[LM_INFO_SZ];
00153 double a[3], i_amp, i_freq, i_fwhm;
00154 int i, nfit, n1, n2;
00155
00156 #if 0
00157 double *err;
00158 #endif
00159
00160 opt[0]=LM_INIT_MU; opt[1]=1E-15; opt[2]=1E-15; opt[3]=1E-20;
00161
00162
00163 *freq = 0.;
00164 *tdecay = 0.;
00165
00166 if ( ! ft ) {
00167 bpm_error( "Invalid pointer in fit_fft(...)",
00168 __FILE__, __LINE__ );
00169 return BPM_FAILURE;
00170 }
00171
00172 if ( fit_fft_prepare( ft, &n1, &n2, &i_amp, &i_freq, &i_fwhm ) == BPM_FAILURE ) {
00173 return BPM_FAILURE;
00174 }
00175
00176
00177 nfit = n2 - n1 + 1;
00178 pspec = (double*) calloc( nfit, sizeof(double) );
00179 for ( i=0; i<nfit; i++ ) {
00180 pspec[i] = c_abs2( ft->wf[i] );
00181 }
00182
00183
00184 par[0] = i_amp;
00185 par[1] = i_freq;
00186 par[2] = i_fwhm;
00187 par[3] = 0.;
00188
00189
00190 a[0] = (double) n1;
00191 a[1] = (double) ft->ns;
00192 a[2] = ft->fs;
00193
00194
00195 #if 0
00196 err = (double*) calloc( nfit, sizeof(double) );
00197 nr_lmchkjac( fcnlor, fcnlorjac, par, npar, nfit, a, err );
00198 for (i=0;i<nfit;i++) printf( "err[%d] = %f\n", i, err[i] ); fflush(stdout);
00199 free(err);
00200 #endif
00201
00202
00203 nr_lmder( fcnlor, fcnlorjac, par, pspec, npar, nfit, FIT_MAX_ITER, opt, info,
00204 NULL, NULL, a );
00205
00206 *freq = par[1];
00207
00208
00209 if ( par[2] != 0. ) *tdecay = 1. / par[2] / PI;
00210
00211
00212 if ( A ) *A = par[0];
00213 if ( C ) *C = par[3];
00214
00215 free( pspec );
00216
00217 return BPM_SUCCESS;
00218 }
00219
00220 #undef FIT_MAX_ITER