00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <iostream>
00026 #include <complex>
00027
00028 #include <fftw.h>
00029 #include <gsl/gsl_heapsort.h>
00030
00031 #include "orsa_fft.h"
00032 #include "orsa_common.h"
00033
00034 using namespace std;
00035
00036 namespace orsa {
00037
00038 double norm( const fftw_complex z) {
00039 return sqrt( z.re * z.re + z.im * z.im );
00040 }
00041
00042 double norm_sq( const fftw_complex z) {
00043 return ( z.re * z.re + z.im * z.im );
00044 }
00045
00046
00047
00048
00049 fftw_complex phi(double omega, fftw_complex in[], const int size) {
00050
00051 fftw_complex result;
00052 result.re = 0;
00053 result.im = 0;
00054
00055 double arg,c,s;
00056 int k;
00057
00058 for(k=0;k<size;k++) {
00059 arg = twopi*k*omega;
00060 c = cos(arg);
00061 s = sin(arg);
00062 result.re += in[k].re * c + in[k].im * s;
00063 result.im -= in[k].re * s - in[k].im * c;
00064 }
00065
00066 double scale = (double)size;
00067 result.re /= scale;
00068 result.im /= scale;
00069
00070
00071 return result;
00072 }
00073
00074
00075
00076
00077 fftw_complex phi_Hanning(double omega, fftw_complex in[], const int size) {
00078
00079 fftw_complex result;
00080 result.re = 0;
00081 result.im = 0;
00082
00083 double arg,c,s;
00084 int k;
00085 double window_factor;
00086
00087 for(k=0;k<size;k++) {
00088 arg = twopi*k*omega;
00089 c = cos(arg);
00090 s = sin(arg);
00091 window_factor = (1-cos(k*twopi/size));
00092 result.re += window_factor * (in[k].re * c + in[k].im * s);
00093 result.im -= window_factor * (in[k].re * s - in[k].im * c);
00094 }
00095
00096 double scale = (double)size;
00097 result.re /= scale;
00098 result.im /= scale;
00099
00100
00101 return result;
00102 }
00103
00104
00105
00106 double phi_amp(double omega, fftw_complex in[], const int size) {
00107 return sqrt( norm_sq(phi( omega,&in[0],size)) +
00108 norm_sq(phi(-omega,&in[0],size)) );
00109 }
00110
00111
00112
00113 double phi_Hanning_amp(double omega, fftw_complex in[], const int size) {
00114 return sqrt( norm_sq(phi_Hanning( omega,&in[0],size)) +
00115 norm_sq(phi_Hanning(-omega,&in[0],size)) );
00116 }
00117
00118
00119 double phi_gsl (double x, void * params) {
00120
00121 struct gsl_d1_minimization_parameters * p = (gsl_d1_minimization_parameters *) params;
00122
00123
00124 return (-phi_amp(x, p->pointer_points_sequence, p->size));
00125
00126 }
00127
00128 double phi_gsl_two (double x, void * params) {
00129
00130 struct gsl_d1_minimization_parameters * p = (gsl_d1_minimization_parameters *) params;
00131
00132
00133 return (-norm(phi(x, p->pointer_points_sequence, p->size)));
00134 }
00135
00136 double phi_Hanning_gsl (double x, void * params) {
00137
00138 struct gsl_d1_minimization_parameters * p = (gsl_d1_minimization_parameters *) params;
00139
00140 return (-phi_Hanning_amp(x, p->pointer_points_sequence, p->size));
00141
00142
00143 }
00144
00145
00146 typedef struct binamp {
00147 int bin;
00148 double amp;
00149 };
00150
00151
00152
00153 int compare_binamp(const binamp *a, const binamp *b) {
00154
00155 if (( (*a).amp - (*b).amp) < 0)
00156 return 1;
00157 if (( (*a).amp - (*b).amp) > 0)
00158 return -1;
00159
00160 return 0;
00161 }
00162
00163
00164 double psd_max_again(const fftw_complex *transformed_signal, const int size) {
00165
00166 vector<double> psd_plus((size-1)/2), psd_minus((size-1)/2);
00167 double Nyquist=0;
00168
00169 int k;
00170
00171 const double DC = norm(transformed_signal[0])/size;
00172 for (k=0;k<(size-1)/2;k++)
00173 psd_plus[k] = norm(transformed_signal[k+1])/size;
00174 for (k=0;k<(size-1)/2;k++)
00175 psd_minus[k] = norm(transformed_signal[size-(k+1)])/size;
00176 if ( (size%2) == 0)
00177 Nyquist = norm(transformed_signal[size/2])/size;
00178
00179
00180
00181
00182 if (0) {
00183 static int filenum=0;
00184 char filename[20];
00185 sprintf(filename,"dump_psd_%02i.dat",filenum);
00186 cerr << "data dump on file " << filename << endl;
00187 FILE *fp = fopen(filename,"w");
00188 if (fp!=0) {
00189 int l;
00190 for (l=psd_minus.size()-1;l>=0;l--) fprintf(fp,"%i %g\n",-(l+1),psd_minus[l]);
00191 fprintf(fp,"%i %g\n",0,DC);
00192 for (l=0;l<(int)psd_plus.size(); l++) fprintf(fp,"%i %g\n", (l+1),psd_plus[l]);
00193 if ( (size%2) == 0) fprintf(fp,"%i %g\n",size/2,Nyquist);
00194 }
00195 fclose(fp);
00196 filenum++;
00197 }
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 double maxpow=DC;
00211 int bin=0;
00212
00213 for (k=1;k<(size-3)/2;k++) {
00214 if ( (psd_plus[k] > psd_plus[k-1]) &&
00215 (psd_plus[k] > psd_plus[k+1]) &&
00216 (psd_plus[k] > maxpow) ) {
00217 maxpow = psd_plus[k];
00218 bin = k+1;
00219 }
00220 }
00221
00222 if ( (psd_plus[0] > DC) &&
00223 (psd_plus[0] > psd_plus[1]) &&
00224 (psd_plus[0] > maxpow) ) {
00225 maxpow = psd_plus[0];
00226 bin = 1;
00227 }
00228
00229 for (k=1;k<(size-3)/2;k++) {
00230 if ( (psd_minus[k] > psd_minus[k-1]) &&
00231 (psd_minus[k] > psd_minus[k+1]) &&
00232 (psd_minus[k] > maxpow) ) {
00233 maxpow = psd_minus[k];
00234 bin = -(k+1);
00235 }
00236 }
00237
00238 if ( (psd_minus[0] > DC) &&
00239 (psd_minus[0] > psd_minus[1]) &&
00240 (psd_minus[0] > maxpow) ) {
00241 maxpow = psd_minus[0];
00242 bin = -1;
00243 }
00244
00245 if ( (DC > psd_plus[0]) &&
00246 (DC > psd_minus[0]) &&
00247 (DC > maxpow) ) {
00248 maxpow = DC;
00249 bin = 0;
00250 }
00251
00252
00253
00254
00255
00256
00257 if (0) {
00258 int filenum=0;
00259 char filename[20];
00260 cerr << "DC: " << DC << endl;
00261 cerr << "size: " << size << endl;
00262 sprintf(filename,"dump_psd_%02i.dat",filenum);
00263 cerr << "data dump on file " << filename << endl;
00264 FILE *fp = fopen(filename,"w");
00265 if (fp!=0) {
00266 int l;
00267 for (l=psd_minus.size()-1;l>=0;l--) fprintf(fp,"%i %g\n",-(l+1),psd_minus[l]);
00268 fprintf(fp,"%i %g\n",0,DC);
00269 for (l=0;l<(int)psd_plus.size(); l++) fprintf(fp,"%i %g\n", (l+1),psd_plus[l]);
00270 if ( (size%2) == 0) fprintf(fp,"%i %g\n",size/2,Nyquist);
00271 }
00272 fclose(fp);
00273 filenum++;
00274 }
00275
00276 if (maxpow>0) return ((double)bin/(double)size);
00277
00278
00279
00280
00281
00282
00283 return ((double)bin/(double)size);
00284 }
00285
00286 void psd_max_again_many(const fftw_complex *transformed_signal, const int size, vector<double> &candidate, const unsigned int nfreq) {
00287
00288 vector<double> psd_plus((size-1)/2), psd_minus((size-1)/2);
00289 double Nyquist=0;
00290
00291 int k;
00292
00293 const double DC = norm(transformed_signal[0])/size;
00294 for (k=0;k<(size-1)/2;k++)
00295 psd_plus[k] = norm(transformed_signal[k+1])/size;
00296 for (k=0;k<(size-1)/2;k++)
00297 psd_minus[k] = norm(transformed_signal[size-(k+1)])/size;
00298 if ( (size%2) == 0)
00299 Nyquist = norm(transformed_signal[size/2])/size;
00300
00301 vector<binamp> vec_binamp;
00302 binamp ba;
00303
00304 for (k=1;k<(size-3)/2;k++) {
00305 if ( (psd_plus[k] > psd_plus[k-1]) &&
00306 (psd_plus[k] > psd_plus[k+1]) ) {
00307
00308 ba.bin = k+1;
00309 ba.amp = psd_plus[k];
00310 vec_binamp.push_back(ba);
00311 }
00312 }
00313
00314 if ( (psd_plus[0] > DC) &&
00315 (psd_plus[0] > psd_plus[1]) ) {
00316
00317 ba.bin = 1;
00318 ba.amp = psd_plus[0];
00319 vec_binamp.push_back(ba);
00320 }
00321
00322 for (k=1;k<(size-3)/2;k++) {
00323 if ( (psd_minus[k] > psd_minus[k-1]) &&
00324 (psd_minus[k] > psd_minus[k+1]) ) {
00325
00326 ba.bin = -(k+1);
00327 ba.amp = psd_minus[k];
00328 vec_binamp.push_back(ba);
00329 }
00330 }
00331
00332 if ( (psd_minus[0] > DC) &&
00333 (psd_minus[0] > psd_minus[1]) ) {
00334
00335 ba.bin = -1;
00336 ba.amp = psd_minus[0];
00337 vec_binamp.push_back(ba);
00338 }
00339
00340 if ( (DC > psd_plus[0]) &&
00341 (DC > psd_minus[0]) ) {
00342
00343 ba.bin = 0;
00344 ba.amp = DC;
00345 vec_binamp.push_back(ba);
00346 }
00347
00348
00349 unsigned int bsize = vec_binamp.size();
00350 binamp *ptr_binamp;
00351 ptr_binamp = (binamp *) malloc(bsize*sizeof(binamp));
00352 {
00353 unsigned int k;
00354 for (k=0;k<bsize;k++) {
00355 ptr_binamp[k].bin = vec_binamp[k].bin;
00356 ptr_binamp[k].amp = vec_binamp[k].amp;
00357 }
00358 }
00359 gsl_heapsort(ptr_binamp,bsize,sizeof(binamp),(gsl_comparison_fn_t)compare_binamp);
00360
00361 candidate.clear();
00362
00363 {
00364 unsigned int k;
00365 for (k=0;k<nfreq;k++) {
00366 candidate.push_back((double)ptr_binamp[k].bin/(double)size);
00367 }
00368 }
00369
00370 free(ptr_binamp);
00371 }
00372
00373
00374
00375 double psd_max(const fftw_complex *transformed_signal, const int size) {
00376
00377 vector<double> psd;
00378
00379 int k;
00380
00381 psd.resize(size/2+1);
00382 psd[0] = norm(transformed_signal[0])/size;
00383 for (k=1;k<(size+1)/2;k++)
00384 psd[k] = sqrt(norm_sq(transformed_signal[k])+norm_sq(transformed_signal[size-k]))/size;
00385 if ( (size%2) == 0)
00386 psd[size/2] = norm(transformed_signal[size/2])/size;
00387
00388
00389
00390
00391
00392
00393
00394 double maxamp=0;
00395 int bin=0;
00396 unsigned int l;
00397
00398 double found=false;
00399 if ( (psd[0] > psd[1]) &&
00400 (psd[0] > maxamp) ) {
00401 maxamp = psd[0];
00402 bin = 0;
00403 found = true;
00404 }
00405
00406 for (l=1;l<(psd.size()-1);l++) {
00407 if ( ( (psd[l] > psd[l-1]) && (psd[l] > psd[l+1]) ) &&
00408 (psd[l] > maxamp) ) {
00409 maxamp = psd[l];
00410 bin = l;
00411 found = true;
00412 }
00413 }
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424 if (0) {
00425 int filenum=0;
00426 char filename[20];
00427 sprintf(filename,"dump_psd_%02i.dat",filenum);
00428 cerr << "data dump on file " << filename << endl;
00429 FILE *fp = fopen(filename,"w");
00430 if (fp!=0) {
00431 for (l=0;l<psd.size();l++) {
00432 fprintf(fp,"%i %g\n",l,psd[l]);
00433 }
00434 }
00435 fclose(fp);
00436 filenum++;
00437 }
00438
00439
00440
00441 if (found) return ((double)bin/(double)size);
00442 else {
00443 cerr << "\n *\n **\n ***\n ****\n*************\n************** WARNING!!! peak don't found... returning (-1)!!\n*************\n ****\n ***\n **\n *\n" << endl;
00444 return (-1);
00445 }
00446 }
00447
00448
00449 void apply_window(fftw_complex *signal_win, fftw_complex *signal, int size) {
00450
00451 int j;
00452 double win_coeff,arg;
00453
00454 for(j=0;j<size;j++) {
00455
00456 arg = twopi*j/size;
00457 win_coeff = (1. - cos(arg));
00458
00459 signal_win[j].re = signal[j].re * win_coeff;
00460 signal_win[j].im = signal[j].im * win_coeff;
00461
00462 }
00463
00464 }
00465
00466
00467 void amph(double *amp, double *phase, fftw_complex *phiR, fftw_complex *phiL, double freq, fftw_complex *in, int size) {
00468
00469 *phiR = phi(freq, in,size);
00470 *phiL = phi(-freq,in,size);
00471
00472 *amp = norm(*phiR);
00473 *phase = secure_atan2(phiR->im,phiR->re);
00474
00475
00476
00477 }
00478
00479 double accurate_peak(double left, double center, double right, fftw_complex *in, int size) {
00480
00481
00482
00483 if ( (center<(-0.5)) || (center>0.5) ) {
00484 cerr << "warning!! Peak out of range!" << endl;
00485 return center;
00486 }
00487
00488
00489 int max_iter = 100;
00490 double center_amp, left_amp, right_amp;
00491
00492 left_amp = norm(phi(left, in,size));
00493 right_amp = norm(phi(right, in,size));
00494 center_amp = norm(phi(center,in,size));
00495
00496
00497
00498
00499 gsl_d1_minimization_parameters par;
00500 gsl_function F;
00501 gsl_min_fminimizer *s;
00502 const gsl_min_fminimizer_type *T;
00503
00504 T = gsl_min_fminimizer_goldensection;
00505 s = gsl_min_fminimizer_alloc(T);
00506
00507 par.size = size;
00508 par.pointer_points_sequence = &in[0];
00509
00510 F.function = &phi_gsl_two;
00511 F.params = ∥
00512
00513 gsl_min_fminimizer_set(s,&F,center,left,right);
00514
00515 double epsrel = 1.0e-10;
00516 double epsabs = 0.0;
00517
00518 int iter = 0;
00519 int status;
00520 do {
00521 iter++;
00522
00523 status = gsl_min_fminimizer_iterate (s);
00524
00525 center = gsl_min_fminimizer_minimum (s);
00526 left = gsl_min_fminimizer_x_lower (s);
00527 right = gsl_min_fminimizer_x_upper (s);
00528
00529
00530 status = gsl_min_test_interval(left,right,epsrel,epsabs);
00531
00532
00533
00534
00535
00536 } while ((status == GSL_CONTINUE) && (iter < max_iter));
00537
00538 return (center);
00539 }
00540
00541
00542 FFT::FFT(OrbitStream &o, FFTPowerSpectrum &psd, vector<Peak> &pks, FFTDataStream &rds) {
00543
00544 os = &o;
00545
00546 fps = &psd;
00547 fps->resize(0);
00548
00549 reconstructed_data_stream = &rds;
00550
00551 peaks = &pks;
00552 peaks->resize(0);
00553
00554 default_peak_reset_frequency = 1.0e-100;
00555 default_peak_reset_amplitude = 1.0e-4;
00556 default_peak_reset_phase = 0.0;
00557
00558 HiResSpectrum = false;
00559
00560 relative_amplitude = 0.05;
00561
00562
00563
00564
00565
00566 T = gsl_min_fminimizer_goldensection;
00567 s = gsl_min_fminimizer_alloc (T);
00568
00569
00570
00571
00572
00573
00574 nfreq = 4;
00575
00576 }
00577
00578
00579 void FFT::FillDataStream(FFTSearch s) {
00580
00581
00582
00583 if (s == HK) {
00584
00585
00586 OrbitStream &ostr = *os;
00587
00588 data_stream.timestep = ostr.timestep;
00589
00590
00591
00592
00593
00594
00595
00596 data_stream.resize(0);
00597
00598 FFTDataStructure tmp_ds;
00599
00600
00601
00602 unsigned int k;
00603 for (k=0;k<ostr.size();k++) {
00604
00605
00606
00607 tmp_ds.time = ostr[k].epoch.Time();
00608 tmp_ds.amplitude = ostr[k].e;
00609 tmp_ds.phase = ostr[k].omega_node + ostr[k].omega_pericenter;
00610
00611 data_stream.push_back(tmp_ds);
00612 }
00613
00614
00615
00616 } else if (s == D) {
00617
00618 OrbitStream &ostr = *os;
00619 data_stream.timestep = ostr.timestep;
00620 data_stream.resize(0);
00621
00622 FFTDataStructure tmp_ds;
00623
00624 unsigned int k;
00625 for (k=0;k<ostr.size();k++) {
00626
00627
00628
00629 tmp_ds.time = ostr[k].epoch.Time();
00630 tmp_ds.amplitude = ostr[k].libration_angle;
00631 tmp_ds.phase = 0.0;
00632
00633 data_stream.push_back(tmp_ds);
00634 }
00635
00636 } else if (s == PQ) {
00637
00638 OrbitStream &ostr = *os;
00639 data_stream.timestep = ostr.timestep;
00640 data_stream.resize(0);
00641
00642 FFTDataStructure tmp_ds;
00643
00644 unsigned int k;
00645 for (k=0;k<ostr.size();k++) {
00646
00647
00648
00649 tmp_ds.time = ostr[k].epoch.Time();
00650 tmp_ds.amplitude = sin(ostr[k].i);
00651 tmp_ds.phase = ostr[k].omega_node;
00652
00653 data_stream.push_back(tmp_ds);
00654 }
00655
00656 } else if (s == NODE) {
00657
00658 OrbitStream &ostr = *os;
00659 data_stream.timestep = ostr.timestep;
00660 data_stream.resize(0);
00661
00662 FFTDataStructure tmp_ds;
00663
00664 unsigned int k;
00665 for (k=0;k<ostr.size();k++) {
00666
00667
00668
00669 tmp_ds.time = ostr[k].epoch.Time();
00670 tmp_ds.amplitude = ostr[k].omega_node;
00671 tmp_ds.phase = 0.0;
00672
00673 data_stream.push_back(tmp_ds);
00674 }
00675 } else if (s == ANOMALY) {
00676
00677 OrbitStream &ostr = *os;
00678 data_stream.timestep = ostr.timestep;
00679 data_stream.resize(0);
00680
00681 FFTDataStructure tmp_ds;
00682
00683 unsigned int k;
00684 for (k=0;k<ostr.size();k++) {
00685
00686
00687
00688 tmp_ds.time = ostr[k].epoch.Time();
00689 tmp_ds.amplitude = ostr[k].M;
00690 tmp_ds.phase = 0.0;
00691
00692 data_stream.push_back(tmp_ds);
00693 }
00694 } else if (s == ANOMALY_PHASE) {
00695
00696 OrbitStream &ostr = *os;
00697 data_stream.timestep = ostr.timestep;
00698 data_stream.resize(0);
00699
00700 FFTDataStructure tmp_ds;
00701
00702 unsigned int k;
00703 for (k=0;k<ostr.size();k++) {
00704
00705
00706
00707 tmp_ds.time = ostr[k].epoch.Time();
00708 tmp_ds.amplitude = 1.0;
00709 tmp_ds.phase = ostr[k].M;
00710
00711 data_stream.push_back(tmp_ds);
00712 }
00713 } else if (s == A_M) {
00714
00715 OrbitStream &ostr = *os;
00716 data_stream.timestep = ostr.timestep;
00717 data_stream.resize(0);
00718
00719 FFTDataStructure tmp_ds;
00720
00721 unsigned int k;
00722 for (k=0;k<ostr.size();k++) {
00723
00724
00725
00726 tmp_ds.time = ostr[k].epoch.Time();
00727 tmp_ds.amplitude = ostr[k].a;
00728 tmp_ds.phase = ostr[k].M;
00729
00730 data_stream.push_back(tmp_ds);
00731 }
00732 }
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757 }
00758
00759 void FFT::FillDataStream(FFTSearchAmplitude sa, FFTSearchPhase sp) {
00760 OrbitStream &ostr = *os;
00761 data_stream.clear();
00762 data_stream.timestep = ostr.timestep;
00763 FFTDataStructure tmp_ds;
00764 const unsigned int size = ostr.size();
00765 data_stream.resize(size);
00766 unsigned int k;
00767
00768 switch (sa) {
00769 case A: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = ostr[k].a; } break;
00770 case E: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = ostr[k].e; } break;
00771 case I: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = ostr[k].i; } break;
00772 case SIN_I: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = sin(ostr[k].i); } break;
00773 case TAN_I_2: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = tan(ostr[k].i/2); } break;
00774 case ONE: for(k=0;k<size;k++) { data_stream[k].time = ostr[k].epoch.Time(); data_stream[k].amplitude = 1.0; } break;
00775 }
00776
00777 switch (sp) {
00778 case OMEGA_NODE: for(k=0;k<size;k++) { data_stream[k].phase = ostr[k].omega_node; } break;
00779 case OMEGA_PERICENTER: for(k=0;k<size;k++) { data_stream[k].phase = ostr[k].omega_pericenter; } break;
00780 case OMEGA_TILDE: for(k=0;k<size;k++) { data_stream[k].phase = ostr[k].omega_node + ostr[k].omega_pericenter; } break;
00781 case MM: for(k=0;k<size;k++) { data_stream[k].phase = ostr[k].M; } break;
00782 case LAMBDA: for(k=0;k<size;k++) { data_stream[k].phase = ostr[k].omega_node + ostr[k].omega_pericenter + ostr[k].M; } break;
00783 case ZERO: for(k=0;k<size;k++) { data_stream[k].phase = 0.0; } break;
00784 }
00785 }
00786
00787 void FFT::Search(FFTSearch se, FFTAlgorithm algo) {
00788
00789 FillDataStream(se);
00790
00791 if (algo==algo_FFT) {
00792 Search_FFT();
00793 } else if (algo==algo_FFTB) {
00794 Search_FFTB();
00795 } else if (algo==algo_MFT) {
00796 Search_MFT();
00797 } else if (algo==algo_FMFT1) {
00798 Search_FMFT1();
00799 } else if (algo==algo_FMFT2) {
00800 Search_FMFT2();
00801 }
00802
00803 ComputeCommonPowerSpectrum();
00804 ComputeCommonReconstructedSignal();
00805
00806 }
00807
00808 void FFT::Search(FFTSearchAmplitude sa, FFTSearchPhase sp, FFTAlgorithm algo) {
00809
00810 FillDataStream(sa,sp);
00811
00812 if (algo==algo_FFT) {
00813 Search_FFT();
00814 } else if (algo==algo_FFTB) {
00815 Search_FFTB();
00816 } else if (algo==algo_MFT) {
00817 Search_MFT();
00818 } else if (algo==algo_FMFT1) {
00819 Search_FMFT1();
00820 } else if (algo==algo_FMFT2) {
00821 Search_FMFT2();
00822 }
00823
00824 ComputeCommonPowerSpectrum();
00825 ComputeCommonReconstructedSignal();
00826
00827 }
00828
00829 void FFT::ComputeCommonPowerSpectrum() {
00830
00831
00832
00833 unsigned int j,k;
00834 unsigned int size = data_stream.size();
00835
00836
00837
00838 fftw_complex *in, *work, *out;
00839 in = (fftw_complex*)malloc(size*sizeof(fftw_complex));
00840 work = (fftw_complex*)malloc(size*sizeof(fftw_complex));
00841 out = (fftw_complex*)malloc(size*sizeof(fftw_complex));
00842
00843 for (j=0;j<size;j++) {
00844 in[j].re = data_stream[j].amplitude * cos(data_stream[j].phase);
00845 in[j].im = data_stream[j].amplitude * sin(data_stream[j].phase);
00846 }
00847
00848 apply_window(work,in,size);
00849
00850 fftw_plan plan;
00851 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
00852 fftw_one(plan, work, out);
00853
00854 fftw_destroy_plan(plan);
00855
00856 vector<double> psd_plus((size-1)/2), psd_minus((size-1)/2);
00857 double DC, Nyquist=0;
00858
00859 DC = norm(out[0])/size;
00860 for (k=0;k<(size-1)/2;k++)
00861 psd_plus[k] = norm(out[k+1])/size;
00862 for (k=0;k<(size-1)/2;k++)
00863 psd_minus[k] = norm(out[size-(k+1)])/size;
00864 if ( (size%2) == 0)
00865 Nyquist = norm(out[size/2])/size;
00866
00867 if (HiResSpectrum) {
00868 const int resample = 5;
00869 const double timestep = data_stream.timestep;
00870 FFTPowerSpectrum &FFTps = *fps;
00871 FFTPowerSpectrumBaseElement tps;
00872 FFTps.clear();
00873 const double freq_start = -7.0e-5;
00874 const double freq_stop = 0.0e-5;
00875 const double freq_incr = 1.0/((double)size*resample)/timestep;
00876 double freq=freq_start;
00877 while (freq <= freq_stop) {
00878 tps.frequency = freq;
00879 tps.power = phi_Hanning_amp(freq*timestep,in,size);
00880 FFTps.push_back(tps);
00881 freq += freq_incr;
00882 }
00883 } else {
00884 int l;
00885 const double timestep = data_stream.timestep;
00886 FFTPowerSpectrum &FFTps = *fps;
00887 FFTPowerSpectrumBaseElement tps;
00888 FFTps.clear();
00889 for (l=psd_minus.size()-1;l>=0;l--) {
00890
00891 tps.frequency = ((double)-(l+1)/(double)size)/timestep;
00892 tps.power = psd_minus[l];
00893 FFTps.push_back(tps);
00894 }
00895
00896 tps.frequency = 0;
00897 tps.power = DC;
00898 FFTps.push_back(tps);
00899
00900 {
00901 unsigned int l;
00902 for (l=0;l<psd_plus.size(); l++) {
00903
00904 tps.frequency = ((double)(l+1)/(double)size)/timestep;
00905 tps.power = psd_plus[l];
00906 FFTps.push_back(tps);
00907 }
00908 }
00909 if ( (size%2) == 0) {
00910
00911 tps.frequency = ((double)(size/2)/(double)size)/timestep;
00912 tps.power = Nyquist;
00913 FFTps.push_back(tps);
00914 }
00915 }
00916
00917 free(in); free(work); free(out);
00918 }
00919
00920 void FFT::ComputeCommonReconstructedSignal() {
00921
00922 unsigned int size = data_stream.size();
00923 reconstructed_data_stream->resize(size);
00924 unsigned int j,pk;
00925 fftw_complex z;
00926 double arg, arg_j,time_zero=data_stream[0].time;
00927 for (j=0;j<size;j++) {
00928 (*reconstructed_data_stream)[j].time = data_stream[j].time;
00929
00930 arg_j = twopi*(data_stream[j].time-time_zero);
00931 z.re = z.im = 0;
00932 for (pk=0;pk<peaks->size();pk++) {
00933 arg = arg_j*(*peaks)[pk].frequency+(*peaks)[pk].phase;
00934 z.re += (*peaks)[pk].amplitude*cos(arg);
00935 z.im += (*peaks)[pk].amplitude*sin(arg);
00936 }
00937 (*reconstructed_data_stream)[j].amplitude = norm(z);
00938 (*reconstructed_data_stream)[j].phase = secure_atan2(z.im,z.re);
00939 }
00940
00941 }
00942
00943 void FFT::Search_FFT() {
00944
00945
00946
00947 unsigned int size = data_stream.size();
00948
00949 vector<Peak> &pks = *peaks;
00950
00951 fftw_plan plan;
00952
00953
00954 fftw_complex *in, *out;
00955 in = (fftw_complex*)malloc(size*sizeof(fftw_complex));
00956 out = (fftw_complex*)malloc(size*sizeof(fftw_complex));
00957
00958 const double timestep = data_stream.timestep;
00959
00960 unsigned int j,k;
00961
00962 for (j=0;j<size;j++) {
00963 in[j].re = data_stream[j].amplitude * cos(data_stream[j].phase);
00964 in[j].im = data_stream[j].amplitude * sin(data_stream[j].phase);
00965 }
00966
00967 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
00968 fftw_one(plan, in, out);
00969 fftw_destroy_plan(plan);
00970
00971 psd.resize(size/2+1);
00972 psd[0] = norm(out[0])/size;
00973 for (k = 1; k < (size+1)/2; ++k)
00974 psd[k] = sqrt(norm_sq(out[k])+norm_sq(out[size-k]))/size;
00975 if (size % 2 == 0)
00976 psd[size/2] = norm(out[size/2])/size;
00977
00978
00979 if (HiResSpectrum) {
00980
00981 int resample = 5;
00982 unsigned int l;
00983 FFTPowerSpectrum &FFTps = *fps;
00984 FFTPowerSpectrumBaseElement tps;
00985 FFTps.resize(0);
00986 for (l=0;l<psd.size()*resample;l++) {
00987 tps.frequency = ((double)l/(double)(size*resample))/timestep;
00988
00989
00990
00991 tps.power = phi_Hanning_amp(((double)l/(double)(size*resample)),in,size);
00992
00993 FFTps.push_back(tps);
00994 }
00995
00996 } else {
00997
00998 unsigned int l;
00999 FFTPowerSpectrum &FFTps = *fps;
01000 FFTPowerSpectrumBaseElement tps;
01001 FFTps.resize(0);
01002 for (l=0;l<psd.size();l++) {
01003 tps.frequency = ((double)l/(double)size)/timestep;
01004 tps.power = psd[l];
01005
01006 FFTps.push_back(tps);
01007 }
01008
01009 }
01010
01011
01012
01013
01014
01015
01016
01017
01018 pks.resize(0);
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028 bool genuine_peak;
01029 double range_start=0.0,range_stop=0.5;
01030 double phi_tmp;
01031 unsigned int ip;
01032 Peak test_peak;
01033 unsigned int bin = 0;
01034
01035
01036 unsigned int bsize = psd.size();
01037 binamp *ptr_binamp = (binamp *) malloc(bsize*sizeof(binamp));
01038 for (k=0;k<bsize;k++) {
01039 ptr_binamp[k].bin = k;
01040 ptr_binamp[k].amp = psd[k];
01041 }
01042 gsl_heapsort(ptr_binamp,bsize,sizeof(binamp),(gsl_comparison_fn_t)compare_binamp);
01043
01044 candidate_bin.resize(0);
01045
01046 double maxamp, relamp;
01047 maxamp = ptr_binamp[0].amp;
01048 k=0;
01049 do {
01050 relamp = ptr_binamp[k].amp / maxamp;
01051 if ((ptr_binamp[k].bin==0) || ptr_binamp[k].bin==(int)bsize-1) {
01052 candidate_bin.push_back(ptr_binamp[k].bin);
01053 } else {
01054 if ((psd[ptr_binamp[k].bin-1] < ptr_binamp[k].amp) &&
01055 (psd[ptr_binamp[k].bin+1] < ptr_binamp[k].amp)) {
01056 candidate_bin.push_back(ptr_binamp[k].bin);
01057 }
01058 }
01059 k++;
01060 } while ((k<bsize) && (candidate_bin.size() != nfreq));
01061
01062
01063 free(ptr_binamp);
01064
01065
01066
01067
01068 unsigned int candidate_bin_counter=0;
01069 for (candidate_bin_counter=0;candidate_bin_counter<candidate_bin.size();candidate_bin_counter++) {
01070
01071 bin = candidate_bin[candidate_bin_counter];
01072
01073 genuine_peak = true;
01074 test_peak.frequency = (double)bin/(double)size;
01075
01076 test_peak.amplitude = phi_amp(test_peak.frequency,&in[0],size);
01077
01078
01079
01080
01081
01082
01083
01084
01085 if (bin == 0) test_peak.amplitude /= sqrt(2.0);
01086
01087 ip = 0;
01088
01089 if (genuine_peak == true) {
01090
01091 if (bin != 0) {
01092 ip = 0;
01093 do {
01094 ip++;
01095 range_start = test_peak.frequency - 0.1*(double)ip/(double)size;
01096
01097
01098 phi_tmp = phi_amp(range_start,&in[0],size);
01099
01100
01101
01102
01103
01104
01105
01106 } while ( (range_start >= 0) &&
01107 (ip < 100) &&
01108 (phi_tmp >= test_peak.amplitude) );
01109
01110 if ( (ip >= 100) ||
01111 (range_start < 0) ) {
01112 genuine_peak = false;
01113 } else {
01114 ip = 0;
01115 do {
01116 ip++;
01117 range_stop = test_peak.frequency + 0.1*(double)ip/(double)size;
01118
01119
01120 phi_tmp = phi_amp(range_stop,&in[0],size);
01121
01122
01123
01124
01125
01126
01127
01128
01129 } while ( (range_stop <= 0.5) &&
01130 (ip < 100) &&
01131 (phi_tmp >= test_peak.amplitude) );
01132
01133 if ( (ip >= 100) ||
01134 (range_stop > 0.5) ) {
01135 genuine_peak = false;
01136 }
01137 }
01138 }
01139 }
01140
01141
01142 if (bin == 0) test_peak.amplitude /= sqrt(2.0);
01143
01144
01145 if (genuine_peak) {
01146
01147 if (bin != 0) {
01148
01149
01150 par.size = size;
01151 par.pointer_points_sequence = &in[0];
01152
01153
01154
01155
01156 F.function = &phi_gsl;
01157
01158
01159
01160
01161
01162
01163
01164
01165 F.params = ∥
01166
01167 gsl_min_fminimizer_set(s,&F,test_peak.frequency,range_start,range_stop);
01168
01169 double epsrel = 1.0e-9;
01170 double epsabs = 0.0;
01171
01172 unsigned int iter = 0;
01173 unsigned int max_iter = 100;
01174 int status;
01175 do {
01176 iter++;
01177
01178 status = gsl_min_fminimizer_iterate (s);
01179
01180 test_peak.frequency = gsl_min_fminimizer_minimum (s);
01181 range_start = gsl_min_fminimizer_x_lower (s);
01182 range_stop = gsl_min_fminimizer_x_upper (s);
01183
01184
01185 status = gsl_min_test_interval (range_start, range_stop, epsrel, epsabs);
01186
01187
01188
01189
01190
01191 } while ((status == GSL_CONTINUE) && (iter < max_iter));
01192
01193 }
01194
01195
01196
01197 test_peak.amplitude = phi_amp(test_peak.frequency,&in[0],size);
01198
01199
01200
01201
01202
01203
01204
01205
01206 if (bin == 0) test_peak.amplitude /= sqrt(2.0);
01207
01208
01209
01210 bool new_peak = true;
01211 unsigned int wp;
01212 if (pks.size() != 0) {
01213 for (wp=0;wp<pks.size();wp++) {
01214 if ( test_peak.amplitude > (0.9999) * pks[wp].amplitude &&
01215 test_peak.amplitude < (1.0001) * pks[wp].amplitude )
01216 new_peak = false;
01217
01218 if ( test_peak.frequency > (0.9999) * pks[wp].frequency &&
01219 test_peak.frequency < (1.0001) * pks[wp].frequency )
01220 new_peak = false;
01221 }
01222 }
01223
01224
01225 if (new_peak) {
01226 test_peak.phiR = phi(+test_peak.frequency,&in[0],size);
01227 test_peak.phiL = phi(-test_peak.frequency,&in[0],size);
01228 test_peak.phase = secure_atan2(test_peak.phiR.im,test_peak.phiR.re);
01229 pks.push_back(test_peak);
01230
01231 }
01232
01233 }
01234
01235 }
01236
01237
01238
01239 for (j=0;j<pks.size();j++) {
01240 pks[j].frequency /= timestep;
01241 }
01242
01243
01244
01245
01246 free(in); free(out);
01247 }
01248
01249 void FFT::Search_FFTB() {
01250
01251 const unsigned int size = data_stream.size();
01252 const double timestep = data_stream.timestep;
01253
01254 fftw_complex *in, *work, *out;
01255 in = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01256 work = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01257 out = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01258
01259 unsigned int j;
01260 for (j=0;j<size;j++) {
01261 in[j].re = data_stream[j].amplitude * cos(data_stream[j].phase);
01262 in[j].im = data_stream[j].amplitude * sin(data_stream[j].phase);
01263 }
01264
01265 apply_window(work,in,size);
01266
01267
01268 fftw_plan plan;
01269 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
01270 fftw_one(plan, work, out);
01271 fftw_destroy_plan(plan);
01272
01273 vector<double> candidate_freq;
01274 psd_max_again_many(out,size,candidate_freq,nfreq);
01275
01276
01277
01278
01279
01280
01281 double f,A,psi;
01282 fftw_complex phiR,phiL;
01283 double centerf,leftf,rightf;
01284
01285 unsigned int l;
01286 Peak tp;
01287 peaks->resize(candidate_freq.size());
01288 for (l=0;l<candidate_freq.size();l++) {
01289
01290 centerf = candidate_freq[l];
01291 leftf = centerf-1.0/size;
01292 rightf = centerf+1.0/size;
01293
01294 f = accurate_peak(leftf,centerf,rightf,work,size);
01295
01296 amph(&A,&psi,&phiR,&phiL,f,work,size);
01297
01298 tp.frequency = f/timestep;
01299 tp.amplitude = A;
01300 tp.phase = psi;
01301
01302 tp.phiR = phiR;
01303 tp.phiL = phiL;
01304
01305 (*peaks)[l] = tp;
01306
01307 }
01308
01309 free(in); free(work); free(out);
01310
01311 }
01312
01313 void FFT::Search_MFT() {
01314 Search_FMFT_main();
01315 }
01316
01317
01318
01319 double dQ(double y) {
01320
01321
01322 if (fabs(y) < 1.0e-12) return (0.0);
01323
01324
01325 if (fabs(y-pi) < 1.0e-12) return (-3.0/(4*pi));
01326
01327
01328 if (fabs(y+pi) < 1.0e-12) return ( 3.0/(4*pi));
01329
01330 double ysq=y*y;
01331 return (pisq/(y*(pisq-ysq))*(cos(y)+(sin(y)/y)*((3*ysq-pisq)/(pisq-ysq))));
01332
01333 }
01334
01335 void FFT::Search_FMFT1() {
01336
01337 Search_FMFT_main();
01338
01339
01340 const vector<Peak> peaks_MFT(*peaks);
01341 const double timestep = data_stream.timestep;
01342
01343 unsigned int peaks_size = peaks_MFT.size();
01344 unsigned int size = data_stream.size();
01345
01346 vector<double> epsilon(peaks_size);
01347
01348
01349 double ddQ_zero = 2.0/pisq - 1.0/3.0;
01350
01351 double eps;
01352 double y_sj;
01353
01354 unsigned int j,s;
01355 for (j=0;j<peaks_size-1;j++) {
01356
01357 eps=0.0;
01358 for (s=j+1;s<peaks_size;s++) {
01359 y_sj = (peaks_MFT[s].frequency - peaks_MFT[j].frequency)*timestep*(size/2.0);
01360
01361
01362 eps += peaks_MFT[s].amplitude * dQ(twopi*y_sj) * cos(twopi*y_sj+peaks_MFT[s].phase-peaks_MFT[j].phase);
01363 }
01364 eps /= peaks_MFT[j].amplitude*ddQ_zero*timestep*(size/2.0);
01365
01366
01367
01368 epsilon[j] = eps;
01369 printf("epsilon[%i]: %.20g\n",j,eps);
01370 }
01371
01372
01373 vector<double> f(nfreq), A(nfreq), psi(nfreq);
01374 {
01375 double Q[nfreq][nfreq], alpha[nfreq][nfreq];
01376 double B[nfreq];
01377
01378 Q[0][0] = 1;
01379 alpha[0][0] = 1;
01380
01381 double fac,xsum,ysum;
01382 f[0] = ( peaks_MFT[0].frequency - epsilon[0] ) * timestep;
01383 psi[0] = peaks_MFT[0].phase;
01384 A[0] = peaks_MFT[0].amplitude;
01385
01386 unsigned int k,m;
01387 for(m=1;m<nfreq;m++) {
01388
01389 f[m] = ( peaks_MFT[m].frequency - epsilon[m] ) * timestep;
01390 psi[m] = peaks_MFT[m].phase;
01391 A[m] = peaks_MFT[m].amplitude;
01392
01393
01394 Q[m][m] = 1;
01395 for(j=0;j<m;j++) {
01396 fac = (f[m]-f[j])*size/2.;
01397 fac *= twopi;
01398
01399
01400 if (fabs(fac) < 1.0e-12) {
01401 Q[m][j] = 1;
01402 } else if (fabs(fabs(fac)-pi)< 1.0e-12) {
01403 Q[m][j] = 0.5;
01404 } else {
01405 Q[m][j] = sin(fac)/fac * pisq / (pisq - fac*fac);
01406 }
01407
01408 Q[j][m] = Q[m][j];
01409 }
01410
01411
01412 for(k=0;k<m;k++){
01413 B[k] = 0;
01414 for(j=0;j<=k;j++)
01415 B[k] += -alpha[k][j]*Q[m][j];
01416 }
01417
01418
01419 alpha[m][m] = 1;
01420 for(j=0;j<m;j++)
01421 alpha[m][m] -= B[j]*B[j];
01422 alpha[m][m] = 1. / sqrt(alpha[m][m]);
01423
01424
01425 for(k=0;k<m;k++) {
01426 alpha[m][k] = 0;
01427
01428 for(j=k;j<m;j++)
01429 alpha[m][k] += B[j]*alpha[j][k];
01430
01431 alpha[m][k] = alpha[m][m]*alpha[m][k];
01432 }
01433
01434 }
01435
01436
01437 for(k=0;k<nfreq;k++) {
01438 xsum=0; ysum=0;
01439 for(j=k;j<nfreq;j++) {
01440 fac = twopi*((f[j]-f[k])*size/2.) + psi[j];
01441 xsum += alpha[j][j]*alpha[j][k]*A[j]*cos(fac);
01442 ysum += alpha[j][j]*alpha[j][k]*A[j]*sin(fac);
01443 }
01444 A[k] = sqrt(xsum*xsum+ysum*ysum);
01445 psi[k] = secure_atan2(ysum,xsum);
01446 }
01447 }
01448
01449 unsigned int k;
01450 for (k=0;k<peaks_MFT.size();k++) {
01451 (*peaks)[k].frequency = f[k]/timestep;
01452 (*peaks)[k].amplitude = A[k];
01453 (*peaks)[k].phase = psi[k];
01454 }
01455 }
01456
01457 void FFT::Search_FMFT2() {
01458
01459 Search_FMFT_main();
01460
01461
01462 vector<Peak> peaks_MFT(*peaks);
01463
01464
01465 unsigned int size = data_stream.size();
01466 FFTDataStream backup_data_stream(data_stream);
01467
01468 unsigned int j,pk;
01469 fftw_complex z;
01470 double arg, arg_j;
01471 for (j=0;j<size;j++) {
01472
01473 arg_j = twopi*data_stream[j].time;
01474 z.re = z.im = 0;
01475 for (pk=0;pk<peaks->size();pk++) {
01476 arg = arg_j*(*peaks)[pk].frequency+(*peaks)[pk].phase;
01477 z.re += (*peaks)[pk].amplitude*cos(arg);
01478 z.im += (*peaks)[pk].amplitude*sin(arg);
01479 }
01480
01481
01482 data_stream[j].amplitude = norm(z);
01483 data_stream[j].phase = secure_atan2(z.im,z.re);
01484 }
01485
01486
01487 Search_FMFT_main();
01488 vector<Peak> peaks_FMFT1(*peaks);
01489
01490 unsigned int k;
01491 for (k=0;k<peaks_MFT.size();k++) {
01492 (*peaks)[k].frequency = peaks_MFT[k].frequency + (peaks_MFT[k].frequency - peaks_FMFT1[k].frequency);
01493 (*peaks)[k].amplitude = peaks_MFT[k].amplitude + (peaks_MFT[k].amplitude - peaks_FMFT1[k].amplitude);
01494 (*peaks)[k].phase = peaks_MFT[k].phase + (peaks_MFT[k].phase - peaks_FMFT1[k].phase );
01495 }
01496
01497 data_stream = backup_data_stream;
01498 }
01499
01500 void FFT::Search_FMFT_main() {
01501
01502
01503
01504 bool nearfreqflag;
01505
01506 const double timestep = data_stream.timestep;
01507
01508 const int size = data_stream.size();
01509
01510
01511
01512 fftw_complex *in, *work, *out;
01513 in = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01514 work = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01515 out = (fftw_complex*)malloc(size*sizeof(fftw_complex));
01516
01517 {
01518 int j;
01519 for (j=0;j<size;j++) {
01520 in[j].re = data_stream[j].amplitude * cos(data_stream[j].phase);
01521 in[j].im = data_stream[j].amplitude * sin(data_stream[j].phase);
01522 }
01523 }
01524
01525 if (0) {
01526
01527
01528 char filename[20];
01529 sprintf(filename,"dump_signal_2n.dat");
01530 cerr << "data dump on file " << filename << endl;
01531 FILE *fp = fopen(filename,"w");
01532 int r=1; while (pow(2.0,r)<=size) {r++;} r--;
01533 int fsize = (int)rint(pow(2.0,r));
01534 cerr << " --> 2^r = " << fsize << endl;
01535 if (fp!=0) {
01536 int l;
01537 for (l=0;l<fsize;l++) {
01538 fprintf(fp,"%i %g %g\n",l,in[l].re,in[l].im);
01539 }
01540 }
01541 fclose(fp);
01542 }
01543
01544 apply_window(work,in,size);
01545
01546
01547 fftw_plan plan;
01548 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
01549 fftw_one(plan, work, out);
01550 fftw_destroy_plan(plan);
01551
01552 double centerf = psd_max_again(out,size);
01553 double leftf = centerf-1.0/size;
01554 double rightf = centerf+1.0/size;
01555
01556 vector<double> f(nfreq), A(nfreq), psi(nfreq);
01557 vector<fftw_complex> phiR(nfreq), phiL(nfreq);
01558
01559 f[0] = accurate_peak(leftf,centerf,rightf,work,size);
01560
01561
01562
01563 amph(&A[0],&psi[0],&phiR[0],&phiL[0],f[0],work,size);
01564
01565
01566 {
01567 int j;
01568 for (j=0;j<size;j++) {
01569 in[j].re -= A[0]*cos(twopi*f[0]*j+psi[0]);
01570 in[j].im -= A[0]*sin(twopi*f[0]*j+psi[0]);
01571 }
01572 }
01573
01574
01575
01576 double Q[nfreq][nfreq], alpha[nfreq][nfreq];
01577 double B[nfreq];
01578
01579 Q[0][0] = 1;
01580 alpha[0][0] = 1;
01581
01582 double fac,xsum,ysum;
01583 unsigned int m,k,j;
01584 for(m=1;m<nfreq;m++) {
01585
01586
01587
01588 apply_window(work,in,size);
01589
01590
01591
01592
01593 fftw_plan plan;
01594 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
01595 fftw_one(plan, work, out);
01596 fftw_destroy_plan(plan);
01597
01598
01599
01600 centerf = psd_max_again(out,size);
01601 leftf = centerf-1.0/size;
01602 rightf = centerf+1.0/size;
01603
01604 f[m] = accurate_peak(leftf,centerf,rightf,work,size);
01605
01606
01607
01608
01609
01610
01611
01612
01613
01614
01615
01616 nearfreqflag=false;
01617 unsigned int fq;
01618 for (fq=0;fq<m;fq++) if( fabs(f[m]-f[fq]) <= 1.0e-12/size) nearfreqflag=true;
01619
01620 while (nearfreqflag) {
01621 f[m] = accurate_peak(leftf,centerf,rightf,work,size);
01622 amph(&A[m],&psi[m],&phiR[m],&phiL[m],f[m],work,size);
01623 printf(" ----- REMOVING DUPLICATED PEAK: : %.20g %.20g %.20g\n",f[m]/timestep,A[m],psi[m]);
01624 {
01625 int j;
01626 for (j=0;j<size;j++) {
01627 in[j].re -= A[m]*cos(twopi*f[m]*j+psi[m]);
01628 in[j].im -= A[m]*sin(twopi*f[m]*j+psi[m]);
01629 }
01630 }
01631 apply_window(work,in,size);
01632
01633 fftw_plan plan;
01634 plan = fftw_create_plan(size, FFTW_FORWARD, FFTW_ESTIMATE);
01635 fftw_one(plan, work, out);
01636 fftw_destroy_plan(plan);
01637 centerf = psd_max_again(out,size);
01638 leftf = centerf-1.0/size;
01639 rightf = centerf+1.0/size;
01640 f[m] = accurate_peak(leftf,centerf,rightf,work,size);
01641 nearfreqflag=false;
01642 unsigned int fq;
01643 for (fq=0;fq<m;fq++) if( fabs(f[m]-f[fq]) <= 1.0e-12/size) nearfreqflag=true;
01644 }
01645
01646
01647 amph(&A[m],&psi[m],&phiR[m],&phiL[m],f[m],work,size);
01648
01649
01650
01651
01652
01653
01654
01655
01656 Q[m][m] = 1;
01657 for(j=0;j<m;j++) {
01658 fac = (f[m]-f[j])*size/2.;
01659 fac *= twopi;
01660
01661
01662 if (fabs(fac) < 1.0e-12) {
01663 Q[m][j] = 1;
01664 } else if (fabs(fabs(fac)-pi)< 1.0e-12) {
01665 Q[m][j] = 0.5;
01666 } else {
01667 Q[m][j] = sin(fac)/fac * pisq / (pisq - fac*fac);
01668 }
01669
01670 Q[j][m] = Q[m][j];
01671 }
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683 for(k=0;k<m;k++){
01684 B[k] = 0;
01685 for(j=0;j<=k;j++)
01686 B[k] += -alpha[k][j]*Q[m][j];
01687 }
01688
01689
01690
01691
01692
01693
01694
01695
01696
01697 alpha[m][m] = 1.0;
01698 for(j=0;j<m;j++)
01699 alpha[m][m] -= (B[j]*B[j]);
01700 if (alpha[m][m]<0) {
01701 cerr << "WARNING!!!! Negative sqrt()!!!! " << alpha[m][m] << endl;
01702 }
01703
01704 alpha[m][m] = 1. / sqrt(alpha[m][m]);
01705
01706
01707 for(k=0;k<m;k++) {
01708 alpha[m][k] = 0;
01709
01710 for(j=k;j<m;j++)
01711 alpha[m][k] += B[j]*alpha[j][k];
01712
01713 alpha[m][k] = alpha[m][m]*alpha[m][k];
01714 }
01715
01716
01717
01718
01719
01720
01721
01722
01723
01724
01725
01726 {
01727 int i;
01728 for(i=0;i<size;i++) {
01729 xsum=0; ysum=0;
01730 for(j=0;j<=m;j++) {
01731 fac = twopi*(f[j]*i + (f[m]-f[j])*size/2.) + psi[m];
01732 xsum += alpha[m][j]*cos(fac);
01733 ysum += alpha[m][j]*sin(fac);
01734 }
01735 in[i].re -= alpha[m][m]*A[m]*xsum;
01736 in[i].im -= alpha[m][m]*A[m]*ysum;
01737 }
01738 }
01739 }
01740
01741
01742
01743
01744
01745
01746
01747
01748 for(k=0;k<nfreq;k++) {
01749 xsum=0; ysum=0;
01750 for(j=k;j<nfreq;j++) {
01751 fac = twopi*((f[j]-f[k])*size/2.) + psi[j];
01752 xsum += alpha[j][j]*alpha[j][k]*A[j]*cos(fac);
01753 ysum += alpha[j][j]*alpha[j][k]*A[j]*sin(fac);
01754 }
01755 A[k] = sqrt(xsum*xsum+ysum*ysum);
01756 psi[k] = secure_atan2(ysum,xsum);
01757 }
01758
01759
01760
01761
01762
01763
01764
01765
01766 peaks->resize(nfreq);
01767 Peak tp;
01768
01769 for(k=0;k<nfreq;k++) {
01770
01771 tp.frequency = f[k]/timestep;
01772 tp.amplitude = A[k];
01773 tp.phase = psi[k];
01774
01775 tp.phiR = phiR[k];
01776 tp.phiL = phiL[k];
01777
01778 (*peaks)[k] = tp;
01779 }
01780
01781 free(in); free(work); free(out);
01782
01783 }
01784
01785 }
01786