freqestleastsquare.cpp
1 /***************************************************************************
2  Copyright (C) 2002-2015 Kentaro Kitagawa
3  kitagawa@phys.s.u-tokyo.ac.jp
4 
5  This program is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Library General Public
7  License as published by the Free Software Foundation; either
8  version 2 of the License, or (at your option) any later version.
9 
10  You should have received a copy of the GNU Library General
11  Public License and a list of authors along with this program;
12  see the files COPYING and AUTHORS.
13 ***************************************************************************/
14 #include "freqestleastsquare.h"
15 
16 void
17 FreqEstLeastSquare::genSpectrum(const std::vector<std::complex<double> >& memin,
18  std::vector<std::complex<double> >& memout,
19  int t0, double tol, FFT::twindowfunc windowfunc, double windowlength) {
20  int t = memin.size();
21  int n = memout.size();
22  int t0a = t0;
23  if(t0a < 0)
24  t0a += (-t0a / n + 1) * n;
25 
26  //Fitting with weights.
27  std::vector<double> weight;
28  window(t, t0, windowfunc, windowlength, weight);
29  double wsum = 0.0, w2sum = 0.0;
30  for(int i = 0; i < t; i++) {
31  wsum += weight[i];
32  w2sum += weight[i] * weight[i];
33  }
34  int wpoints = wsum*wsum/w2sum; //# of fittable data in time domain.
35  wpoints = lrint(wpoints * numberOfNoises(memin) / (double)t); //# of fittable data in freq. domain.
36  wpoints = std::min(std::max(wpoints, t/100 + 1), t);
37 // fprintf(stderr, "# of data points = %d\n", wpoints);
38 
39  //Standard error.
40  double sigma2 = 0.0;
41  for(int i = 0; i < t; i++) {
42  sigma2 += std::norm(memin[i]) * weight[i];
43  }
44  sigma2 /= wsum;
45 
46  // Peak search by ZF-FFT;
47  std::fill(m_ifft.begin(), m_ifft.end(), std::complex<double>(0.0));
48  std::vector<std::complex<double> > convwnd(m_ifft);
49  for(int i = 0; i < t; i++) {
50  m_ifft[(t0a + i) % n] = memin[i] * weight[i];
51  }
52  m_fftN->exec(m_ifft, memout);
53  for(int i = 0; i < n; i++) {
54  memout[i] /= wsum;
55  }
56 
57  std::vector<std::complex<double> > wave(memin);
58  std::deque<std::complex<double> > zlist;
59 
60  double ic = 1e99;
61  for(int lp = 0; lp < 32; lp++) {
62  int npeaks = m_peaks.size();
63  double loglikelifood = -wpoints * (log(2*M_PI) + 1.0 + log(sigma2));
64  double ic_new = m_funcIC(loglikelifood, npeaks * 3.0, wpoints * 2.0);
65  if(ic_new > ic) {
66  if(m_peaks.size()) {
67  m_peaks.pop_back();
68  zlist.pop_back();
69  }
70  break;
71  }
72  ic = ic_new;
73 
74  double freq = 0.0;
75  std::complex<double> z(0.0);
76  double normz = 0;
77  for(int i = 0; i < n; i++) {
78  if(normz < std::norm(memout[i])) {
79  freq = i;
80  z = memout[i];
81  normz = std::norm(z);
82  }
83  }
84  freq *= t / (double)n;
85 
86  //Prepare exp(-omega t)
87  std::vector<std::complex<double> > coeff(t);
88  double p = -2.0 * M_PI / (double)t * freq;
89  for(int i = 0; i < t;) {
90  std::complex<double> x = std::polar(1.0, (i + t0) * p);
91  std::complex<double> y = std::polar(1.0, p);
92  for(int j = 0; (j < 1024) && (i < t); j++) {
93  coeff[i] = x;
94  x *= y;
95  i++;
96  }
97  }
98  //Standard error.
99  sigma2 = 0.0;
100  for(int i = 0; i < t; i++) {
101  sigma2 += std::norm(wave[i] - z * std::conj(coeff[i])) * weight[i];
102  }
103  sigma2 /= wsum;
104 
105 // fprintf(stderr, "NPeak = %d, sigma2 = %g, freq = %g, z = %g, ph = %g, ic = %g\n",
106 // npeaks, sigma2, freq, std::abs(z), std::arg(z), ic);
107 
108  // Newton's method for non-linear least square fit.
109  for(int it = 0; it < 10; it++) {
110  // Derivertive.
111  double ds2df = 0.0;
112  double ds2dx = 0.0;
113  double ds2dy = 0.0;
114  // Jacobian.
115  double d2s2df2 = 0.0;
116 // double d2s2dx2 = 1.0;
117 // double d2s2dy2 = 1.0;
118  double d2s2dfx = 0.0;
119  double d2s2dfy = 0.0;
120 // double d2s2dxy = 0.0;
121  double kstep = 2.0 * M_PI / (double)t;
122  double k = kstep * t0;
123  for(int i = 0; i < t; i++) {
124  k += kstep;
125  std::complex<double> yew = wave[i] * coeff[i] * weight[i];
126  std::complex<double> yewzk = std::conj(z) * yew * k;
127  ds2df -= std::imag(yewzk);
128  std::complex<double> a = -yew + z * weight[i];
129  ds2dx += std::real(a);
130  ds2dy += std::imag(a);
131  d2s2df2 += std::real(yewzk * k);
132  a = yew * k;
133  d2s2dfx -= std::imag(a);
134  d2s2dfy += std::real(a);
135  }
136  ds2df /= wsum;
137  ds2dx /= wsum;
138  ds2dy /= wsum;
139  d2s2df2 /= wsum;
140  d2s2dfx /= wsum;
141  d2s2dfy /= wsum;
142  double detJ = d2s2df2 - d2s2dfx*d2s2dfx - d2s2dfy*d2s2dfy;
143  double df = -ds2df + ds2dx * d2s2dfx + ds2dy * d2s2dfy;
144  df /= detJ;
145  double dx = ds2df * d2s2dfx - ds2dx * (d2s2df2 - d2s2dfy*d2s2dfy) - ds2dy * d2s2dfx*d2s2dfy;
146  dx /= detJ;
147  double dy = ds2df * d2s2dfy - ds2dx * d2s2dfx*d2s2dfy - ds2dy * (d2s2df2 - d2s2dfx*d2s2dfx);
148  dy /= detJ;
149 
150 // fprintf(stderr, "Ds2 = %g,%g,%g\nJ=[%g,%g,%g;%g,1,0;%g,0,1]\nDx=%g,%g,%g\n",
151 // ds2df,ds2dx,ds2dy,d2s2df2,d2s2dfx,d2s2dfy,d2s2dfx,d2s2dfy,df,dx,dy);
152 
153  double sor = 1.0;
154  sor = std::min(sor, 0.4 / fabs(df));
155  sor = std::min(sor, 0.2*sqrt(std::norm(z)/fabs(dx*dx+dy*dy)));
156  df *= sor;
157  dx *= sor;
158  dy *= sor;
159  freq += df;
160  z += std::complex<double>(dx, dy);
161  //Prepare exp(-omega t)
162  double p = -2.0 * M_PI / (double)t * freq;
163  for(int i = 0; i < t;) {
164  std::complex<double> x = std::polar(1.0, (i + t0) * p);
165  std::complex<double> y = std::polar(1.0, p);
166  for(int j = 0; (j < 1024) && (i < t); j++) {
167  coeff[i] = x;
168  x *= y;
169  i++;
170  }
171  }
172 
173 // fprintf(stderr, "It = %d, freq = %g, z = %g, ph = %g\n",
174 // it, freq, std::abs(z), std::arg(z));
175 
176  if((df < 0.001) && (dx*dx+dy*dy < tol*tol*std::norm(z))) {
177  break;
178  }
179  }
180 
181  //Standard error.
182  double sigma2 = 0.0;
183  for(int i = 0; i < t; i++) {
184  sigma2 += std::norm(wave[i] - z * std::conj(coeff[i])) * weight[i];
185  }
186  sigma2 /= wsum;
187 
188  m_peaks.push_back(std::pair<double, double>(std::abs(z) * n, freq / t * n));
189  zlist.push_back(z);
190  //Subtract the wave.
191  for(int i = 0; i < t; i++) {
192  wave[i] -= z * std::conj(coeff[i]);
193  }
194 
195  // Recalculate ZF-FFT.
196  std::fill(m_ifft.begin(), m_ifft.end(), std::complex<double>(0.0));
197  for(int i = 0; i < t; i++) {
198  m_ifft[(t0a + i) % n] = wave[i] * weight[i];
199  }
200  m_fftN->exec(m_ifft, memout);
201  for(int i = 0; i < n; i++) {
202  memout[i] /= wsum;
203  }
204  }
205  std::fill(m_ifft.begin(), m_ifft.end(), std::complex<double>(0.0));
206  for(int i = 0; i < m_peaks.size(); i++) {
207  double freq = m_peaks[i].second;
208  std::complex<double> z(zlist[i]);
209  double p = 2.0 * M_PI / (double)n * freq;
210  for(int i = 0; i < n;) {
211  std::complex<double> x = std::polar(1.0, (i + n/2 - n) * p);
212  std::complex<double> y = std::polar(1.0, p);
213  x *= z;
214  for(int j = 0; (j < 1024) && (i < n); j++) {
215  m_ifft[(i + n/2) % n] += x;
216  x *= y;
217  i++;
218  }
219  }
220  }
221  m_fftN->exec(m_ifft, memout);
222 }
Generated for KAME4 by  doxygen 1.8.3