00001
00005 #include <bpm/bpm_nr.h>
00006 #include <bpm/bpm_messages.h>
00007 #include <bpm/bpm_process.h>
00008
00009 #define FIT_MAX_ITER 10000
00010
00011 #define FIT_AMP 0
00012 #define FIT_PHASE 1
00013 #define FIT_FREQ 2
00014 #define FIT_TDECAY 3
00015
00016 #define FIT_T0 0
00017 #define FIT_FS 1
00018
00019 void fcnwfjac( double* par, double *jac, int npars, int ns, void *a ) {
00020 int i, j;
00021 double t, sinarg, exparg, cosarg;
00022
00023
00024
00025 for( i=j=0; i<ns; i++ ) {
00026 sample_to_time( ( (double*)a )[FIT_FS], ns, i, &t );
00027 if ( t >= ((double*)a)[FIT_T0] ) {
00028 sinarg = sin( 2.*PI*par[FIT_FREQ]* ( t - ((double*)a)[FIT_T0] ) + par[FIT_PHASE] );
00029 cosarg = cos( 2.*PI*par[FIT_FREQ]* ( t - ((double*)a)[FIT_T0] ) + par[FIT_PHASE] );
00030 exparg = exp( -( t - ((double*)a)[FIT_T0] ) / par[FIT_TDECAY] );
00031
00032
00033
00034
00035 jac[j++] = exparg * sinarg;
00036
00037
00038 jac[j++] = par[FIT_AMP] * exparg * cosarg;
00039
00040
00041 jac[j++] = par[FIT_AMP] * exparg * cosarg * 2.*PI*( t - ((double*)a)[FIT_T0] );
00042
00043
00044 jac[j++] = par[FIT_AMP] * exparg * (t - ((double*)a)[FIT_T0])/SQR( par[FIT_TDECAY]) * sinarg;
00045
00046 } else {
00047 jac[j++] = 0.;
00048 jac[j++] = 0.;
00049 jac[j++] = 0.;
00050 jac[j++] = 0.;
00051 }
00052
00053 }
00054 }
00055
00056
00057
00058
00059
00060
00061 void fcnwf( double* par, double *sinwf, int npars, int ns, void *a ) {
00062
00063 double t;
00064 int i;
00065
00066 for( i=0; i<ns; i++ ) {
00067 sample_to_time( ( (double*)a )[FIT_FS], ns, i, &t );
00068 if ( t >= ( (double*)a )[FIT_T0] ) {
00069 sinwf[i] = par[FIT_AMP]
00070 * exp( - ( t-( (double*)a )[FIT_T0]) / par[FIT_TDECAY] )
00071 * sin( 2.*PI*par[FIT_FREQ]*(t-( (double*)a )[FIT_T0]) + par[FIT_PHASE] );
00072 } else {
00073 sinwf[i] = 0.;
00074 }
00075 }
00076
00077 return;
00078 }
00079
00080 int fit_waveform( doublewf_t *w, double t0,
00081 double i_freq, double i_tdecay, double i_amp, double i_phase,
00082 double *freq, double *tdecay, double *amp, double *phase ) {
00083
00084 doublewf_t *yval;
00085 double a[2];
00086 const int npars = 4;
00087 double par[npars];
00088 double opt[4], info[LM_INFO_SZ];
00089 int i;
00090
00091 double err[256];
00092
00093 opt[0]=LM_INIT_MU; opt[1]=1E-15; opt[2]=1E-15; opt[3]=1E-20;
00094
00095 if ( ! w ) {
00096 bpm_error( "Invalid waveform pointer in fit_waveform(...)",
00097 __FILE__, __LINE__ );
00098 return BPM_FAILURE;
00099 }
00100
00101
00102 yval = doublewf( w->ns, w->fs );
00103 if ( ! yval ) {
00104 bpm_error( "Unable to allocate memory for waveform in fit_waveform(...)",
00105 __FILE__, __LINE__ );
00106 return BPM_FAILURE;
00107 }
00108
00109
00110 a[FIT_T0] = t0;
00111 a[FIT_FS] = w->fs;
00112
00113 par[FIT_AMP] = i_amp;
00114 par[FIT_PHASE] = i_phase;
00115 par[FIT_FREQ] = i_freq;
00116 par[FIT_TDECAY] = i_tdecay;
00117
00118
00119 nr_lmder( fcnwf, fcnwfjac, par, yval->wf, npars, w->ns, FIT_MAX_ITER, opt, info, NULL, NULL, a );
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 *amp = par[FIT_AMP];
00134 *phase = par[FIT_PHASE];
00135 *freq = par[FIT_FREQ];
00136 *tdecay = par[FIT_TDECAY];
00137
00138 doublewf_delete( yval );
00139
00140 return BPM_SUCCESS;
00141 }
00142
00143 #undef FIT_MAX_ITER
00144 #undef FIT_AMP
00145 #undef FIT_PHASE
00146 #undef FIT_FREQ
00147 #undef FIT_TDECAY
00148 #undef FIT_T0
00149 #undef FIT_FS
00150