orsa_fft.cc

Go to the documentation of this file.
00001 /* 
00002    ORSA - Orbit Reconstruction, Simulation and Analysis
00003    Copyright (C) 2002-2004 Pasquale Tricarico
00004    
00005    This program is free software; you can redistribute it and/or
00006    modify it under the terms of the GNU General Public License
00007    as published by the Free Software Foundation; either version 2
00008    of the License, or (at your option) any later version.
00009    
00010    As a special exception, Pasquale Tricarico gives permission to
00011    link this program with Qt commercial edition, and distribute the
00012    resulting executable, without including the source code for the Qt
00013    commercial edition in the source distribution.
00014    
00015    This program is distributed in the hope that it will be useful,
00016    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018    GNU General Public License for more details.
00019    
00020    You should have received a copy of the GNU General Public License
00021    along with this program; if not, write to the Free Software
00022    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
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   // genuine phi, without windowing
00048   //! Discrete Fourier Transform
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     // OUT HERE!
00071     return result; 
00072   }
00073   
00074   
00075   // phi with Hanning windowing
00076   //! Discrete Fourier Transform with Hanning windowing
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     // OUT HERE!
00101     return result; 
00102   }
00103   
00104   
00105   //! amplitude for spectrum, without windowing
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   //! amplitude for spectrum, with Hanning windowing
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     // return  (-phi_Hanning_amp(x, p->pointer_points_sequence, p->size));
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     // return  (-phi_Hanning_amp(x, p->pointer_points_sequence, p->size));
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     // return  (-phi_amp(x, p->pointer_points_sequence, p->size));
00142     
00143   }
00144   
00145   
00146   typedef struct binamp {
00147     int     bin;
00148     double  amp;
00149   };
00150   
00151   
00152   //! sort binamp struct from the bigger to the smaller...
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) // size is even
00177       Nyquist = norm(transformed_signal[size/2])/size;  // Nyquist freq.
00178     
00179     // cerr << "...some test data: " << DC << "  " << psd_plus[0]  << "  " << psd_plus[1]  << "  " << psd_plus[2]  << "  " << psd_plus[3]  << "  " << psd_plus[4] << endl; 
00180     
00181     // DEBUG: DUMP DATA TO FILE
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     // psd.resize(size);
00200     //
00201     /* psd[0] = norm(transformed_signal[0])/size;
00202        for (k=1;k<(size-1)/2;k++)
00203        psd[k] = norm(transformed_signal[k])/size;
00204        for (k=(size/2)+1;k<size;k++)
00205        psd[k] = norm(transformed_signal[size-k])/size;
00206        if ( (size%2) == 0) // size is even
00207        psd[size/2] = norm(transformed_signal[size/2])/size;  // Nyquist freq.
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     // TODO: Nyquist...
00253     
00254     // cerr << " psd_max_again: returning " << ((double)bin/(double)size) << "   amp: " << maxpow << "        bin: " << bin << endl;
00255     
00256     // DEBUG: DUMP DATA TO FILE
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     /* else { 
00278        cerr << "\n        *\n        **\n        ***\n        ****\n*************\n**************  WARNING!!! peak don't found... returning (-1)!!\n*************\n        ****\n        ***\n        **\n        *\n" << endl;
00279        return (-1);
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) // size is even
00299       Nyquist = norm(transformed_signal[size/2])/size;  // Nyquist freq.
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         // candidate.push_back((double)(k+1)/(double)size);
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       // candidate.push_back(1.0/(double)size);
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         // candidate.push_back((double)-(k+1)/(double)size);
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       // candidate.push_back(-1.0/(double)size);
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       // candidate.push_back(0.0);
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   // rough estimate of the PSD max freq (the same as bracket...)
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) // size is even
00386       psd[size/2] = norm(transformed_signal[size/2])/size;  // Nyquist freq.
00387     
00388     /* cerr << " === DUMP PSD ===" << endl;
00389        for (k=0;psd.size();++k) {
00390        printf("PSD[%05i]=%g\n",k,psd[k]);
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     /* for (l=1;l<psd.size();l++) {
00416        if (psd[l] > maxamp) {
00417        maxamp = psd[l];
00418        bin    = l;
00419        }
00420        }
00421     */ 
00422     
00423     // DEBUG: DUMP DATA TO FILE
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     // cerr << " psd_max: returning " << ((double)bin/(double)size) << "   amp: " << maxamp << "        bin: " << bin << "  found: " << found << endl;
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     // cerr << "amph() DUMP:   amp = " << *amp << "  phase = " << *phase << endl;
00476     
00477   }
00478   
00479   double accurate_peak(double left, double center, double right, fftw_complex *in, int size) {
00480     
00481     // check values 
00482     // if ( (center<0) || (center>0.5) ) return center; // ERROR...
00483     if ( (center<(-0.5)) || (center>0.5) ) { 
00484       cerr << "warning!! Peak out of range!" << endl;
00485       return center; // ERROR...
00486     }
00487     
00488     // int    c_iter=0, l_iter=0, r_iter=0, max_iter = 100;
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     // TODO: check for gsl peak search conditions!!!
00497     
00498     // gsl stuff
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   = &par;
00512     //
00513     gsl_min_fminimizer_set(s,&F,center,left,right);
00514     
00515     double epsrel = 1.0e-10; // for the d=1 minimization
00516     double epsabs = 0.0;     // for the d=1 minimization
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       // delta     = range_stop - range_start;
00529       // minimize!
00530       status = gsl_min_test_interval(left,right,epsrel,epsabs);
00531       
00532       // post-minimization...
00533       /* printf("%5d [%f, %f] %.7f %.7e  bin:%i\n", 
00534          iter,range_start,range_stop,test_peak.frequency,range_stop-range_start,bin);
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     // freq_start = 0.0;
00563     // freq_stop  = 100.0;
00564     
00565     // gsl stuff
00566     T = gsl_min_fminimizer_goldensection;  
00567     s = gsl_min_fminimizer_alloc (T);
00568     
00569     // defaults
00570     // search_type = HK;
00571     // window_type = NONE;
00572     // algorithm_type = FourierTransformPeaks;
00573     
00574     nfreq = 4;
00575     
00576   }
00577   
00578   
00579   void FFT::FillDataStream(FFTSearch s) {
00580     
00581     // cerr << "Filling data stream..." << endl;
00582     
00583     if (s == HK) {
00584       
00585       // FFTDataStream  &fds  = *data_stream;
00586       OrbitStream    &ostr = *os;
00587       
00588       data_stream.timestep = ostr.timestep;
00589       // cerr << "data_stream timestep: " << data_stream.timestep << endl;
00590       
00591       // please, handle with care the reserve() method!
00592       // first time allocation...
00593       // this reserve call seems to be buggy, with segfaults...
00594       // if (data_stream.size() == 0) data_stream.reserve(ostr.size());
00595       // clean vector...
00596       data_stream.resize(0);
00597       
00598       FFTDataStructure tmp_ds;
00599       
00600       // cerr << "size to fill: " << ostr.size() << endl;
00601       
00602       unsigned int k;      
00603       for (k=0;k<ostr.size();k++) {
00604         
00605         // cerr << "filling: " << k << endl;
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       // was s == HK
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         // cerr << "filling: " << k << endl;
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         // cerr << "filling: " << k << endl;
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         // cerr << "filling: " << k << endl;
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         // cerr << "filling: " << k << endl;
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         // cerr << "filling: " << k << endl;
00706         
00707         tmp_ds.time      = ostr[k].epoch.Time();
00708         tmp_ds.amplitude = 1.0; // dummy value
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         // cerr << "filling: " << k << endl;
00725         
00726         tmp_ds.time      = ostr[k].epoch.Time();
00727         tmp_ds.amplitude = ostr[k].a; // dummy value
00728         tmp_ds.phase     = ostr[k].M;
00729         
00730         data_stream.push_back(tmp_ds);
00731       }
00732     }
00733     /* 
00734        } else if (s == TESTING) {
00735        
00736        OrbitStream    &ostr = *os;
00737        data_stream.timestep = ostr.timestep;
00738        data_stream.resize(0);
00739        
00740        FFTDataStructure tmp_ds;
00741        
00742        unsigned int k;      
00743        for (k=0;k<ostr.size();k++) {
00744        
00745        // cerr << "filling: " << k << endl;
00746        
00747        tmp_ds.time      = ostr[k].epoch.Time();
00748        // tmp_ds.amplitude = ostr[k].a; // externally modified to be ostr[k].a = \sqrt{\frac{a-mean(a_J)}{a_J}}
00749        tmp_ds.amplitude = ostr[k].a; // externally modified to be ostr[k].a = $\sqrt{\frac{a}{a_J}}$
00750        tmp_ds.phase     = ostr[k].libration_angle;;
00751        
00752        data_stream.push_back(tmp_ds);
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     // cerr << "===>> Computing Power Spectrum..." << endl;
00832     
00833     unsigned int j,k;
00834     unsigned int size = data_stream.size();
00835     // cerr << "data_stream.size(): " << data_stream.size() << endl;
00836     
00837     // fftw_complex in[size], work[size], out[size];
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     // fftw_one(plan, in, out);
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) // size is even
00865       Nyquist = norm(out[size/2])/size;  // Nyquist freq.
00866     
00867     if (HiResSpectrum) {
00868       const int resample = 5; // 1
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; // physical units. i.e. [y^{-1}]
00874       const double freq_stop  =  0.0e-5; // physical units
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         // fprintf(fp,"%i  %g\n",-(l+1),psd_minus[l]);
00891         tps.frequency = ((double)-(l+1)/(double)size)/timestep;
00892         tps.power     = psd_minus[l];
00893         FFTps.push_back(tps);
00894       }
00895       // fprintf(fp,"%i  %g\n",0,DC);
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           // fprintf(fp,"%i  %g\n", (l+1),psd_plus[l]);
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         // fprintf(fp,"%i  %g\n",size/2,Nyquist);
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       // cerr << "reco time: " << (*reconstructed_data_stream)[j].time << endl;
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     // cerr << "reco times: " <<  (*reconstructed_data_stream)[0].time << " to   " << (*reconstructed_data_stream)[size-1].time << endl;
00941   }
00942   
00943   void FFT::Search_FFT() {
00944     
00945     // FillDataStream(se);
00946     
00947     unsigned int size = data_stream.size();
00948     
00949     vector<Peak> &pks = *peaks;
00950     
00951     fftw_plan plan;
00952     
00953     // fftw_complex in[size], out[size];
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) // size is even
00976       psd[size/2] = norm(out[size/2])/size;  // Nyquist freq.
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         // Hanning or flat (NONE) windowing...
00990         // tps.power = phi_amp(((double)l/(double)(size*resample)),in,size);
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     // DEBUG
01012     /* for (l=0;l<psd.size();l++) {
01013        cerr << (*fps)[l].frequency << "  " << (*fps)[l].power << endl;
01014        }
01015     */
01016     
01017     // peaks reset
01018     pks.resize(0);
01019     /* pks.resize(number_of_peaks);
01020        for (k=0;k<number_of_peaks;k++)
01021        pks[k].Set(default_peak_reset_frequency,
01022        default_peak_reset_amplitude,
01023        default_peak_reset_phase); 
01024     */
01025     
01026     // cerr << "Pass (4)" << endl;
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     // cerr << "sizeof(binamp): " << sizeof(binamp) << endl;
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     // include bin zero! (DC)
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     // } while (relamp >= relative_amplitude);
01062     //
01063     free(ptr_binamp);
01064     
01065     
01066     // cerr << "number of candidate peaks: " << candidate_bin.size() << endl;
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       // if (w == NONE) {
01076       test_peak.amplitude = phi_amp(test_peak.frequency,&in[0],size); 
01077       /* } else if (w == HANNING) {
01078          test_peak.amplitude = phi_Hanning_amp(test_peak.frequency,&in[0],size); 
01079          } else {
01080          cerr << "Invalid Windowing --> EXIT" << endl;
01081          exit(0);
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             // phi_tmp = phi_amp(range_start,&in[0],size);
01097             // if (w == NONE) {
01098               phi_tmp = phi_amp(range_start,&in[0],size); 
01099               /* } else if (w == HANNING) {
01100                  phi_tmp = phi_Hanning_amp(range_start,&in[0],size); 
01101                  } else {
01102                  cerr << "Invalid Windowing --> EXIT" << endl;
01103                  exit(0);
01104                  }
01105               */
01106           } while ( (range_start >= 0) && 
01107                     (ip < 100) && 
01108                     (phi_tmp >= test_peak.amplitude) );
01109           // cerr << "range_start iterations: " << ip << endl;
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               // phi_tmp = phi_amp(range_stop,&in[0],size);
01119               // if (w == NONE) {
01120                 phi_tmp = phi_amp(range_stop,&in[0],size); 
01121                 /* } else if (w == HANNING) {
01122                    phi_tmp = phi_Hanning_amp(range_stop,&in[0],size); 
01123                    } else {
01124                    cerr << "Invalid Windowing --> EXIT" << endl;
01125                    exit(0);
01126                    }
01127                 */
01128                 
01129             } while ( (range_stop <= 0.5) && 
01130                       (ip < 100) && 
01131                       (phi_tmp >= test_peak.amplitude) );
01132             // cerr << "range_stop  iterations: " << ip << endl;
01133             if ( (ip >= 100) || 
01134                  (range_stop > 0.5) ) {
01135               genuine_peak = false;  
01136             }
01137           }  
01138         }
01139       }
01140       
01141       // get the right normalizzation
01142       if (bin == 0) test_peak.amplitude /= sqrt(2.0);
01143       
01144       // look for maximums
01145       if (genuine_peak) {
01146         
01147         if (bin != 0) {
01148           
01149           // gsl stuff
01150           par.size = size;
01151           par.pointer_points_sequence = &in[0];
01152           //
01153           
01154           // F.function = &phi_gsl;
01155           // if (w == NONE) {
01156           F.function = &phi_gsl;
01157           /* } else if (w == HANNING) {
01158              F.function = &phi_Hanning_gsl;
01159              } else {
01160              cerr << "Invalid Windowing --> EXIT" << endl;
01161              exit(0);
01162              }
01163           */
01164           
01165           F.params   = &par;
01166           
01167           gsl_min_fminimizer_set(s,&F,test_peak.frequency,range_start,range_stop);
01168           
01169           double epsrel = 1.0e-9; // for the d=1 minimization
01170           double epsabs = 0.0;    // for the d=1 minimization
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             // delta     = range_stop - range_start;
01184             // minimize!
01185             status = gsl_min_test_interval (range_start, range_stop, epsrel, epsabs);
01186             
01187             // post-minimization...
01188             /* printf("%5d [%f, %f] %.7f %.7e  bin:%i\n", 
01189                iter,range_start,range_stop,test_peak.frequency,range_stop-range_start,bin);
01190             */
01191           } while ((status == GSL_CONTINUE) && (iter < max_iter));
01192           
01193         }
01194         
01195         // test_peak.amplitude = phi_amp(test_peak.frequency,&in[0],size); 
01196         // if (w == NONE) {
01197         test_peak.amplitude = phi_amp(test_peak.frequency,&in[0],size); 
01198         /* } else if (w == HANNING) {
01199            test_peak.amplitude = phi_Hanning_amp(test_peak.frequency,&in[0],size); 
01200            } else {
01201            cerr << "Invalid Windowing --> EXIT" << endl;
01202            exit(0);
01203            }
01204         */
01205         
01206         if (bin == 0) test_peak.amplitude /= sqrt(2.0);
01207         
01208         // check if this peak is very close 
01209         // to the existing one already in the list
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         // quicker, but not sorted...
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           // cerr << "Added peak!" << endl;
01231         }
01232         
01233       }
01234       
01235     }
01236     
01237     // before than return
01238     // rescale freqs.
01239     for (j=0;j<pks.size();j++) {
01240       pks[j].frequency /= timestep;
01241     }
01242     
01243     // not here, probably in the destructor...
01244     // gsl_min_fminimizer_free(s); 
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     // FFT
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     /* unsigned int l;
01276        for (l=0;l<candidate_freq.size();l++) {
01277        cerr << "candidate freq: " << candidate_freq[l]/timestep << endl;
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       /* COMPUTE AMPLITUDE AND PHASE */
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   // Eq. (34)
01319   double dQ(double y) {
01320     
01321     // y==0
01322     if (fabs(y) < 1.0e-12) return (0.0);
01323     
01324     // y==PI
01325     if (fabs(y-pi) < 1.0e-12) return (-3.0/(4*pi));
01326     
01327     // y==-PI
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     // save the peaks found
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     // EQ. (35)
01349     double ddQ_zero = 2.0/pisq - 1.0/3.0;
01350     
01351     double eps; // correction
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         // y_sj = (peaks_MFT[s].frequency - peaks_MFT[j].frequency)*timestep;
01361         // cerr << "y_sj: " << y_sj << endl;
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       // eps /= peaks_MFT[j].amplitude*ddQ_zero*(size/2.0);
01366       // eps /= peaks_MFT[j].amplitude*ddQ_zero*timestep;
01367       //
01368       epsilon[j] = eps;
01369       printf("epsilon[%i]: %.20g\n",j,eps);
01370     }
01371     
01372     // correction for amplitde and phase
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         /* EQUATION (3) in Sidlichovsky and Nesvorny (1997) */
01394         Q[m][m] = 1;
01395         for(j=0;j<m;j++) {
01396           fac = (f[m]-f[j])*size/2.;
01397           fac *= twopi;
01398           // cerr << " ...TEST NEAR PI: " << fmod(fac,pi) << "   " << fac << endl;
01399           // fix, if fac==0
01400           if (fabs(fac) < 1.0e-12) { // 0
01401             Q[m][j] = 1;
01402           } else if (fabs(fabs(fac)-pi)< 1.0e-12) { // +/- PI
01403             Q[m][j] = 0.5; // 1/2
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         /* EQUATION (17) */
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         /* EQUATION (18) */
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         /* EQUATION (19) */
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       /* EQUATION (26) */
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     // save the peaks found
01462     vector<Peak> peaks_MFT(*peaks);
01463     
01464     /* GENERATE THE QUASIPERIODIC FUNCTION COMPUTED BY MFT */
01465     unsigned int size = data_stream.size();
01466     FFTDataStream backup_data_stream(data_stream);
01467     // reconstructed_data_stream->resize(size);
01468     unsigned int j,pk;
01469     fftw_complex z;
01470     double arg, arg_j;
01471     for (j=0;j<size;j++) {
01472       // (*reconstructed_data_stream)[j].time = data_stream[j].time;
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       // (*reconstructed_data_stream)[j].amplitude = norm(z);
01481       // (*reconstructed_data_stream)[j].phase     = secure_atan2(z.im,z.re);
01482       data_stream[j].amplitude = norm(z);
01483       data_stream[j].phase     = secure_atan2(z.im,z.re);
01484     }
01485     
01486     // cerr << "Second step..." << endl;
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     // nfreq = 12;
01503     
01504     bool nearfreqflag;
01505     
01506     const double timestep = data_stream.timestep;
01507     
01508     const int size = data_stream.size();
01509     // cerr << "FFT::Search_FMFT_main  size = " << size << endl;
01510     
01511     // fftw_complex in[size], work[size], out[size];    
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       // DUMP ON FILE
01527       // int filenum=0;
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     // FFT
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     // f[0] = golden(phi_amp,leftf,centerf,rightf,work,size);
01561     
01562     /* COMPUTE AMPLITUDE AND PHASE */
01563     amph(&A[0],&psi[0],&phiR[0],&phiL[0],f[0],work,size);
01564     
01565     /* SUBSTRACT THE FIRST HARMONIC FROM THE SIGNAL */ 
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     /* HERE STARTS THE MAIN LOOP  *************************************/ 
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       // cerr << "test in: " << in[0].re << "  " << in[0].im << "  " << in[1].re << "  " << in[1].im << "  " << endl;
01587       
01588       apply_window(work,in,size);
01589       
01590       // cerr << "test work: " << work[0].re << "  " << work[0].im << "  " << work[1].re << "  " << work[1].im << "  " << endl;
01591       
01592       // FFT
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       // cerr << "test out: " << out[0].re << "  " << out[0].im << "  " << out[1].re << "  " << out[1].im << "  " << endl;
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       // f[m] = golden(phi_amp,leftf,centerf,rightf,work,size);
01606       // if (f[m]==-1) break;
01607       
01608       /* cerr << " Peaks found up to now..." << endl;
01609          for(k=0;k<=m;k++) {
01610          printf("f[%i]: %.20g\n",k,f[k]/timestep);
01611          }
01612       */
01613       
01614       /* CHECK WHETHER THE NEW FREQUENCY IS NOT TOO CLOSE TO ANY PREVIOUSLY
01615          DETERMINED ONE */
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         // FFT
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       /* COMPUTE AMPLITUDE AND PHASE */
01647       amph(&A[m],&psi[m],&phiR[m],&phiL[m],f[m],work,size);
01648       
01649       /* cerr << " Peaks found up to now..." << endl;
01650          for(k=0;k<=m;k++) {
01651          printf("f[%i]: %.20g  %.20g  %.20g\n",k,f[k]/timestep,A[k],psi[k]);
01652          }
01653       */
01654       
01655       /* EQUATION (3) in Sidlichovsky and Nesvorny (1997) */
01656       Q[m][m] = 1;
01657       for(j=0;j<m;j++) {
01658         fac = (f[m]-f[j])*size/2.;
01659         fac *= twopi;
01660         // cerr << " ...TEST NEAR PI: " << fmod(fac,pi) << "   " << fac << endl;
01661         // fix, if fac==0
01662         if (fabs(fac) < 1.0e-12) { // 0
01663           Q[m][j] = 1;
01664         } else if (fabs(fabs(fac)-pi)< 1.0e-12) { // +/- PI
01665           Q[m][j] = 0.5; // 1/2
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       // Q DUMP
01674       /* printf("Q dump...   m=%i\n",m);
01675          for (k=0;k<=m;k++) {
01676          for (j=0;j<=k;j++) {
01677          printf("Q[%i][%i]: %.20g\n",k,j,Q[k][j]);
01678          }
01679          }
01680       */
01681       
01682       /* EQUATION (17) */
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       // B DUMP
01690       /* printf("B dump...   m=%i\n",m);
01691          for (k=0;k<m;k++) {
01692          printf("B[%i]: %.20g\n",k,B[k]);
01693          }
01694       */
01695       
01696       /* EQUATION (18) */
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       // cerr << "sqrt of: " << alpha[m][m] << endl;
01704       alpha[m][m] = 1. / sqrt(alpha[m][m]);
01705       
01706       /* EQUATION (19) */
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       // alpha DUMP
01717       /* printf("alpha dump...   m=%i\n",m);
01718          for (k=0;k<=m;k++) {
01719          for (j=0;j<=k;j++) {
01720          printf("alpha[%i][%i]: %.20g\n",k,j,alpha[k][j]);
01721          }
01722          }
01723       */
01724       
01725       /* EQUATION (22) */
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     /* cerr << " Peaks found up to now..." << endl;
01742        for(k=0;k<nfreq;k++) {
01743        printf("f[%i]: %.20g  %.20g  %.20g\n",k,f[k]/timestep,A[k],psi[k]);
01744        }
01745     */
01746     
01747     /* EQUATION (26) */
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        cerr << " Peaks at last..." << endl;
01761        for(k=0;k<nfreq;k++) {
01762        printf("k=%i   f: %g   A: %g   psi: %g\n",k,f[k]/timestep,A[k],psi[k]);
01763        }
01764     */
01765     
01766     peaks->resize(nfreq);
01767     Peak tp;
01768     // cerr << "timestep: " << timestep << endl;
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 } // namespace orsa
01786   

Generated on Wed May 30 13:04:52 2007 for liborsa by  doxygen 1.5.2