xue@11
|
1 /*
|
xue@11
|
2 Harmonic sinusoidal modelling and tools
|
xue@11
|
3
|
xue@11
|
4 C++ code package for harmonic sinusoidal modelling and relevant signal processing.
|
xue@11
|
5 Centre for Digital Music, Queen Mary, University of London.
|
xue@11
|
6 This file copyright 2011 Wen Xue.
|
xue@11
|
7
|
xue@11
|
8 This program is free software; you can redistribute it and/or
|
xue@11
|
9 modify it under the terms of the GNU General Public License as
|
xue@11
|
10 published by the Free Software Foundation; either version 2 of the
|
xue@11
|
11 License, or (at your option) any later version.
|
xue@11
|
12 */
|
xue@1
|
13 //---------------------------------------------------------------------------
|
xue@1
|
14
|
xue@1
|
15 #include <stddef.h>
|
Chris@2
|
16 #include "sinest.h"
|
xue@1
|
17 #include "fft.h"
|
xue@1
|
18 #include "opt.h"
|
Chris@2
|
19 #include "sinsyn.h"
|
xue@1
|
20 #include "splines.h"
|
Chris@2
|
21 #include "windowfunctions.h"
|
xue@1
|
22
|
Chris@5
|
23 /** \file sinest.h */
|
Chris@5
|
24
|
xue@1
|
25 //---------------------------------------------------------------------------
|
Chris@5
|
26 /**
|
xue@1
|
27 function dsincd_unn: derivative of unnormalized discrete sinc function
|
xue@1
|
28
|
xue@1
|
29 In: x, scale N
|
xue@1
|
30
|
xue@1
|
31 Returns the derivative of sincd_unn(x, N)
|
xue@1
|
32 */
|
xue@1
|
33 double dsincd_unn(double x, int N)
|
xue@1
|
34 {
|
xue@1
|
35 double r=0;
|
xue@1
|
36 double omg=M_PI*x;
|
xue@1
|
37 double domg=omg/N;
|
xue@1
|
38 if (fabs(x)>1e-6)
|
xue@1
|
39 {
|
xue@1
|
40 r=M_PI*(cos(omg)-sin(omg)*cos(domg)/sin(domg)/N)/sin(domg);
|
xue@1
|
41 }
|
xue@1
|
42 else
|
xue@1
|
43 {
|
xue@1
|
44 if (domg!=0)
|
xue@1
|
45 {
|
xue@1
|
46 double sindomg=sin(domg);
|
xue@1
|
47 r=-omg*omg*omg*(1-1.0/(1.0*N*N))/3*M_PI/N/sindomg/sindomg;
|
xue@1
|
48 }
|
xue@1
|
49 else
|
xue@1
|
50 r=0;
|
xue@1
|
51 }
|
xue@1
|
52 return r;
|
xue@1
|
53 }//dsincd_unn
|
xue@1
|
54
|
Chris@5
|
55 /**
|
xue@1
|
56 function ddsincd_unn: 2nd-order derivative of unnormalized discrete sinc function
|
xue@1
|
57
|
xue@1
|
58 In: x, scale (equivalently, window size) N
|
xue@1
|
59
|
xue@1
|
60 Returns the 2nd-order derivative of sincd_unn(x, N)
|
xue@1
|
61 */
|
xue@1
|
62 double ddsincd_unn(double x, int N)
|
xue@1
|
63 {
|
xue@1
|
64 double r=0;
|
xue@1
|
65 double omg=M_PI*x;
|
xue@1
|
66 double domg=omg/N;
|
xue@1
|
67 double PI2=M_PI*M_PI;
|
xue@1
|
68 double NN=1.0/N/N-1;
|
xue@1
|
69 if (domg==0)
|
xue@1
|
70 {
|
xue@1
|
71 r=PI2*N*NN/3;
|
xue@1
|
72 }
|
xue@1
|
73 else
|
xue@1
|
74 {
|
xue@1
|
75 if (fabs(x)>1e-5)
|
xue@1
|
76 {
|
xue@1
|
77 r=sin(domg)*cos(omg)-sin(omg)*cos(domg)/N;
|
xue@1
|
78 }
|
xue@1
|
79 else
|
xue@1
|
80 {
|
xue@1
|
81 r=omg*omg*omg/N*NN/3;
|
xue@1
|
82 }
|
xue@1
|
83 double ss=sin(omg)*NN;
|
xue@1
|
84 r=-2.0/N*cos(domg)*r/sin(domg)/sin(domg)+ss;
|
xue@1
|
85 r=r*PI2/sin(domg);
|
xue@1
|
86 }
|
xue@1
|
87 return r;
|
xue@1
|
88 }//ddsincd_unn
|
xue@1
|
89
|
xue@1
|
90 //---------------------------------------------------------------------------
|
Chris@5
|
91 /**
|
xue@1
|
92 function Window: calculates the cosine-family-windowed spectrum of a complex sinusoid on [0:N-1] at
|
xue@1
|
93 frequency f bins with zero central phase.
|
xue@1
|
94
|
xue@1
|
95 In: f: frequency, in bins
|
xue@1
|
96 N: window size
|
xue@1
|
97 M, c[]: cosine-family window decomposition coefficients
|
xue@1
|
98 Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2.
|
xue@1
|
99
|
xue@1
|
100 Returns pointer to x. x is created anew if x=0 is specified on start.
|
xue@1
|
101 */
|
xue@1
|
102 cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2)
|
xue@1
|
103 {
|
xue@1
|
104 if (K1<0) K1=0;
|
xue@1
|
105 if (K2>N/2-1) K2=N/2-1;
|
xue@1
|
106
|
xue@1
|
107 if (!x) x=new cdouble[K2-K1+1];
|
xue@1
|
108 memset(x, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
109
|
xue@1
|
110 for (int l=K1-M; l<=K2+M; l++)
|
xue@1
|
111 {
|
xue@1
|
112 double ang=(f-l)*M_PI;
|
xue@1
|
113 double omg=ang/N;
|
xue@1
|
114 long double si, co, sinn=sin(ang);
|
xue@1
|
115 si=sin(omg), co=cos(omg);
|
xue@1
|
116 double sa=(ang==0)?N:(sinn/si);
|
xue@1
|
117 double saco=sa*co;
|
xue@1
|
118
|
xue@1
|
119 int k1=l-M, k2=l+M;
|
xue@1
|
120 if (k1<K1) k1=K1;
|
xue@1
|
121 if (k2>K2) k2=K2;
|
xue@1
|
122
|
xue@1
|
123 for (int k=k1; k<=k2; k++)
|
xue@1
|
124 {
|
xue@1
|
125 int m=k-l, kt=k-K1;
|
xue@1
|
126 if (m<0) m=-m;
|
xue@1
|
127 if (k%2)
|
xue@1
|
128 {
|
xue@1
|
129 x[kt].x-=c[m]*saco;
|
xue@1
|
130 x[kt].y+=c[m]*sinn;
|
xue@1
|
131 }
|
xue@1
|
132 else
|
xue@1
|
133 {
|
xue@1
|
134 x[kt].x+=c[m]*saco;
|
xue@1
|
135 x[kt].y-=c[m]*sinn;
|
xue@1
|
136 }
|
xue@1
|
137 }
|
xue@1
|
138 }
|
xue@1
|
139 return x;
|
xue@1
|
140 }//Window
|
xue@1
|
141
|
Chris@5
|
142 /**
|
xue@1
|
143 function dWindow: calculates the cosine-family-windowed spectrum and its derivative of a complex
|
xue@1
|
144 sinusoid on [0:N-1] at frequency f bins with zero central phase.
|
xue@1
|
145
|
xue@1
|
146 In: f: frequency, in bins
|
xue@1
|
147 N: window size
|
xue@1
|
148 M, c[]: cosine-family window decomposition coefficients
|
xue@1
|
149 Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
|
xue@1
|
150 dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2
|
xue@1
|
151
|
xue@1
|
152 No return value.
|
xue@1
|
153 */
|
xue@1
|
154 void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
|
xue@1
|
155 {
|
xue@1
|
156 if (K1<0) K1=0;
|
xue@1
|
157 if (K2>N/2-1) K2=N/2-1;
|
xue@1
|
158 memset(x, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
159 memset(dx, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
160
|
xue@1
|
161 for (int l=K1-M; l<=K2+M; l++)
|
xue@1
|
162 {
|
xue@1
|
163 double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
|
xue@1
|
164 long double si, co, sinn=sin(Omg), cosn=cos(Omg);
|
xue@1
|
165 si=sin(omg), co=cos(omg);
|
xue@1
|
166 double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N);
|
xue@1
|
167 double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, cosnpi=cosn*M_PI;
|
xue@1
|
168
|
xue@1
|
169 int k1=l-M, k2=l+M;
|
xue@1
|
170 if (k1<K1) k1=K1;
|
xue@1
|
171 if (k2>K2) k2=K2;
|
xue@1
|
172
|
xue@1
|
173 for (int k=k1; k<=k2; k++)
|
xue@1
|
174 {
|
xue@1
|
175 int m=k-l, kt=k-K1;
|
xue@1
|
176 if (m<0) m=-m;
|
xue@1
|
177 if (k%2)
|
xue@1
|
178 {
|
xue@1
|
179 x[kt].x-=c[m]*saco;
|
xue@1
|
180 x[kt].y+=c[m]*sinn;
|
xue@1
|
181 dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
|
xue@1
|
182 dx[kt].y+=c[m]*cosnpi;
|
xue@1
|
183 }
|
xue@1
|
184 else
|
xue@1
|
185 {
|
xue@1
|
186 x[kt].x+=c[m]*saco;
|
xue@1
|
187 x[kt].y-=c[m]*sinn;
|
xue@1
|
188 dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
|
xue@1
|
189 dx[kt].y-=c[m]*cosnpi;
|
xue@1
|
190 }
|
xue@1
|
191 }
|
xue@1
|
192 }
|
xue@1
|
193 }//dWindow
|
xue@1
|
194
|
Chris@5
|
195 /**
|
xue@1
|
196 function ddWindow: calculates the cosine-family-windowed spectrum and its 1st and 2nd derivatives of
|
xue@1
|
197 a complex sinusoid on [0:N-1] at frequency f bins with zero central phase.
|
xue@1
|
198
|
xue@1
|
199 In: f: frequency, in bins
|
xue@1
|
200 N: window size
|
xue@1
|
201 M, c[]: cosine-family window decomposition coefficients
|
xue@1
|
202 Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
|
xue@1
|
203 dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2
|
xue@1
|
204 ddx[0...K2-K1] containing the 2nd-order derivative spectrum at bins K1, ..., K2
|
xue@1
|
205
|
xue@1
|
206 No return value.
|
xue@1
|
207 */
|
xue@1
|
208 void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
|
xue@1
|
209 {
|
xue@1
|
210 if (K1<0) K1=0;
|
xue@1
|
211 if (K2>N/2-1) K2=N/2-1;
|
xue@1
|
212 memset(x, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
213 memset(dx, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
214 memset(ddx, 0, sizeof(cdouble)*(K2-K1+1));
|
xue@1
|
215
|
xue@1
|
216 for (int l=K1-M; l<=K2+M; l++)
|
xue@1
|
217 {
|
xue@1
|
218 double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
|
xue@1
|
219 long double si, co, sinn=sin(Omg), cosn=cos(Omg);
|
xue@1
|
220 si=sin(omg), co=cos(omg);
|
xue@1
|
221 double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
|
xue@1
|
222 double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, sinnpipi=sinn*M_PI*M_PI, cosnpi=cosn*M_PI,
|
xue@1
|
223 cosnpipi_n=cosnpi*M_PI/N, sipi_n=si*M_PI/N;
|
xue@1
|
224
|
xue@1
|
225 int k1=l-M, k2=l+M;
|
xue@1
|
226 if (k1<K1) k1=K1;
|
xue@1
|
227 if (k2>K2) k2=K2;
|
xue@1
|
228
|
xue@1
|
229 for (int k=k1; k<=k2; k++)
|
xue@1
|
230 {
|
xue@1
|
231 int m=k-l, kt=k-K1;
|
xue@1
|
232 if (m<0) m=-m;
|
xue@1
|
233 if (k%2)
|
xue@1
|
234 {
|
xue@1
|
235 x[kt].x-=c[m]*saco;
|
xue@1
|
236 x[kt].y+=c[m]*sinn;
|
xue@1
|
237 dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
|
xue@1
|
238 dx[kt].y+=c[m]*cosnpi;
|
xue@1
|
239 ddx[kt].x-=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
|
xue@1
|
240 ddx[kt].y-=c[m]*sinnpipi;
|
xue@1
|
241 }
|
xue@1
|
242 else
|
xue@1
|
243 {
|
xue@1
|
244 x[kt].x+=c[m]*saco;
|
xue@1
|
245 x[kt].y-=c[m]*sinn;
|
xue@1
|
246 dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
|
xue@1
|
247 dx[kt].y-=c[m]*cosnpi;
|
xue@1
|
248 ddx[kt].x+=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
|
xue@1
|
249 ddx[kt].y+=c[m]*sinnpipi;
|
xue@1
|
250 }
|
xue@1
|
251 }
|
xue@1
|
252 }
|
xue@1
|
253 }//ddWindow
|
xue@1
|
254
|
xue@1
|
255 //---------------------------------------------------------------------------
|
Chris@5
|
256 /**
|
xue@1
|
257 function IPWindow: computes the truncated inner product of a windowed spectrum with that of a sinusoid
|
xue@1
|
258 at reference frequency f.
|
xue@1
|
259
|
xue@1
|
260 In: x[0:N-1]: input spectrum
|
xue@1
|
261 f: reference frequency, in bins
|
xue@1
|
262 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
263 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
264 returnamplitude: specifies return value, true for amplitude, false for angle
|
xue@1
|
265
|
xue@1
|
266 Returns the amplitude or phase of the inner product, as specified by $returnamplitude. The return
|
xue@1
|
267 value is interpreted as the actual amplitude/phase of a sinusoid being estimated at f.
|
xue@1
|
268 */
|
xue@1
|
269 double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude)
|
xue@1
|
270 {
|
xue@1
|
271 cdouble r=IPWindowC(f, x, N, M, c, iH2, K1, K2);
|
xue@1
|
272 double result;
|
xue@1
|
273 if (returnamplitude) result=sqrt(r.x*r.x+r.y*r.y);
|
xue@1
|
274 else result=arg(r);
|
xue@1
|
275 return result;
|
xue@1
|
276 }//IPWindow
|
xue@1
|
277 //wrapper function
|
xue@1
|
278 double IPWindow(double f, void* params)
|
xue@1
|
279 {
|
xue@1
|
280 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
281 return IPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, true);
|
xue@1
|
282 }//IPWindow
|
xue@1
|
283
|
Chris@5
|
284 /**
|
xue@1
|
285 function ddIPWindow: computes the norm of the truncated inner product of a windowed spectrum with
|
xue@1
|
286 that of a sinusoid at reference frequency f, as well as its 1st and 2nd derivatives.
|
xue@1
|
287
|
xue@1
|
288 In: x[0:N-1]: input spectrum
|
xue@1
|
289 f: reference frequency, in bins
|
xue@1
|
290 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
291 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
292 Out: ipwindow and dipwindow: the truncated inner product norm and its derivative
|
xue@1
|
293
|
xue@1
|
294 Returns the 2nd derivative of the norm of the truncated inner product.
|
xue@1
|
295 */
|
xue@1
|
296 double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow)
|
xue@1
|
297 {
|
xue@1
|
298 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
299 int K=K2-K1+1;
|
xue@1
|
300 cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2], *lx=&x[K1];
|
xue@1
|
301 ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
|
xue@1
|
302 cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
|
xue@1
|
303 delete[] w;
|
xue@1
|
304
|
xue@1
|
305 double R2=~r,
|
xue@1
|
306 R=sqrt(R2),
|
xue@1
|
307 dR2=2*(r.x*dr.x+r.y*dr.y),
|
xue@1
|
308 dR=dR2/(2*R),
|
xue@1
|
309 ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr),
|
xue@1
|
310 ddR=(R*ddR2-dR2*dR)/(2*R2);
|
xue@1
|
311 ipwindow=R*iH2;
|
xue@1
|
312 dipwindow=dR*iH2;
|
xue@1
|
313 return ddR*iH2;
|
xue@1
|
314 }//ddIPWindow
|
xue@1
|
315 //wrapper function
|
xue@1
|
316 double ddIPWindow(double f, void* params)
|
xue@1
|
317 {
|
xue@1
|
318 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
319 return ddIPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow);
|
xue@1
|
320 }//ddIPWindow
|
xue@1
|
321
|
xue@1
|
322 //---------------------------------------------------------------------------
|
Chris@5
|
323 /**
|
xue@1
|
324 function IPWindowC: computes the truncated inner product of a windowed spectrum with that of a
|
xue@1
|
325 sinusoid at reference frequency f.
|
xue@1
|
326
|
xue@1
|
327 In: x[0:N-1]: input spectrum
|
xue@1
|
328 f: reference frequency, in bins
|
xue@1
|
329 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
330 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
331
|
xue@1
|
332 Returns the inner product. The return value is interpreted as the actual amplitude-phase factor of a
|
xue@1
|
333 sinusoid being estimated at f.
|
xue@1
|
334 */
|
xue@1
|
335 cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2)
|
xue@1
|
336 {
|
xue@1
|
337 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
338 int K=K2-K1+1;
|
xue@1
|
339 cdouble *w=new cdouble[K];
|
xue@1
|
340 cdouble *lx=&x[K1], result=0;
|
xue@1
|
341 Window(w, f, N, M, c, K1, K2);
|
xue@1
|
342 for (int k=0; k<K; k++) result+=lx[k]^w[k];
|
xue@1
|
343 delete[] w;
|
xue@1
|
344 result*=iH2;
|
xue@1
|
345 return result;
|
xue@1
|
346 }//IPWindowC
|
xue@1
|
347
|
xue@1
|
348 //---------------------------------------------------------------------------
|
Chris@5
|
349 /**
|
xue@1
|
350 function sIPWindow: computes the total energy of truncated inner products between multiple windowed
|
xue@1
|
351 spectra and that of a sinusoid at a reference frequency f. This does not consider phase alignment
|
xue@1
|
352 between the spectra, supposedly measured at a sequence of known instants.
|
xue@1
|
353
|
xue@1
|
354 In: x[L][N]: input spectra
|
xue@1
|
355 f: reference frequency, in bins
|
xue@1
|
356 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
357 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
358 Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)
|
xue@1
|
359
|
xue@1
|
360 Returns the energy of the vector of inner products.
|
xue@1
|
361 */
|
xue@1
|
362 double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd)
|
xue@1
|
363 {
|
xue@1
|
364 double sip=0;
|
xue@1
|
365 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
366 int K=K2-K1+1;
|
xue@1
|
367 cdouble *w=new cdouble[K];
|
xue@1
|
368 Window(w, f, N, M, c, K1, K2);
|
xue@1
|
369 for (int l=0; l<L; l++)
|
xue@1
|
370 {
|
xue@1
|
371 cdouble *lx=&x[l][K1];
|
xue@1
|
372 cdouble r=Inner(K, lx, w);
|
xue@1
|
373 if (lmd) lmd[l]=r*iH2;
|
xue@1
|
374 sip+=~r;
|
xue@1
|
375 }
|
xue@1
|
376 sip*=iH2;
|
xue@1
|
377 delete[] w;
|
xue@1
|
378 return sip;
|
xue@1
|
379 }//sIPWindow
|
xue@1
|
380 //wrapper function
|
xue@1
|
381 double sIPWindow(double f, void* params)
|
xue@1
|
382 {
|
xue@1
|
383 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dipwindow; double ipwindow; cdouble* lmd;} *p=(l_ip *)params;
|
xue@1
|
384 return sIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->lmd);
|
xue@1
|
385 }//sIPWindow
|
xue@1
|
386
|
Chris@5
|
387 /**
|
xue@1
|
388 function dsIPWindow: computes the total energy of truncated inner products between multiple windowed
|
xue@1
|
389 spectra and that of a sinusoid at a reference frequency f, as well as its derivative. This does not
|
xue@1
|
390 consider phase synchronization between the spectra, supposedly measured at a sequence of known
|
xue@1
|
391 instants.
|
xue@1
|
392
|
xue@1
|
393 In: x[L][N]: input spectra
|
xue@1
|
394 f: reference frequency, in bins
|
xue@1
|
395 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
396 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
397 Out: sip, the energy of the vector of inner products.
|
xue@1
|
398
|
xue@1
|
399 Returns the derivative of the energy of the vector of inner products.
|
xue@1
|
400 */
|
xue@1
|
401 double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip)
|
xue@1
|
402 {
|
xue@1
|
403 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
404 int K=K2-K1+1;
|
xue@1
|
405 cdouble *w=new cdouble[K*2], *dw=&w[K];
|
xue@1
|
406 dWindow(dw, w, f, N, M, c, K1, K2);
|
xue@1
|
407 double dsip; sip=0;
|
xue@1
|
408 for (int l=0; l<L; l++)
|
xue@1
|
409 {
|
xue@1
|
410 cdouble* lx=&x[l][K1];
|
xue@1
|
411 cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
|
xue@1
|
412 double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y);
|
xue@1
|
413 sip+=R2, dsip+=dR2;
|
xue@1
|
414 }
|
xue@1
|
415 sip*=iH2, dsip*=iH2;
|
xue@1
|
416 delete[] w;
|
xue@1
|
417 return dsip;
|
xue@1
|
418 }//dsIPWindow
|
xue@1
|
419 //wrapper function
|
xue@1
|
420 double dsIPWindow(double f, void* params)
|
xue@1
|
421 {
|
xue@1
|
422 struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double sip;} *p=(l_ip1 *)params;
|
xue@1
|
423 return dsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
|
xue@1
|
424 }//dsIPWindow
|
xue@1
|
425
|
Chris@5
|
426 /**
|
xue@1
|
427 function dsdIPWindow_unn: computes the energy of unnormalized truncated inner products between a given
|
xue@1
|
428 windowed spectrum and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd
|
xue@1
|
429 derivatives. "Unnormalized" indicates that the inner product cannot be taken as the actual amplitude-
|
xue@1
|
430 phase factor of a sinusoid, but deviate from that by an unspecified factor.
|
xue@1
|
431
|
xue@1
|
432 In: x[N]: input spectrum
|
xue@1
|
433 f: reference frequency, in bins
|
xue@1
|
434 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
435 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
436 Out: sipwindow and dsipwindow, the energy and its derivative of the unnormalized inner product.
|
xue@1
|
437
|
xue@1
|
438 Returns the 2nd derivative of the inner product.
|
xue@1
|
439 */
|
xue@1
|
440 double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn)
|
xue@1
|
441 {
|
xue@1
|
442 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
443 int K=K2-K1+1;
|
xue@1
|
444
|
xue@1
|
445 cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
|
xue@1
|
446
|
xue@1
|
447 ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
|
xue@1
|
448
|
xue@1
|
449 double rr=0, ri=0, drr=0, dri=0, ddrr=0, ddri=0;
|
xue@1
|
450 cdouble *lx=&x[K1];
|
xue@1
|
451 for (int k=0; k<K; k++)
|
xue@1
|
452 {
|
xue@1
|
453 rr+=lx[k].x*w[k].x+lx[k].y*w[k].y;
|
xue@1
|
454 ri+=lx[k].y*w[k].x-lx[k].x*w[k].y;
|
xue@1
|
455 drr+=lx[k].x*dw[k].x+lx[k].y*dw[k].y;
|
xue@1
|
456 dri+=lx[k].y*dw[k].x-lx[k].x*dw[k].y;
|
xue@1
|
457 ddrr+=lx[k].x*ddw[k].x+lx[k].y*ddw[k].y;
|
xue@1
|
458 ddri+=lx[k].y*ddw[k].x-lx[k].x*ddw[k].y;
|
xue@1
|
459 }
|
xue@1
|
460 delete[] w;
|
xue@1
|
461
|
xue@1
|
462 double R2=rr*rr+ri*ri,
|
xue@1
|
463 dR2=2*(rr*drr+ri*dri),
|
xue@1
|
464 ddR2=2*(rr*ddrr+ri*ddri+drr*drr+dri*dri);
|
xue@1
|
465 sipwindow=R2;
|
xue@1
|
466 dsipwindow=dR2;
|
xue@1
|
467 if (w_unn) w_unn->x=rr, w_unn->y=ri;
|
xue@1
|
468 return ddR2;
|
xue@1
|
469 }//ddsIPWindow_unn
|
xue@1
|
470
|
Chris@5
|
471 /**
|
xue@1
|
472 function ddsIPWindow: computes the total energy of truncated inner products between multiple windowed
|
xue@1
|
473 spectra and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd derivatives.
|
xue@1
|
474 This does not consider phase synchronization between the spectra, supposedly measured at a sequence
|
xue@1
|
475 of known instants.
|
xue@1
|
476
|
xue@1
|
477 In: x[L][N]: input spectra
|
xue@1
|
478 f: reference frequency, in bins
|
xue@1
|
479 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
480 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
481 Out: sip and dsip, the energy of the vector of inner products and its derivative.
|
xue@1
|
482
|
xue@1
|
483 Returns the 2nd derivative of the energy of the vector of inner products.
|
xue@1
|
484 */
|
xue@1
|
485 double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip)
|
xue@1
|
486 {
|
xue@1
|
487 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
488 int K=K2-K1+1;
|
xue@1
|
489 cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
|
xue@1
|
490 ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
|
xue@1
|
491 double ddsip=0; dsip=sip=0;
|
xue@1
|
492 for (int l=0; l<L; l++)
|
xue@1
|
493 {
|
xue@1
|
494 cdouble* lx=&x[l][K1];
|
xue@1
|
495 cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
|
xue@1
|
496 double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y), ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr);
|
xue@1
|
497 sip+=R2, dsip+=dR2, ddsip+=ddR2;
|
xue@1
|
498 }
|
xue@1
|
499 sip*=iH2, dsip*=iH2, ddsip*=iH2;
|
xue@1
|
500 delete[] w;
|
xue@1
|
501 return ddsip;
|
xue@1
|
502 }//ddsIPWindow
|
xue@1
|
503 //wrapper function
|
xue@1
|
504 double ddsIPWindow(double f, void* params)
|
xue@1
|
505 {
|
xue@1
|
506 struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dsip; double sip;} *p=(l_ip1 *)params;
|
xue@1
|
507 return ddsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dsip, p->sip);
|
xue@1
|
508 }//ddsIPWindow
|
xue@1
|
509
|
xue@1
|
510 //---------------------------------------------------------------------------
|
Chris@5
|
511 /**
|
xue@1
|
512 function sIPWindowC: computes the total energy of truncated inner products between multiple frames of
|
xue@1
|
513 a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f.
|
xue@1
|
514
|
xue@1
|
515 In: x[L][N]: the spectrogram
|
xue@1
|
516 offst_rel: frame offset, relative to frame size
|
xue@1
|
517 f: reference frequency, in bins
|
xue@1
|
518 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
519 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
520 Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)
|
xue@1
|
521
|
xue@1
|
522 Returns the energy of the vector of inner products.
|
xue@1
|
523 */
|
xue@1
|
524 double sIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd)
|
xue@1
|
525 {
|
xue@1
|
526 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
527 int K=K2-K1+1;
|
xue@1
|
528 cdouble *w=new cdouble[K];
|
xue@1
|
529 double Cr=0;
|
xue@1
|
530 cdouble Cc=0;
|
xue@1
|
531 Window(w, f, N, M, c, K1, K2);
|
xue@1
|
532 for (int l=0; l<L; l++)
|
xue@1
|
533 {
|
xue@1
|
534 cdouble *lx=&x[l][K1];
|
xue@1
|
535 cdouble r=Inner(K, lx, w);
|
xue@1
|
536 Cr+=~r;
|
xue@1
|
537 double ph=-4*M_PI*f*offst_rel*l;
|
xue@1
|
538 cdouble r2=r*r;
|
xue@1
|
539 Cc+=r2.rotate(ph);
|
xue@1
|
540 if (lmd) lmd[l]=r;
|
xue@1
|
541 }
|
xue@1
|
542 delete[] w;
|
xue@1
|
543 double result=0.5*iH2*(Cr+abs(Cc));
|
xue@1
|
544 if (lmd)
|
xue@1
|
545 {
|
xue@1
|
546 double absCc=abs(Cc), hiH2=0.5*iH2;
|
xue@1
|
547 cdouble ej2ph=Cc/absCc;
|
xue@1
|
548 for (int l=0; l<L; l++)
|
xue@1
|
549 {
|
xue@1
|
550 double ph=4*M_PI*f*offst_rel*l;
|
xue@1
|
551 lmd[l]=hiH2*(lmd[l]+(ej2ph**lmd[l]).rotate(ph));
|
xue@1
|
552 }
|
xue@1
|
553 }
|
xue@1
|
554 return result;
|
xue@1
|
555 }//sIPWindowC
|
xue@1
|
556 //wrapper function
|
xue@1
|
557 double sIPWindowC(double f, void* params)
|
xue@1
|
558 {
|
xue@1
|
559 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
560 return sIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2);
|
xue@1
|
561 }//sIPWindowC
|
xue@1
|
562
|
Chris@5
|
563 /**
|
xue@1
|
564 function dsIPWindowC: computes the total energy of truncated inner products between multiple frames of
|
xue@1
|
565 a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, together
|
xue@1
|
566 with its derivative.
|
xue@1
|
567
|
xue@1
|
568 In: x[L][N]: the spectrogram
|
xue@1
|
569 offst_rel: frame offset, relative to frame size
|
xue@1
|
570 f: reference frequency, in bins
|
xue@1
|
571 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
572 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
573 Out: sip: energy of the vector of the inner products
|
xue@1
|
574
|
xue@1
|
575 Returns the 1st derivative of the energy of the vector of inner products.
|
xue@1
|
576 */
|
xue@1
|
577 double dsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip)
|
xue@1
|
578 {
|
xue@1
|
579 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
580 int K=K2-K1+1;
|
xue@1
|
581
|
xue@1
|
582 cdouble *w=new cdouble[K*2], *dw=&w[K];
|
xue@1
|
583 dWindow(dw, w, f, N, M, c, K1, K2);
|
xue@1
|
584 double Cr=0, dCr=0;
|
xue@1
|
585 cdouble Cc=0, dCc=0;
|
xue@1
|
586 for (int l=0; l<L; l++)
|
xue@1
|
587 {
|
xue@1
|
588 cdouble *lx=&x[l][K1];
|
xue@1
|
589 cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
|
xue@1
|
590 Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y);
|
xue@1
|
591 int two=2;
|
xue@1
|
592 cdouble r2=r*r, dr2=r*dr*two;
|
xue@1
|
593 double lag=-4*M_PI*offst_rel*l, ph=lag*f;
|
xue@1
|
594 Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph);
|
xue@1
|
595 }
|
xue@1
|
596 double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y);
|
xue@1
|
597 double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1);
|
xue@1
|
598 sip=0.5*iH2*(Cr+Cc1);
|
xue@1
|
599 double dsip=0.5*iH2*(dCr+dCc1);
|
xue@1
|
600 delete[] w;
|
xue@1
|
601 return dsip;
|
xue@1
|
602 }//dsIPWindowC
|
xue@1
|
603 //wrapper function
|
xue@1
|
604 double dsIPWindowC(double f, void* params)
|
xue@1
|
605 {
|
xue@1
|
606 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sip;} *p=(l_ip *)params;
|
xue@1
|
607 return dsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
|
xue@1
|
608 }//dsIPWindowC
|
xue@1
|
609
|
Chris@5
|
610 /**
|
xue@1
|
611 function ddsIPWindowC: computes the total energy of truncated inner products between multiple frames
|
xue@1
|
612 of a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f,
|
xue@1
|
613 together with its 1st and 2nd derivatives.
|
xue@1
|
614
|
xue@1
|
615 In: x[L][N]: the spectrogram
|
xue@1
|
616 offst_rel: frame offset, relative to frame size
|
xue@1
|
617 f: reference frequency, in bins
|
xue@1
|
618 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
619 K1, K2: spectrum truncation bounds, in bins, inclusive
|
xue@1
|
620 Out: sipwindow, dsipwindow: energy of the vector of the inner products and its derivative
|
xue@1
|
621
|
xue@1
|
622 Returns the 2nd derivative of the energy of the vector of inner products.
|
xue@1
|
623 */
|
xue@1
|
624 double ddsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsipwindow, double& sipwindow)
|
xue@1
|
625 {
|
xue@1
|
626 if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
|
xue@1
|
627 int K=K2-K1+1;
|
xue@1
|
628
|
xue@1
|
629 cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
|
xue@1
|
630 ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
|
xue@1
|
631 double Cr=0, dCr=0, ddCr=0;
|
xue@1
|
632 cdouble Cc=0, dCc=0, ddCc=0;
|
xue@1
|
633 for (int l=0; l<L; l++)
|
xue@1
|
634 {
|
xue@1
|
635 cdouble *lx=&x[l][K1];
|
xue@1
|
636 cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
|
xue@1
|
637 Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); ddCr+=2*(r.x*ddr.x+r.y*ddr.y+~dr);
|
xue@1
|
638 int two=2;
|
xue@1
|
639 cdouble r2=r*r, dr2=r*dr*two, ddr2=(dr*dr+r*ddr)*two;
|
xue@1
|
640 double lag=-4*M_PI*offst_rel*l, ph=lag*f;
|
xue@1
|
641 Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph), ddCc=ddCc+(ddr2+cdouble(0,2*lag)*dr2-r2*lag*lag).rotate(ph);
|
xue@1
|
642 }
|
xue@1
|
643 double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y), ddCc2=2*(Cc.x*ddCc.x+Cc.y*ddCc.y+~dCc);
|
xue@1
|
644 double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1), ddCc1=(Cc1*ddCc2-dCc2*dCc1)/(2*Cc2);
|
xue@1
|
645 sipwindow=0.5*iH2*(Cr+Cc1);
|
xue@1
|
646 dsipwindow=0.5*iH2*(dCr+dCc1);
|
xue@1
|
647 double ddsipwindow=0.5*iH2*(ddCr+ddCc1);
|
xue@1
|
648 delete[] w;
|
xue@1
|
649 return ddsipwindow;
|
xue@1
|
650 }//ddsIPWindowC
|
xue@1
|
651 //wrapper function
|
xue@1
|
652 double ddsIPWindowC(double f, void* params)
|
xue@1
|
653 {
|
xue@1
|
654 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
655 return ddsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow);
|
xue@1
|
656 }//ddsIPWindowC
|
xue@1
|
657
|
xue@1
|
658 //--------------------------------------------------------------------------
|
xue@1
|
659 /*
|
xue@1
|
660 Least-square-error sinusoid detection function
|
xue@1
|
661
|
xue@1
|
662 version1: picking the highest peak and take measurement of a single sinusoid
|
xue@1
|
663 version2: given a rough peak location and take measurement of a single sinusoid
|
xue@1
|
664
|
xue@1
|
665 Complex spectrum x is calculated using N data points windowed by a window function that is specified
|
xue@1
|
666 by the parameter set (M, c, iH2). c[0:M] is provided according to Table 3 in the transfer report, on
|
xue@1
|
667 pp.11. iH2 is simply 1/H2, where H2 can be calculated using formula (2.17) on pp.12.
|
xue@1
|
668
|
xue@1
|
669 f & epf are given/returned in bins.
|
xue@1
|
670
|
xue@1
|
671 Further reading: "Least-square-error estimation of sinusoids.pdf"
|
xue@1
|
672 */
|
xue@1
|
673
|
Chris@5
|
674 /**
|
xue@1
|
675 function LSESinusoid: LSE estimation of the predominant stationary sinusoid.
|
xue@1
|
676
|
xue@1
|
677 In: x[N]: windowed spectrum
|
xue@1
|
678 B: spectral truncation half width, in bins.
|
xue@1
|
679 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
680 epf: frequency error tolerance, in bins
|
xue@1
|
681 Out: a and pp: amplitude and phase estimates
|
xue@1
|
682
|
xue@1
|
683 Returns the frequency estimate, in bins.
|
xue@1
|
684 */
|
xue@1
|
685 double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
|
xue@1
|
686 {
|
xue@1
|
687 struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; //(l_hx *)¶ms;
|
Chris@3
|
688 int dfshift=offsetof(l_hx, dhxpeak);
|
xue@1
|
689
|
xue@1
|
690 int inp;
|
xue@1
|
691 double minp=0;
|
xue@1
|
692 for (int i=0; i<N; i++)
|
xue@1
|
693 {
|
xue@1
|
694 double lf=i, tmp;
|
xue@1
|
695 p.k1=ceil(lf-B); if (p.k1<0) p.k1=0;
|
xue@1
|
696 p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
697 tmp=IPWindow(lf, &p);
|
xue@1
|
698 if (minp<tmp) inp=i, minp=tmp;
|
xue@1
|
699 }
|
xue@1
|
700
|
xue@1
|
701 double f=inp;
|
xue@1
|
702 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
703 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
704 double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
|
xue@1
|
705 if (tmp==-1)
|
xue@1
|
706 {
|
xue@1
|
707 Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
|
xue@1
|
708 }
|
xue@1
|
709 else
|
xue@1
|
710 a=p.hxpeak;
|
xue@1
|
711 pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
|
xue@1
|
712 return f;
|
xue@1
|
713 }//LSESinusoid
|
xue@1
|
714
|
xue@1
|
715 /*function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency.
|
xue@1
|
716
|
xue@1
|
717 In: x[N]: windowed spectrum
|
xue@1
|
718 f: initial frequency, in bins
|
xue@1
|
719 B: spectral truncation half width, in bins.
|
xue@1
|
720 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
721 epf: frequency error tolerance, in bins
|
xue@1
|
722 Out: f, a and pp: frequency, amplitude and phase estimates
|
xue@1
|
723
|
xue@1
|
724 No return value.
|
xue@1
|
725 */
|
xue@1
|
726 void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
|
xue@1
|
727 {
|
xue@1
|
728 struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};
|
Chris@3
|
729 int dfshift=offsetof(l_hx, dhxpeak);
|
xue@1
|
730
|
xue@1
|
731 double inp=f;
|
xue@1
|
732 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
733 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
734 double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
|
xue@1
|
735 if (tmp==-1)
|
xue@1
|
736 {
|
xue@1
|
737 Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
|
xue@1
|
738 }
|
xue@1
|
739 else
|
xue@1
|
740 a=p.hxpeak;
|
xue@1
|
741 pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
|
xue@1
|
742 }//LSESinusoid
|
xue@1
|
743
|
Chris@5
|
744 /**
|
xue@1
|
745 function LSESinusoid: LSE estimation of stationary sinusoid predominant within [f1, f2].
|
xue@1
|
746
|
xue@1
|
747 In: x[N]: windowed spectrum
|
xue@1
|
748 [f1, f2]: frequency range
|
xue@1
|
749 B: spectral truncation half width, in bins.
|
xue@1
|
750 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
751 epf: frequency error tolerance, in bins
|
xue@1
|
752 Out: a and pp: amplitude and phase estimates
|
xue@1
|
753
|
xue@1
|
754 Returns the frequency estimate, in bins.
|
xue@1
|
755 */
|
xue@1
|
756 double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
|
xue@1
|
757 {
|
xue@1
|
758 struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};
|
Chris@3
|
759 int dfshift=offsetof(l_hx, dhxpeak);
|
xue@1
|
760
|
xue@1
|
761 int inp;
|
xue@1
|
762 double minp=0;
|
xue@1
|
763 for (int i=f1; i<f2; i++)
|
xue@1
|
764 {
|
xue@1
|
765 double lf=i, tmp;
|
xue@1
|
766 p.k1=ceil(lf-B); if (p.k1<0) p.k1=0;
|
xue@1
|
767 p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
768 tmp=IPWindow(lf, &p);
|
xue@1
|
769 if (minp<tmp) inp=i, minp=tmp;
|
xue@1
|
770 }
|
xue@1
|
771
|
xue@1
|
772 double f=inp;
|
xue@1
|
773 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
774 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
775 double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
|
xue@1
|
776 if (tmp==-1)
|
xue@1
|
777 {
|
xue@1
|
778 Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
|
xue@1
|
779 }
|
xue@1
|
780 else
|
xue@1
|
781 a=p.hxpeak;
|
xue@1
|
782 pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
|
xue@1
|
783 return f;
|
xue@1
|
784 }//LSESinusoid
|
xue@1
|
785
|
Chris@5
|
786 /**
|
xue@1
|
787 function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency within [f1,
|
xue@1
|
788 f2].
|
xue@1
|
789
|
xue@1
|
790 In: x[N]: windowed spectrum
|
xue@1
|
791 f: initial frequency, in bins
|
xue@1
|
792 [f1, f2]: frequency range
|
xue@1
|
793 B: spectral truncation half width, in bins.
|
xue@1
|
794 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
795 epf: frequency error tolerance, in bins
|
xue@1
|
796 Out: f, a and pp: frequency, amplitude and phase estimates
|
xue@1
|
797
|
xue@1
|
798 Returns 1 if managed to find a sinusoid, 0 if not, upon which $a and $pp are estimated at the initial
|
xue@1
|
799 f.
|
xue@1
|
800 */
|
xue@1
|
801 int LSESinusoid(double& f, double f1, double f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
|
xue@1
|
802 {
|
xue@1
|
803 struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};//(l_hx *)¶ms;
|
Chris@3
|
804 int dfshift=offsetof(l_hx, dhxpeak);
|
xue@1
|
805
|
xue@1
|
806 int result=0;
|
xue@1
|
807 double inp=f;
|
xue@1
|
808 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
809 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
810 double tmp=Newton(f, ddIPWindow, &p, dfshift, epf, 100, 1e-256, f1, f2);
|
xue@1
|
811 if (tmp!=-1 && f>f1 && f<f2)
|
xue@1
|
812 {
|
xue@1
|
813 result=1;
|
xue@1
|
814 a=p.hxpeak;
|
xue@1
|
815 pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
|
xue@1
|
816 }
|
xue@1
|
817 else
|
xue@1
|
818 {
|
xue@1
|
819 Search1DmaxEx(f, &p, IPWindow, f1, f2, &a, epf);
|
xue@1
|
820 if (f<=f1 || f>=f2)
|
xue@1
|
821 {
|
xue@1
|
822 f=inp;
|
xue@1
|
823 cdouble r=IPWindowC(f, x, N, M, c, iH2, p.k1, p.k2);
|
xue@1
|
824 a=abs(r);
|
xue@1
|
825 pp=arg(r);
|
xue@1
|
826 }
|
xue@1
|
827 else
|
xue@1
|
828 {
|
xue@1
|
829 result=1;
|
xue@1
|
830 pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
|
xue@1
|
831 }
|
xue@1
|
832 }
|
xue@1
|
833 return result;
|
xue@1
|
834 }//LSESinusoid
|
xue@1
|
835
|
Chris@5
|
836 /**
|
xue@1
|
837 function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
|
xue@1
|
838 considering phase-frequency consistency across frames.
|
xue@1
|
839
|
xue@1
|
840 In: x[Fr][N]: spectrogram
|
xue@1
|
841 f: initial frequency, in bins
|
xue@1
|
842 [f1, f2]: frequency range
|
xue@1
|
843 B: spectral truncation half width, in bins.
|
xue@1
|
844 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
845 epf: frequency error tolerance, in bins
|
xue@1
|
846 Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates
|
xue@1
|
847
|
xue@1
|
848 Returns an error bound of the frequency estimate.
|
xue@1
|
849 */
|
xue@1
|
850 double LSESinusoidMP(double& f, double f1, double f2, cdouble** x, int Fr, int N, double B, int M, double* c, double iH2, double* a, double* ph, double epf)
|
xue@1
|
851 {
|
xue@1
|
852 struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int L; cdouble** x; double dsip; double sip; cdouble* lmd;} p={N, 0, 0, M, c,iH2, Fr, x, 0, 0, 0};
|
Chris@3
|
853 int dfshift=offsetof(l_ip1, dsip), fshift=offsetof(l_ip1, sip);
|
xue@1
|
854
|
xue@1
|
855 double inp=f;
|
xue@1
|
856 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
857 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
858 double errf=Newton1dmax(f, f1, f2, ddsIPWindow, &p, dfshift, fshift, dsIPWindow, dfshift, epf);
|
xue@1
|
859 if (errf<0) errf=Search1Dmax(f, &p, sIPWindow, f1, f2, a, epf);
|
xue@1
|
860 if (a || ph)
|
xue@1
|
861 {
|
xue@1
|
862 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
863 {
|
xue@1
|
864 cdouble r=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);
|
xue@1
|
865 if (a) a[fr]=abs(r);
|
xue@1
|
866 if (ph) ph[fr]=arg(r);
|
xue@1
|
867 }
|
xue@1
|
868 }
|
xue@1
|
869 return errf;
|
xue@1
|
870 }//LSESinusoidMP
|
xue@1
|
871
|
Chris@5
|
872 /**
|
xue@1
|
873 function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
|
xue@1
|
874 considering phase-frequency consistency across frames.
|
xue@1
|
875
|
xue@1
|
876 In: x[Fr][N]: spectrogram
|
xue@1
|
877 f: initial frequency, in bins
|
xue@1
|
878 [f1, f2]: frequency range
|
xue@1
|
879 B: spectral truncation half width, in bins.
|
xue@1
|
880 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
881 epf: frequency error tolerance, in bins
|
xue@1
|
882 Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates
|
xue@1
|
883
|
xue@1
|
884 Returns an error bound of the frequency estimate. Although the frequencies are estimated assuming
|
xue@1
|
885 cross-frame frequency-phase consistency, the final output phase angles are reestimated independently
|
xue@1
|
886 for each frame using the frequency estimate.
|
xue@1
|
887 */
|
xue@1
|
888 double LSESinusoidMPC(double& f, double f1, double f2, cdouble** x, int Fr, int N, int Offst, double B, int M, double* c, double iH2, double* a, double* ph, double epf)
|
xue@1
|
889 {
|
xue@1
|
890 struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sdip; double sip;}
|
xue@1
|
891 p={N, 0, 0, M, c,iH2, Fr, Offst*1.0/N, x, 0, 0};
|
Chris@3
|
892 int dfshift=offsetof(l_ip, sdip), fshift=offsetof(l_ip, sip);
|
xue@1
|
893
|
xue@1
|
894 double inp=f;
|
xue@1
|
895 p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
|
xue@1
|
896 p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
|
xue@1
|
897 double errf=Newton1dmax(f, f1, f2, ddsIPWindowC, &p, dfshift, fshift, dsIPWindowC, dfshift, epf);
|
xue@1
|
898 if (errf<0) errf=Search1Dmax(f, &p, sIPWindowC, f1, f2, a, epf);
|
xue@1
|
899 if (a || ph)
|
xue@1
|
900 {
|
xue@1
|
901 cdouble* lmd=new cdouble[Fr];
|
xue@1
|
902 sIPWindowC(f, Fr, Offst*1.0/N, x, N, M, c, iH2, p.k1, p.k2, lmd);
|
xue@1
|
903 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
904 {
|
xue@1
|
905 lmd[fr]=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);
|
xue@1
|
906
|
xue@1
|
907 if (a) a[fr]=abs(lmd[fr]);
|
xue@1
|
908 if (ph) ph[fr]=arg(lmd[fr]);
|
xue@1
|
909 }
|
xue@1
|
910 delete[] lmd;
|
xue@1
|
911 }
|
xue@1
|
912 return errf;
|
xue@1
|
913 }//LSESinusoidMPC
|
xue@1
|
914
|
xue@1
|
915 //---------------------------------------------------------------------------
|
Chris@5
|
916 /**
|
xue@1
|
917 function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
|
xue@1
|
918 suppression index of eps, i.e. the least square error is minimized with an additional eps*||lmd||^2
|
xue@1
|
919 term.
|
xue@1
|
920
|
xue@1
|
921 In: x[Wid]: spectrum
|
xue@1
|
922 f[I]: frequencies
|
xue@1
|
923 M, c[]: cosine-family window specification parameters
|
xue@1
|
924 K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
|
xue@1
|
925 eps: energy suppression factor
|
xue@1
|
926 Out: lmd[I]: amplitude-phase factors
|
xue@1
|
927
|
xue@1
|
928 No return value.
|
xue@1
|
929 */
|
xue@1
|
930 void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps)
|
xue@1
|
931 {
|
xue@1
|
932 if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
|
xue@1
|
933 MList* List=new MList;
|
xue@1
|
934 cdouble** Allocate2L(cdouble, I, K, wt, List);
|
xue@1
|
935 for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
|
xue@1
|
936 cdouble** whw=MultiplyXcXt(I, K, wt, List);
|
xue@1
|
937 cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
|
xue@1
|
938 for (int i=0; i<I; i++) whw[i][i]+=eps;
|
xue@1
|
939 GECP(I, lmd, whw, whx);
|
xue@1
|
940 delete List;
|
xue@1
|
941 }//IPMulti
|
xue@1
|
942
|
Chris@5
|
943 /**
|
xue@1
|
944 function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
|
xue@1
|
945 suppression index of eps, and optionally returns residue and sensitivity indicators for each sinusoid.
|
xue@1
|
946
|
xue@1
|
947 In: x[Wid]: spectrum
|
xue@1
|
948 f[I]: frequencies
|
xue@1
|
949 M, c[]: cosine-family window specification parameters
|
xue@1
|
950 K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
|
xue@1
|
951 eps: energy suppression factor
|
xue@1
|
952 Out: lmd[I]: amplitude-phase factors
|
xue@1
|
953 sens[I]: sensitivity indicators
|
xue@1
|
954 r1[I]: residue indicators, measured by correlating residue with sinusoid spectra, optional
|
xue@1
|
955
|
xue@1
|
956 No return value. Sensibitily is computed BEFORE applying eps.
|
xue@1
|
957 */
|
xue@1
|
958 void IPMulti(int I, double* f, cdouble* lmd, cfloat* x, int Wid, int K1, int K2, int M, double* c, double eps, double* sens, double* r1)
|
xue@1
|
959 {
|
xue@1
|
960 if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
|
xue@1
|
961 MList* List=new MList;
|
xue@1
|
962 cdouble** Allocate2L(cdouble, I, K, wt, List);
|
xue@1
|
963 for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
|
xue@1
|
964 cdouble** whw=MultiplyXcXt(I, K, wt, List);
|
xue@1
|
965
|
xue@1
|
966 //*computes sensitivity if required
|
xue@1
|
967 if (sens)
|
xue@1
|
968 {
|
xue@1
|
969 cdouble** iwhw=Copy(I, whw, List);
|
xue@1
|
970 GICP(I, iwhw);
|
xue@1
|
971 cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
|
xue@1
|
972 for (int i=0; i<I; i++)
|
xue@1
|
973 {
|
xue@1
|
974 sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
|
xue@1
|
975 }
|
xue@1
|
976 } //*/
|
xue@1
|
977 cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
|
xue@1
|
978 for (int i=0; i<I; i++) whw[i][i]+=eps;
|
xue@1
|
979 GECP(I, lmd, whw, whx);
|
xue@1
|
980 //compute residue if required
|
xue@1
|
981 if (r1)
|
xue@1
|
982 {
|
xue@1
|
983 cdouble* wlmd=MultiplyXty(K, I, wt, lmd, List); //reconstruct
|
xue@1
|
984 for (int k=0; k<K; k++) wlmd[k]=wlmd[k]-x[K1+k]; //-residue
|
xue@1
|
985 for (int i=0; i<I; i++) //r1[i]=Inner(K, wlmd, wt[i]).abs(); //-residue weighted by window
|
xue@1
|
986 {
|
xue@1
|
987 r1[i]=0;
|
xue@1
|
988 for (int k=0; k<K; k++) r1[i]+=abs(wlmd[k])*abs(wt[i][k]);
|
xue@1
|
989 }
|
xue@1
|
990 }
|
xue@1
|
991 delete List;
|
xue@1
|
992 }//IPMulti
|
xue@1
|
993
|
Chris@5
|
994 /**
|
xue@1
|
995 function IPMultiSens: computes the sensitivity of the least square estimation of multiple sinusoids given
|
xue@1
|
996 their frequencies .
|
xue@1
|
997
|
xue@1
|
998 In: f[I]: frequencies
|
xue@1
|
999 M, c[]: cosine-family window specification parameters
|
xue@1
|
1000 K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
|
xue@1
|
1001 eps: energy suppression factor
|
xue@1
|
1002 Out: sens[I]: sensitivity indicators
|
xue@1
|
1003
|
xue@1
|
1004 No return value. Sensibility is computed AFTER applying eps
|
xue@1
|
1005 */
|
xue@1
|
1006 void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps)
|
xue@1
|
1007 {
|
xue@1
|
1008 if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
|
xue@1
|
1009 MList* List=new MList;
|
xue@1
|
1010 cdouble** Allocate2L(cdouble, I, K, wt, List);
|
xue@1
|
1011 for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
|
xue@1
|
1012
|
xue@1
|
1013 cdouble** whw=MultiplyXcXt(I, K, wt, List);
|
xue@1
|
1014 for (int i=0; i<I; i++) whw[i][i]+=eps;
|
xue@1
|
1015
|
xue@1
|
1016 cdouble** iwhw=Copy(I, whw, List);
|
xue@1
|
1017 GICP(I, iwhw);
|
xue@1
|
1018 cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
|
xue@1
|
1019 for (int i=0; i<I; i++)
|
xue@1
|
1020 {
|
xue@1
|
1021 sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
|
xue@1
|
1022 }
|
xue@1
|
1023 delete List;
|
xue@1
|
1024 }//IPMultiSens
|
xue@1
|
1025
|
Chris@5
|
1026 /**
|
xue@1
|
1027 function IPMulti: least square estimation of multi-sinusoids with GIVEN frequencies. This version
|
xue@1
|
1028 operates in groups at least B bins from each other, rather than LSE all frequencies together.
|
xue@1
|
1029
|
xue@1
|
1030 In: x[Wid]: spectrum
|
xue@1
|
1031 f[I]: frequencies, must be ordered low to high.
|
xue@1
|
1032 B: number of bins beyond which sinusoids are treated as non-interfering
|
xue@1
|
1033 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
1034 Out: lmd[I]: amplitude-phase factors
|
xue@1
|
1035
|
xue@1
|
1036 Returns 0.
|
xue@1
|
1037 */
|
xue@1
|
1038 double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B)
|
xue@1
|
1039 {
|
xue@1
|
1040 int i=0, ist=0;
|
xue@1
|
1041 double Bw=B;
|
xue@1
|
1042 while (i<I)
|
xue@1
|
1043 {
|
xue@1
|
1044 if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
|
xue@1
|
1045 {
|
xue@1
|
1046 if (i==I-1) i++;
|
xue@1
|
1047 //process frequencies from ist to i-1
|
xue@1
|
1048 if (i-1==ist) //one sinusoid
|
xue@1
|
1049 {
|
xue@1
|
1050 double fb=f[ist]; int K1=floor(fb-B+0.5), K2=floor(fb+B+0.5);
|
xue@1
|
1051 lmd[ist]=IPWindowC(fb, x, Wid, M, c, iH2, K1, K2);
|
xue@1
|
1052 }
|
xue@1
|
1053 else
|
xue@1
|
1054 {
|
xue@1
|
1055 MList* List=new MList;
|
xue@1
|
1056 int N=i-ist, K1=floor(f[ist]-B+0.5), K2=floor(f[i-1]+B+0.5), K=K2-K1+1;
|
xue@1
|
1057 cdouble** Allocate2L(cdouble, N, K, wt, List);
|
xue@1
|
1058 for (int n=0; n<N; n++) Window(wt[n], f[ist+n], Wid, M, c, K1, K2);
|
xue@1
|
1059 cdouble* whx=MultiplyXcy(N, K, wt, &x[K1], List); //w*'x=(wt*)x
|
xue@1
|
1060 cdouble** whw=MultiplyXcXt(N, K, wt, List);
|
xue@1
|
1061 /*debug cdouble** C=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** C2=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** Bh=SubMatrix(0, whw, 1, 4, 0, 1, List); cdouble* Y2=SubVector(0, whx, 1, 4);
|
xue@1
|
1062 cdouble x2[4]; cdouble x1=lmd[ist], Bhx1[4], dx2[4]; for (int j=0; j<4; j++) Bhx1[j]=x1^Bh[j][0]; GECP(4, x2, C, Y2); GECP(4, dx2, C2, Bhx1);*/
|
xue@1
|
1063 GECP(N, &lmd[ist], whw, whx); //solving complex linear system (w*'w)a=w*'x
|
xue@1
|
1064 delete List;
|
xue@1
|
1065 }
|
xue@1
|
1066 ist=i;
|
xue@1
|
1067 }
|
xue@1
|
1068 i++;
|
xue@1
|
1069 }
|
xue@1
|
1070 return 0;
|
xue@1
|
1071 }//IPMulti
|
xue@1
|
1072
|
Chris@5
|
1073 /**
|
xue@1
|
1074 function IPMulti_Direct: LSE estimation of multiple sinusoids given frequencies AND PHASES (direct
|
xue@1
|
1075 method)
|
xue@1
|
1076
|
xue@1
|
1077 In: x[Wid]: spectrum
|
xue@1
|
1078 f[I], ph[I]: frequencies and phase angles.
|
xue@1
|
1079 B: spectral truncation half width, in bins; sinusoids over 3B bins apart are regarded non-interfering
|
xue@1
|
1080 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
1081 Out: a[I]: amplitudes
|
xue@1
|
1082
|
xue@1
|
1083 Returns square norm of the residue.
|
xue@1
|
1084 */
|
xue@1
|
1085 double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B)
|
xue@1
|
1086 {
|
xue@1
|
1087 MList* List=new MList;
|
xue@1
|
1088 int i=0, ist=0, hWid=Wid/2;
|
xue@1
|
1089 cdouble* r=Copy(hWid, x, List); //to store the residue
|
xue@1
|
1090
|
xue@1
|
1091 double Bw=3.0*B;
|
xue@1
|
1092 while (i<I)
|
xue@1
|
1093 {
|
xue@1
|
1094 if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
|
xue@1
|
1095 {
|
xue@1
|
1096 if (i==I-1) i++;
|
xue@1
|
1097
|
xue@1
|
1098 //process frequencies from ist to i-1
|
xue@1
|
1099 if (i-1==ist) //one sinusoid
|
xue@1
|
1100 {
|
xue@1
|
1101 double fb=f[ist];
|
xue@1
|
1102 cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
|
xue@1
|
1103 for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
|
xue@1
|
1104 double ip=Inner(2*hWid, (double*)x, (double*)w);
|
xue@1
|
1105 a[ist]=ip*iH2;
|
xue@1
|
1106 MultiAdd(hWid, r, r, w, -a[ist]);
|
xue@1
|
1107 delete[] w;
|
xue@1
|
1108 }
|
xue@1
|
1109 else
|
xue@1
|
1110 {
|
xue@1
|
1111 int N=i-ist;
|
xue@1
|
1112 cdouble** Allocate2L(cdouble, N, hWid, wt, List);
|
xue@1
|
1113 for (int n=0; n<N; n++)
|
xue@1
|
1114 {
|
xue@1
|
1115 Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
|
xue@1
|
1116 for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
|
xue@1
|
1117 }
|
xue@1
|
1118 double* whxr=MultiplyXy(N, hWid*2, (double**)wt, (double*)x, List); //w*'x=(wt*)x
|
xue@1
|
1119 double** whwr=MultiplyXXt(N, hWid*2, (double**)wt, List);
|
xue@1
|
1120 GECP(N, &a[ist], whwr, whxr); //solving complex linear system (w*'w)a=w*'x
|
xue@1
|
1121 for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
|
xue@1
|
1122 }
|
xue@1
|
1123 ist=i;
|
xue@1
|
1124 }
|
xue@1
|
1125 i++;
|
xue@1
|
1126 }
|
xue@1
|
1127 double result=Inner(hWid, r, r).x;
|
xue@1
|
1128 delete List;
|
xue@1
|
1129 return result;
|
xue@1
|
1130 }//IPMulti_Direct
|
xue@1
|
1131
|
Chris@5
|
1132 /**
|
xue@1
|
1133 function IPMulti_GS: LSE estimation of multiple sinusoids given frequencies AND PHASES (Gram-Schmidt method)
|
xue@1
|
1134
|
xue@1
|
1135 In: x[Wid]: spectrum
|
xue@1
|
1136 f[I], ph[I]: frequencies and phase angles.
|
xue@1
|
1137 B: spectral truncation, in bins; sinusoids over 3B bins apart are regarded non-interfering
|
xue@1
|
1138 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
1139 Out: a[I]: amplitudes
|
xue@1
|
1140
|
xue@1
|
1141 Returns square norm of the residue.
|
xue@1
|
1142 */
|
xue@1
|
1143 double IPMulti_GS(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B, double** L, double** Q)
|
xue@1
|
1144 {
|
xue@1
|
1145 MList* List=new MList;
|
xue@1
|
1146 int i=0, ist=0, hWid=Wid/2;
|
xue@1
|
1147 cdouble* r=Copy(hWid, x, List); //to store the residue
|
xue@1
|
1148 double Bw=3.0*B;
|
xue@1
|
1149 while (i<I)
|
xue@1
|
1150 {
|
xue@1
|
1151 if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
|
xue@1
|
1152 {
|
xue@1
|
1153 if (i==I-1) i++;
|
xue@1
|
1154
|
xue@1
|
1155 //process frequencies from ist to i-1
|
xue@1
|
1156 if (i-1==ist) //one sinusoid
|
xue@1
|
1157 {
|
xue@1
|
1158 double fb=f[ist];
|
xue@1
|
1159 cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
|
xue@1
|
1160 for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
|
xue@1
|
1161 double ip=Inner(2*hWid, (double*)x, (double*)w);
|
xue@1
|
1162 a[ist]=ip*iH2;
|
xue@1
|
1163 MultiAdd(hWid, r, r, w, -a[ist]);
|
xue@1
|
1164 delete[] w;
|
xue@1
|
1165 }
|
xue@1
|
1166 else
|
xue@1
|
1167 {
|
xue@1
|
1168 int N=i-ist;
|
xue@1
|
1169 cdouble** Allocate2L(cdouble, N, hWid, wt, List);
|
xue@1
|
1170 Alloc2L(N, N, L, List); Alloc2L(N, hWid*2, Q, List);
|
xue@1
|
1171 for (int n=0; n<N; n++)
|
xue@1
|
1172 {
|
xue@1
|
1173 Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
|
xue@1
|
1174 for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
|
xue@1
|
1175 }
|
xue@1
|
1176 LQ_GS(N, hWid*2, (double**)wt, L, Q);
|
xue@1
|
1177 double* atl=MultiplyxYt(N, hWid*2, (double*)x, Q, List);
|
xue@1
|
1178 GExL(N, &a[ist], L, atl);
|
xue@1
|
1179 for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
|
xue@1
|
1180 }
|
xue@1
|
1181 ist=i;
|
xue@1
|
1182 }
|
xue@1
|
1183 i++;
|
xue@1
|
1184 }
|
xue@1
|
1185 double result=Inner(hWid, r, r).x;
|
xue@1
|
1186 delete List;
|
xue@1
|
1187 return result;
|
xue@1
|
1188 }//IPMulti_GS
|
xue@1
|
1189
|
Chris@5
|
1190 /**
|
xue@1
|
1191 function IPMulti: LSE estimation of I sinusoids given frequency and phase and J sinusoids given
|
xue@1
|
1192 frequency only
|
xue@1
|
1193
|
xue@1
|
1194 In: x[Wid]: spectrum
|
xue@1
|
1195 f[I+J], ph[I]: frequencies and phase angles
|
xue@1
|
1196 M, c[], iH2: cosine-family window specification parameters
|
xue@1
|
1197 Out: a[I+J]: amplitudes
|
xue@1
|
1198 ph[I:I+J-1]: phase angles not given on start
|
xue@1
|
1199 wt[I+2J][hWid], Q[I+2J][hWid], L[I+2J][I+2J]: internal w matrix and its LQ factorization, optional
|
xue@1
|
1200
|
xue@1
|
1201 Returns the residue vector, newly created and registered to RetList, if specified. On start a[] should
|
xue@1
|
1202 have valid storage no less than I+2J.
|
xue@1
|
1203 */
|
xue@1
|
1204 cdouble* IPMulti(int I, int J, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, cdouble** wt, cdouble** Q, double** L, MList* RetList)
|
xue@1
|
1205 {
|
xue@1
|
1206 MList* List=new MList;
|
xue@1
|
1207 int hWid=Wid/2;
|
xue@1
|
1208 cdouble* r=Copy(hWid, x, RetList); //to store the residue
|
xue@1
|
1209 if (!wt){Allocate2L(cdouble, I+J*2, hWid, wt, List);}
|
xue@1
|
1210 if (!Q){Allocate2L(cdouble, I+J*2, hWid, Q, List);}
|
xue@1
|
1211 if (!L){Allocate2L(double, I+J*2, I+J*2, L, List);}
|
xue@1
|
1212 memset(wt[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
|
xue@1
|
1213 memset(Q[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
|
xue@1
|
1214 memset(L[0], 0, sizeof(double)*(I+J*2)*(I+J*2));
|
xue@1
|
1215
|
xue@1
|
1216 //*The direct form
|
xue@1
|
1217 for (int i=0; i<I; i++)
|
xue@1
|
1218 {
|
xue@1
|
1219 Window(wt[i], f[i], Wid, M, c, 0, hWid-1);
|
xue@1
|
1220 for (int k=0; k<hWid; k++) wt[i][k].rotate(ph[i]);
|
xue@1
|
1221 }
|
xue@1
|
1222 for (int j=0; j<J; j++)
|
xue@1
|
1223 {
|
xue@1
|
1224 cdouble *w1=wt[I+j*2], *w2=wt[I+j*2+1];
|
xue@1
|
1225 Window(w1, f[I+j], Wid, M, c, 0, hWid-1);
|
xue@1
|
1226 for (int k=0; k<hWid; k++) w2[k].y=w1[k].x, w2[k].x=-w1[k].y;
|
xue@1
|
1227 }
|
xue@1
|
1228
|
xue@1
|
1229 LQ_GS(I+J*2, hWid*2, (double**)wt, L, (double**)Q);
|
xue@1
|
1230 double *atl=MultiplyxYt(I+J*2, hWid*2, (double*)x, (double**)Q, List);
|
xue@1
|
1231 GExL(I+J*2, a, L, atl);
|
xue@1
|
1232
|
xue@1
|
1233 for (int i=0; i<I+J*2; i++) MultiAdd(hWid, r, r, wt[i], -a[i]);
|
xue@1
|
1234 for (int j=0; j<J; j++)
|
xue@1
|
1235 {
|
xue@1
|
1236 double xx=a[I+j*2], yy=a[I+j*2+1];
|
xue@1
|
1237 a[I+j]=sqrt(xx*xx+yy*yy);
|
xue@1
|
1238 ph[I+j]=atan2(yy, xx);
|
xue@1
|
1239 }
|
xue@1
|
1240 delete List;
|
xue@1
|
1241 return r;
|
xue@1
|
1242 }//IPMulti
|
xue@1
|
1243
|
xue@1
|
1244 //---------------------------------------------------------------------------
|
xue@1
|
1245 /*
|
xue@1
|
1246 Routines for estimation two sinusoids with 1 fixed and 1 flexible frequency
|
xue@1
|
1247
|
xue@1
|
1248 Further reading: "LSE estimation for 2 sinusoids with 1 at a fixed frequency.pdf"
|
xue@1
|
1249 */
|
xue@1
|
1250
|
Chris@5
|
1251 /**
|
xue@1
|
1252 function WindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
|
xue@1
|
1253 sinusoids at frequencies f1 and f2, df=f1-f2.
|
xue@1
|
1254
|
xue@1
|
1255 In: df: frequency difference, in bins
|
xue@1
|
1256 N: DFT size
|
xue@1
|
1257 M, d[]: cosine-family window specification parameters (see "further reading").
|
xue@1
|
1258 Out: w[0], the inner product, optional
|
xue@1
|
1259
|
xue@1
|
1260 Returns square norm of the inner product.
|
xue@1
|
1261 */
|
xue@1
|
1262 double WindowDuo(double df, int N, double* d, int M, cdouble* w)
|
xue@1
|
1263 {
|
xue@1
|
1264 double wr=0, wi=0;
|
xue@1
|
1265 for (int m=-2*M; m<=2*M; m++)
|
xue@1
|
1266 {
|
xue@1
|
1267 double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
|
xue@1
|
1268 double si=sin(omg), co=cos(omg), sinn=sin(Omg);
|
xue@1
|
1269 double sa=(ang==0)?N:(sinn/si);
|
xue@1
|
1270 double dm; if (m<0) dm=d[-m]; else dm=d[m];
|
xue@1
|
1271 wr+=dm*sa*co, wi+=-dm*sinn;
|
xue@1
|
1272 }
|
xue@1
|
1273 wr*=N, wi*=N;
|
xue@1
|
1274 if (w) w->x=wr, w->y=wi;
|
xue@1
|
1275 double result=wr*wr+wi*wi;
|
xue@1
|
1276 return result;
|
xue@1
|
1277 }//WindowDuo
|
xue@1
|
1278
|
Chris@5
|
1279 /**
|
xue@1
|
1280 function ddWindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
|
xue@1
|
1281 sinusoids at frequencies f1 and f2, df=f1-f2, with its 1st and 2nd derivatives
|
xue@1
|
1282
|
xue@1
|
1283 In: df: frequency difference, in bins
|
xue@1
|
1284 N: DFT size
|
xue@1
|
1285 M, d[]: cosine-family window specification parameters (see "further reading" for d[]).
|
xue@1
|
1286 Out: w[0], the inner product, optional
|
xue@1
|
1287 window, dwindow: square norm and its derivative, of the inner product
|
xue@1
|
1288
|
xue@1
|
1289 Returns 2nd derivative of the square norm of the inner product.
|
xue@1
|
1290 */
|
xue@1
|
1291 double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w)
|
xue@1
|
1292 {
|
xue@1
|
1293 double wr=0, wi=0, dwr=0, dwi=0, ddwr=0, ddwi=0, PI_N=M_PI/N, PIPI_N=PI_N*M_PI, PIPI=M_PI*M_PI;
|
xue@1
|
1294 for (int m=-2*M; m<=2*M; m++)
|
xue@1
|
1295 {
|
xue@1
|
1296 double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
|
xue@1
|
1297 double si=sin(omg), co=cos(omg), sinn=sin(Omg), cosn=cos(Omg);
|
xue@1
|
1298 double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
|
xue@1
|
1299 double dm; if (m<0) dm=d[-m]; else dm=d[m];
|
xue@1
|
1300 wr+=dm*sa*co, wi+=-dm*sinn;
|
xue@1
|
1301 dwr+=dm*(dsa*co-PI_N*sinn), dwi+=-dm*M_PI*cosn;
|
xue@1
|
1302 ddwr+=dm*(ddsa*co-PI_N*dsa*si-PIPI_N*cosn), ddwi+=dm*PIPI*sinn;
|
xue@1
|
1303 }
|
xue@1
|
1304 wr*=N, wi*=N, dwr*=N, dwi*=N, ddwr*=N, ddwi*=N;
|
xue@1
|
1305 window=wr*wr+wi*wi;
|
xue@1
|
1306 dwindow=2*(wr*dwr+wi*dwi);
|
xue@1
|
1307 if (w) w->x=wr, w->y=wi;
|
xue@1
|
1308 double ddwindow=2*(wr*ddwr+dwr*dwr+wi*ddwi+dwi*dwi);
|
xue@1
|
1309 return ddwindow;
|
xue@1
|
1310 }//ddWindowDuo
|
xue@1
|
1311
|
Chris@5
|
1312 /**
|
xue@1
|
1313 function sIPWindowDuo: calculates the square norm of the orthogonal projection of a windowed spectrum
|
xue@1
|
1314 onto the linear span of the windowed spectra of two sinusoids at reference frequencies f1 and f2.
|
xue@1
|
1315
|
xue@1
|
1316 In: x[N]: spectrum
|
xue@1
|
1317 f1, f2: reference frequencies.
|
xue@1
|
1318 M, c[], d[], iH2: cosine-family window specification parameters.
|
xue@1
|
1319 K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.
|
xue@1
|
1320 Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors
|
xue@1
|
1321
|
xue@1
|
1322 Returns the square norm of the orthogonal projection.
|
xue@1
|
1323 */
|
xue@1
|
1324 double sIPWindowDuo(double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2)
|
xue@1
|
1325 {
|
xue@1
|
1326 int K=K2-K1+1;
|
xue@1
|
1327 cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
|
xue@1
|
1328 Window(w1, f1, N, M, c, K1, K2);
|
xue@1
|
1329 double w1w1=0;
|
xue@1
|
1330 for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
|
xue@1
|
1331 for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];
|
xue@1
|
1332 Window(w1, f2, N, M, c, K1, K2);
|
xue@1
|
1333 cdouble r1w2=0, w12; for (int k=0; k<K; k++) r1w2+=(r1[k]^w1[k]);
|
xue@1
|
1334 double w=WindowDuo(f1-f2, N, d, M, &w12);
|
xue@1
|
1335 double v=1.0/iH2-w*iH2;
|
xue@1
|
1336 double result=~xw1/w1w1+~r1w2/v;
|
xue@1
|
1337 cdouble mu2=r1w2/v;
|
xue@1
|
1338 lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;
|
xue@1
|
1339 delete[] w1;
|
xue@1
|
1340 return result;
|
xue@1
|
1341 }//sIPWindowDuo
|
xue@1
|
1342 //wrapper function
|
xue@1
|
1343 double sIPWindowDuo(double f2, void* params)
|
xue@1
|
1344 {
|
xue@1
|
1345 struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
1346 cdouble r1, r2;
|
xue@1
|
1347 return sIPWindowDuo(p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
|
xue@1
|
1348 }//sIPWindowDuo
|
xue@1
|
1349
|
Chris@5
|
1350 /**
|
xue@1
|
1351 function ddsIPWindowDuo: calculates the square norm, and its 1st and 2nd derivatives against f2,, of
|
xue@1
|
1352 the orthogonal projection of a windowed spectrum onto the linear span of the windowed spectra of two
|
xue@1
|
1353 sinusoids at reference frequencies f1 and f2.
|
xue@1
|
1354
|
xue@1
|
1355 In: x[N]: spectrum
|
xue@1
|
1356 f1, f2: reference frequencies.
|
xue@1
|
1357 M, c[], d[], iH2: cosine-family window specification parameters.
|
xue@1
|
1358 K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.
|
xue@1
|
1359
|
xue@1
|
1360 Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors
|
xue@1
|
1361 ddsip[3]: the 2nd, 1st and 0th derivatives (against f2) of the square norm.
|
xue@1
|
1362
|
xue@1
|
1363 No return value.
|
xue@1
|
1364 */
|
xue@1
|
1365 void ddsIPWindowDuo(double* ddsip2, double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2)
|
xue@1
|
1366 {
|
xue@1
|
1367 int K=K2-K1+1;
|
xue@1
|
1368 cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
|
xue@1
|
1369 Window(w1, f1, N, M, c, K1, K2);
|
xue@1
|
1370 double w1w1=0;
|
xue@1
|
1371 for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
|
xue@1
|
1372 for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];
|
xue@1
|
1373
|
xue@1
|
1374 cdouble r1w2, w12;
|
xue@1
|
1375 double u, du, ddu=ddsIPWindow_unn(f2, &r1[-K1], N, M, c, K1, K2, du, u, &r1w2);
|
xue@1
|
1376 double w, dw, ddw=ddWindowDuo(f1-f2, N, d, M, dw, w, &w12); dw=-dw;
|
xue@1
|
1377 double v=1.0/iH2-w*iH2, dv=-iH2*dw, ddv=-iH2*ddw;
|
xue@1
|
1378 double iv=1.0/v;//, div=-dv*iv*iv, ddiv=(2*dv*dv-v*ddv)*iv*iv*iv;
|
xue@1
|
1379
|
xue@1
|
1380 ddsip2[2]=~xw1/w1w1+u*iv;
|
xue@1
|
1381 ddsip2[1]=iv*(du-iv*u*dv);
|
xue@1
|
1382 ddsip2[0]=iv*(ddu-iv*(u*ddv+2*du*dv-2*iv*u*dv*dv));
|
xue@1
|
1383
|
xue@1
|
1384 cdouble mu2=r1w2*iv;
|
xue@1
|
1385 lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;
|
xue@1
|
1386
|
xue@1
|
1387 delete[] w1;
|
xue@1
|
1388 }//ddsIPWindowDuo
|
xue@1
|
1389 //wrapper function
|
xue@1
|
1390 double ddsIPWindowDuo(double f2, void* params)
|
xue@1
|
1391 {
|
xue@1
|
1392 struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params;
|
xue@1
|
1393 double ddsip2[3]; cdouble r1, r2;
|
xue@1
|
1394 ddsIPWindowDuo(ddsip2, p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
|
xue@1
|
1395 p->dipwindow=ddsip2[1], p->ipwindow=ddsip2[2];
|
xue@1
|
1396 return ddsip2[0];
|
xue@1
|
1397 }//ddsIPWindowDuo
|
xue@1
|
1398
|
Chris@5
|
1399 /**
|
xue@1
|
1400 function LSEDuo: least-square estimation of two sinusoids of which one has a fixed frequency
|
xue@1
|
1401
|
xue@1
|
1402 In: x[N]: the windowed spectrum
|
xue@1
|
1403 f1: the fixed frequency
|
xue@1
|
1404 f2: initial value of the flexible frequency
|
xue@1
|
1405 fmin, fmax: search range for f2, the flexible frequency
|
xue@1
|
1406 B: spectral truncation half width
|
xue@1
|
1407 M, c[], d[], iH2:
|
xue@1
|
1408 epf: frequency error tolerance
|
xue@1
|
1409 Out: f2: frequency estimate
|
xue@1
|
1410 lmd1, lmd2: amplitude-phase factor estimates
|
xue@1
|
1411 Returns 1 if managed to find a good f2, 0 if not, upon which the initial f2 is used for estimating
|
xue@1
|
1412
|
xue@1
|
1413 amplitudes and phase angles.
|
xue@1
|
1414 */
|
xue@1
|
1415 int LSEDuo(double& f2, double fmin, double fmax, double f1, cdouble* x, int N, double B, double* c, double* d, int M, double iH2, cdouble& r1, cdouble &r2, double epf)
|
xue@1
|
1416 {
|
xue@1
|
1417 int result=0;
|
xue@1
|
1418 double inp=f2;
|
xue@1
|
1419 int k1=ceil(inp-B); if (k1<0) k1=0;
|
xue@1
|
1420 int k2=floor(inp+B); if (k2>=N/2) k2=N/2-1;
|
xue@1
|
1421 struct l_hx {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} p={N, k1, k2, c, d, M, iH2, x, f1, 0, 0};
|
Chris@3
|
1422 int dfshift=offsetof(l_hx, dipwindow);// fshift=int(&((l_hx*)0)->ipwindow);
|
xue@1
|
1423
|
xue@1
|
1424 double tmp=Newton(f2, ddsIPWindowDuo, &p, dfshift, epf, 100, 1e-256, fmin, fmax);
|
xue@1
|
1425 if (tmp!=-1 && f2>fmin && f2<fmax) result=1;
|
xue@1
|
1426 else
|
xue@1
|
1427 {
|
xue@1
|
1428 Search1DmaxEx(f2, &p, sIPWindowDuo, fmin, fmax, NULL, epf);
|
xue@1
|
1429 if (f2<=fmin || f2>=fmax) f2=inp;
|
xue@1
|
1430 else result=1;
|
xue@1
|
1431 }
|
xue@1
|
1432 sIPWindowDuo(f1, f2, x, N, c, d, M, iH2, k1, k2, r1, r2);
|
xue@1
|
1433 return result;
|
xue@1
|
1434 }//LSEDuo
|
xue@1
|
1435
|
xue@1
|
1436 //---------------------------------------------------------------------------
|
xue@1
|
1437 /*
|
xue@1
|
1438 Time-frequency reassignment sinusoid estimation routines.
|
xue@1
|
1439
|
xue@1
|
1440 Further reading: A. R?bel, ¡°Estimating partial frequency and frequency slope using reassignment
|
xue@1
|
1441 operators,¡± in Proc. ICMC¡¯02. G?teborg. 2002.
|
xue@1
|
1442 */
|
xue@1
|
1443
|
Chris@5
|
1444 /**
|
xue@1
|
1445 function CDFTW: single-frequency windowed DTFT, centre-aligned
|
xue@1
|
1446
|
xue@1
|
1447 In: data[Wid]: waveform data x
|
xue@1
|
1448 win[Wid+1]: window function
|
xue@1
|
1449 k: frequency, in bins, where bin=1/Wid
|
xue@1
|
1450 Out: X: DTFT of xw at frequency k bins
|
xue@1
|
1451
|
xue@1
|
1452 No return value.
|
xue@1
|
1453 */
|
xue@1
|
1454 void CDFTW(cdouble& X, double k, int Wid, cdouble* data, double* win)
|
xue@1
|
1455 {
|
xue@1
|
1456 X=0;
|
xue@1
|
1457 int hWid=Wid/2;
|
xue@1
|
1458 for (int i=0; i<Wid; i++)
|
xue@1
|
1459 {
|
xue@1
|
1460 cdouble tmp=data[i]*win[Wid-i];
|
xue@1
|
1461 double ph=-2*M_PI*(i-hWid)*k/Wid;
|
xue@1
|
1462 tmp.rotate(ph);
|
xue@1
|
1463 X+=tmp;
|
xue@1
|
1464 }
|
xue@1
|
1465 }//CDFTW
|
xue@1
|
1466
|
Chris@5
|
1467 /**
|
xue@1
|
1468 function CuDFTW: single-frequency windowed DTFT of t*data[t], centre-aligned
|
xue@1
|
1469
|
xue@1
|
1470 In: data[Wid]: waveform data x
|
xue@1
|
1471 wid[Wid+1]: window function
|
xue@1
|
1472 k: frequency, in bins
|
xue@1
|
1473 Out: X: DTFT of txw at frequency k bins
|
xue@1
|
1474
|
xue@1
|
1475 No return value.
|
xue@1
|
1476 */
|
xue@1
|
1477 void CuDFTW(cdouble& X, int k, int Wid, cdouble* data, double* win)
|
xue@1
|
1478 {
|
xue@1
|
1479 X=0;
|
xue@1
|
1480 int hWid=Wid/2;
|
xue@1
|
1481 for (int i=0; i<Wid; i++)
|
xue@1
|
1482 {
|
xue@1
|
1483 double tw=((i-hWid)*win[Wid-i]);
|
xue@1
|
1484 cdouble tmp=data[i]*tw;
|
xue@1
|
1485 double ph=-2*M_PI*(i-hWid)*k/Wid;
|
xue@1
|
1486 tmp.rotate(ph);
|
xue@1
|
1487 X+=tmp;
|
xue@1
|
1488 }
|
xue@1
|
1489 }//CuDFTW
|
xue@1
|
1490
|
Chris@5
|
1491 /**
|
xue@1
|
1492 function TFReas: time-frequency reassignment
|
xue@1
|
1493
|
xue@1
|
1494 In: data[Wid]: waveform data
|
xue@1
|
1495 win[Wid+1], dwin[Wid+1], ddwin[Wid+1]: window function and its derivatives
|
xue@1
|
1496 f, t: initial digital frequency and time
|
xue@1
|
1497 Out: f, t: reassigned digital frequency and time
|
xue@1
|
1498 fslope: estimate of frequency derivative
|
xue@1
|
1499 plogaslope[0]: estimate of the derivative of logarithmic amplitude, optional
|
xue@1
|
1500
|
xue@1
|
1501 No return value.
|
xue@1
|
1502 */
|
xue@1
|
1503 void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope)
|
xue@1
|
1504 {
|
xue@1
|
1505 int fi=floor(f*Wid+0.5);
|
xue@1
|
1506
|
xue@1
|
1507 cdouble x, xt, xw;
|
xue@1
|
1508 CDFTW(x, fi, Wid, data, win);
|
xue@1
|
1509 CuDFTW(xw, fi, Wid, data, win); xt.x=xw.y; xw.y=-xw.x; xw.x=xt.x;
|
xue@1
|
1510 CDFTW(xt, fi, Wid, data, dwin);
|
xue@1
|
1511 double px=~x;
|
xue@1
|
1512 t=t-(xw.y*x.x-xw.x*x.y)/px;
|
xue@1
|
1513 f=1.0*fi/Wid+(xt.y*x.x-xt.x*x.y)/px/(2*M_PI);
|
xue@1
|
1514 if (plogaslope) plogaslope[0]=-(xt.x*x.x+xt.y*x.y)/px;
|
xue@1
|
1515 cdouble xtt, xtw;
|
xue@1
|
1516 CuDFTW(xtw, fi, Wid, data, dwin); xtt.x=xtw.y; xtw.y=-xtw.x; xtw.x=xtt.x;
|
xue@1
|
1517 CDFTW(xtt, fi, Wid, data, ddwin);
|
xue@1
|
1518 double dtdt=-(xtw.y*x.x-xtw.x*x.y)/px+((xt.y*x.x-xt.x*x.y)*(xw.x*x.x+xw.y*x.y)+(xt.x*x.x+xt.y*x.y)*(xw.y*x.x-xw.x*x.y))/px/px,
|
xue@1
|
1519 dwdt=(xtt.y*x.x-xtt.x*x.y)/px-2*(xt.x*x.x+xt.y*x.y)*(xt.y*x.x-xt.x*x.y)/px/px;
|
xue@1
|
1520 if (dtdt!=0) fslope=dwdt/dtdt/(2*M_PI);
|
xue@1
|
1521 else fslope=0;
|
xue@1
|
1522 } //TFReas*/
|
xue@1
|
1523
|
Chris@5
|
1524 /**
|
xue@1
|
1525 function TFReas: sinusoid estimation using reassignment method
|
xue@1
|
1526
|
xue@1
|
1527 In: data[Wid]: waveform data
|
xue@1
|
1528 w[Wid+1], dw[Wid+1], ddw[Wid+1]: window function and its derivatives
|
xue@1
|
1529 win[Wid]: window function used for estimating amplitude and phase by projection onto a chirp
|
xue@1
|
1530 t: time for which the parameters are estimated
|
xue@1
|
1531 f: initial frequency at t
|
xue@1
|
1532 Out: f, a, ph: digital frequency, amplitude and phase angle estimated at t
|
xue@1
|
1533 fslope: frequency derivative estimate
|
xue@1
|
1534
|
xue@1
|
1535 No return value.
|
xue@1
|
1536 */
|
xue@1
|
1537 void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win)
|
xue@1
|
1538 {
|
xue@1
|
1539 double localt=t, logaslope;
|
xue@1
|
1540 TFReas(f, localt, fslope, Wid, data, w, dw, ddw, &logaslope);
|
xue@1
|
1541
|
xue@1
|
1542 if (logaslope*Wid>6) logaslope=6.0/Wid;
|
xue@1
|
1543 else if (logaslope*Wid<-6) logaslope=-6.0/Wid;
|
xue@1
|
1544
|
xue@1
|
1545 f=f+fslope*(t-localt); //obtain frequency estimate at t
|
xue@1
|
1546
|
xue@1
|
1547 cdouble x=0;
|
xue@1
|
1548 if (win==0)
|
xue@1
|
1549 {
|
xue@1
|
1550 for (int n=0; n<Wid; n++)
|
xue@1
|
1551 {
|
xue@1
|
1552 double ni=n-t;
|
xue@1
|
1553 cdouble tmp=data[n];
|
xue@1
|
1554 double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
|
xue@1
|
1555 tmp.rotate(p);
|
xue@1
|
1556 x+=tmp;
|
xue@1
|
1557 }
|
xue@1
|
1558 a=abs(x)/Wid;
|
xue@1
|
1559 }
|
xue@1
|
1560 else
|
xue@1
|
1561 {
|
xue@1
|
1562 double sumwin=0;
|
xue@1
|
1563 for (int n=0; n<Wid; n++)
|
xue@1
|
1564 {
|
xue@1
|
1565 double ni=n-t;
|
xue@1
|
1566 cdouble tmp=data[n]*win[n];
|
xue@1
|
1567 double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
|
xue@1
|
1568 tmp.rotate(p);
|
xue@1
|
1569 x+=tmp; sumwin+=win[n];
|
xue@1
|
1570 }
|
xue@1
|
1571 a=abs(x)/sumwin;
|
xue@1
|
1572 }
|
xue@1
|
1573 ph=arg(x);
|
xue@1
|
1574 }//TFReas
|
xue@1
|
1575
|
xue@1
|
1576 //---------------------------------------------------------------------------
|
xue@1
|
1577 /*
|
xue@1
|
1578 Routines for additive and multiplicative reestimation of sinusoids.
|
xue@1
|
1579
|
xue@1
|
1580 Further reading: Wen X. and M. Sandler, "Additive and multiplicative reestimation schemes
|
xue@1
|
1581 for the sinusoid modeling of audio," in Proc. EUSIPCO'09, Glasgow, 2009.
|
xue@1
|
1582 */
|
xue@1
|
1583
|
Chris@5
|
1584 /**
|
xue@1
|
1585 function AdditiveUpdate: additive reestimation of time-varying sinusoid
|
xue@1
|
1586
|
xue@1
|
1587 In: x[Count]: waveform data
|
xue@1
|
1588 Wid, Offst: frame size and hop
|
xue@1
|
1589 fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
|
xue@1
|
1590 das[Count]: initial estimate of amplitude derivative
|
xue@1
|
1591 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1592 LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
|
xue@1
|
1593 Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update
|
xue@1
|
1594
|
xue@1
|
1595 No return value.
|
xue@1
|
1596 */
|
xue@1
|
1597 void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
|
xue@1
|
1598 {
|
xue@1
|
1599 int HWid=Wid/2, Fr=(Count-Wid)/Offst+1;
|
xue@1
|
1600
|
xue@1
|
1601 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
1602 {
|
xue@1
|
1603 int i=HWid+Offst*fr;
|
xue@1
|
1604 if (fs[i]<0 || fs[i]>0.5){}
|
xue@1
|
1605 }
|
xue@1
|
1606
|
xue@1
|
1607 cdouble *y=new cdouble[Count];
|
xue@1
|
1608 double *lf=new double[Count*4], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3];
|
xue@1
|
1609
|
xue@1
|
1610 __int16* ref=new __int16[Count];
|
xue@1
|
1611 for (int i=0; i<Count; i++) y[i]=x[i].x-as[i]*cos(phs[i]), ref[i]=floor(fs[i]*Wid+0.5);
|
xue@1
|
1612 memcpy(lf, fs, sizeof(double)*Count);
|
xue@1
|
1613 BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, ref, reserved, LogA);
|
xue@1
|
1614
|
xue@1
|
1615 //merge and interpolate
|
xue@1
|
1616 double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
|
xue@1
|
1617 *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
|
xue@1
|
1618 *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
|
xue@1
|
1619 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
1620 {
|
xue@1
|
1621 int i=HWid+Offst*fr;
|
xue@1
|
1622 double a=as[i], b=la[i], fai=phs[i], thet=lp[i], f=fs[i], g=lf[i], delt=fai-thet, da=das[i], db=lda[i];
|
xue@1
|
1623 xs[fr]=i;
|
xue@1
|
1624 if (fabs(f-g)*Wid>1)
|
xue@1
|
1625 {
|
xue@1
|
1626 afr[fr]=a, pfr[fr]=fai, ffr[fr]=f;
|
xue@1
|
1627 }
|
xue@1
|
1628 else
|
xue@1
|
1629 {
|
xue@1
|
1630 double rr=a*cos(fai)+b*cos(thet);
|
xue@1
|
1631 double ii=a*sin(fai)+b*sin(thet);
|
xue@1
|
1632 ffr[fr]=(a*f*(a+b*cos(delt))+b*g*(b+a*cos(delt))+(da*b-a*db)*sin(delt)/(2*M_PI))/(a*a+b*b+2*a*b*cos(delt));
|
xue@1
|
1633 afr[fr]=sqrt(rr*rr+ii*ii);
|
xue@1
|
1634 pfr[fr]=atan2(ii, rr);
|
xue@1
|
1635 }
|
xue@1
|
1636 if (LogA) afr[fr]=log(afr[fr]);
|
xue@1
|
1637 }
|
xue@1
|
1638 CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
|
xue@1
|
1639 CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
|
xue@7
|
1640 for (int fr=0; fr<Fr-1; fr++) Sinusoid(Offst, &fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
|
xue@7
|
1641 double tmpph=pfr[0]; Sinusoid_direct(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], tmpph, LogA);
|
xue@7
|
1642 ShiftTrinomial(xs[Fr-1]-xs[Fr-2], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
|
xue@7
|
1643 ShiftTrinomial(xs[Fr-1]-xs[Fr-2], aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2]);
|
xue@7
|
1644 tmpph=pfr[Fr-1]; Sinusoid_direct(&fs[int(xs[Fr-1])], &as[int(xs[Fr-1])], &phs[int(xs[Fr-1])], &das[int(xs[Fr-1])], 0, HWid, aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph, LogA);
|
xue@1
|
1645 delete[] fa; //*/
|
xue@1
|
1646 /*
|
xue@1
|
1647 for (int i=0; i<Count; i++)
|
xue@1
|
1648 {
|
xue@1
|
1649 double rr=as[i]*cos(phs[i])+la[i]*cos(lp[i]);
|
xue@1
|
1650 double ii=as[i]*sin(phs[i])+la[i]*sin(lp[i]);
|
xue@1
|
1651 as[i]=sqrt(rr*rr+ii*ii);
|
xue@1
|
1652 phs[i]=atan2(ii, rr);
|
xue@1
|
1653 } //*/
|
xue@1
|
1654 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
1655 {
|
xue@1
|
1656 int i=HWid+Offst*fr;
|
xue@1
|
1657 if (fs[i]<0 || fs[i]>0.5){}
|
xue@1
|
1658 }
|
xue@1
|
1659 delete[] y; delete[] lf; delete[] ref;
|
xue@1
|
1660 }//AdditiveUpdate
|
xue@1
|
1661
|
Chris@5
|
1662 /**
|
xue@1
|
1663 function AdditiveAnalyzer: sinusoid analyzer with one additive update
|
xue@1
|
1664
|
xue@1
|
1665 In: x[Count]: waveform data
|
xue@1
|
1666 Wid, Offst: frame size and hop size
|
xue@1
|
1667 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1668 ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
|
xue@1
|
1669 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1670 LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
|
xue@1
|
1671 Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
|
xue@1
|
1672 das[Count]: estimate of amplitude derivative
|
xue@1
|
1673
|
xue@1
|
1674 No return value.
|
xue@1
|
1675 */
|
xue@1
|
1676 void AdditiveAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
|
xue@1
|
1677 {
|
xue@1
|
1678 BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
|
xue@1
|
1679 AdditiveUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved, LogA);
|
xue@1
|
1680 }//AdditiveAnalyzer
|
xue@1
|
1681
|
Chris@5
|
1682 /**
|
xue@1
|
1683 function MultiplicativeUpdate: multiplicative reestimation of time-varying sinusoid
|
xue@1
|
1684
|
xue@1
|
1685 In: x[Count]: waveform data
|
xue@1
|
1686 Wid, Offst: frame size and hop
|
xue@1
|
1687 fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
|
xue@1
|
1688 das[Count]: initial estimate of amplitude derivative
|
xue@1
|
1689 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1690 LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
|
xue@1
|
1691 Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update
|
xue@1
|
1692
|
xue@1
|
1693 No return value.
|
xue@1
|
1694 */
|
xue@1
|
1695 void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
|
xue@1
|
1696 {
|
xue@1
|
1697 int HWid=Wid/2;
|
xue@1
|
1698 cdouble *y=new cdouble[Count];
|
xue@1
|
1699 double *lf=new double[Count*8], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3],
|
xue@1
|
1700 *lf2=&lf[Count*4], *la2=&lf2[Count], *lp2=&lf2[Count*2], *lda2=&lf2[Count*3];
|
xue@1
|
1701 __int16 *lref=new __int16[Count];
|
xue@1
|
1702
|
xue@1
|
1703 for (int i=0; i<Count; i++) y[i]=x[i]*(cdouble(1.0).rotate(-phs[i]+i*0.15*2*M_PI)),
|
xue@1
|
1704 lref[i]=0.15*Wid;
|
xue@1
|
1705 BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, lref, reserved, LogA);
|
xue@1
|
1706 for (int i=0; i<Count; i++) y[i]=y[i]*(cdouble(1.0/la[i]).rotate(-lp[i]+i*0.15*2*M_PI)), lref[i]=0.15*Wid;
|
xue@1
|
1707 BasicAnalyzer(lf2, la2, lp2, lda2, y, Count, Wid, Offst, lref, reserved, LogA);
|
xue@1
|
1708
|
xue@1
|
1709 /*
|
xue@1
|
1710 for (int i=0; i<Count; i++)
|
xue@1
|
1711 {
|
xue@1
|
1712 as[i]=la[i]*la2[i];
|
xue@1
|
1713 phs[i]=phs[i]+lp[i]+lp2[i]-0.3*2*M_PI*i;
|
xue@1
|
1714 fs[i]=fs[i]+lf[i]+lf2[i]-0.3;
|
xue@1
|
1715 } //*/
|
xue@1
|
1716
|
xue@1
|
1717 //merge
|
xue@1
|
1718 int Fr=(Count-Wid)/Offst+1;
|
xue@1
|
1719 double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
|
xue@1
|
1720 *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
|
xue@1
|
1721 *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
|
xue@1
|
1722 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
1723 {
|
xue@1
|
1724 int i=HWid+Offst*fr;
|
xue@1
|
1725 xs[fr]=i;
|
xue@1
|
1726 afr[fr]=la[i]*la2[i];
|
xue@1
|
1727 if (LogA) afr[fr]=log(afr[fr]);
|
xue@1
|
1728 ffr[fr]=fs[i]+lf[i]-0.15+lf2[i]-0.15;
|
xue@1
|
1729 pfr[fr]=phs[i]+lp[i]+lp2[i]-0.3*i*2*M_PI;
|
xue@1
|
1730 }
|
xue@1
|
1731 CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
|
xue@1
|
1732 CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
|
xue@7
|
1733 for (int fr=0; fr<Fr-1; fr++) Sinusoid(Offst, &fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
|
xue@7
|
1734 double tmpph=pfr[0]; Sinusoid_direct(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], tmpph, LogA);
|
xue@7
|
1735 ShiftTrinomial(xs[Fr-1]-xs[Fr-2], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
|
xue@7
|
1736 ShiftTrinomial(xs[Fr-1]-xs[Fr-2], aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2]);
|
xue@7
|
1737 tmpph=pfr[Fr-1]; Sinusoid_direct(&fs[int(xs[Fr-1])], &as[int(xs[Fr-1])], &phs[int(xs[Fr-1])], &das[int(xs[Fr-1])], 0, HWid, aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph, LogA);
|
xue@7
|
1738
|
xue@7
|
1739 delete[] fa; //*/
|
xue@1
|
1740
|
xue@1
|
1741 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
1742 {
|
xue@1
|
1743 int i=HWid+Offst*fr;
|
xue@1
|
1744 if (fs[i]<0 || fs[i]>0.5){}
|
xue@1
|
1745 }
|
xue@1
|
1746
|
xue@1
|
1747 delete[] y; delete[] lf; delete[] lref;
|
xue@1
|
1748 }//MultiplicativeUpdate
|
xue@1
|
1749
|
Chris@5
|
1750 /**
|
xue@1
|
1751 function MultiplicativeAnalyzer: sinusoid analyzer with one multiplicative update
|
xue@1
|
1752
|
xue@1
|
1753 In: x[Count]: waveform data
|
xue@1
|
1754 Wid, Offst: frame size and hop size
|
xue@1
|
1755 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1756 ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
|
xue@1
|
1757 BasicAnalyzer: pointer to a sinusoid analyzer
|
xue@1
|
1758 LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
|
xue@1
|
1759 Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
|
xue@1
|
1760 das[Count]: estimate of amplitude derivative
|
xue@1
|
1761
|
xue@1
|
1762 No return value.
|
xue@1
|
1763 */
|
xue@1
|
1764 void MultiplicativeAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
|
xue@1
|
1765 {
|
xue@1
|
1766 BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
|
xue@1
|
1767 MultiplicativeUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved);
|
xue@1
|
1768 }//MultiplicativeAnalyzer
|
xue@1
|
1769
|
xue@1
|
1770 /*
|
xue@1
|
1771 This is an earlier version of the multiplicative method without using a user-provided BasicAnalyzer.
|
xue@1
|
1772 This updates the sinusoid estimates at the selected consecutive FRAMES of x. Only frequency modulation
|
xue@1
|
1773 is included in the multiplier. The first frame (0) is centred at x[Wid/2]. fs, as, and phs are based
|
xue@1
|
1774 on frames rather than samples. Updates include frame frst, but not frame fren.
|
xue@1
|
1775 */
|
xue@1
|
1776 void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst)
|
xue@1
|
1777 {
|
xue@1
|
1778 int HWid=Wid/2;
|
xue@1
|
1779
|
xue@1
|
1780 double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
|
xue@1
|
1781 *xs=&fa[Fr*8];
|
xue@1
|
1782 for (int fr=0; fr<Fr; fr++) xs[fr]=HWid+Offst*fr;
|
xue@1
|
1783 CubicSpline(Fr-1, fa, fb, fc, fd, xs, fs, 1, 1);
|
xue@1
|
1784
|
xue@1
|
1785 int dst=Offst*frst, den=Offst*(fren-1)+Wid, dcount=den-dst;
|
xue@1
|
1786 double *f=new double[dcount*2], *ph=&f[dcount];
|
xue@7
|
1787 for (int fr=frst; fr<fren-1; fr++) Sinusoid(Offst, &f[int(xs[fr])-dst], &ph[int(xs[fr])-dst], fa[fr], fb[fr], fc[fr], fd[fr], phs[fr], phs[fr+1]);
|
xue@7
|
1788 if (frst==0)
|
xue@7
|
1789 {
|
xue@7
|
1790 double tmpph=phs[0];
|
xue@7
|
1791 Sinusoid_direct(&f[int(xs[0])-dst], &ph[int(xs[0])-dst], -HWid, 0, fa[0], fb[0], fc[0], fd[0], tmpph);
|
xue@7
|
1792 }
|
xue@7
|
1793 else
|
xue@7
|
1794 Sinusoid(Offst, &f[int(xs[frst-1])-dst], &ph[int(xs[frst-1])-dst], fa[frst-1], fb[frst-1], fc[frst-1], fd[frst-1], phs[frst-1], phs[frst]);
|
xue@7
|
1795 if (fren==Fr)
|
xue@7
|
1796 {
|
xue@7
|
1797 double tmpph=phs[Fr-1];
|
xue@7
|
1798 ShiftTrinomial(Offst, fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
|
xue@7
|
1799 Sinusoid_direct(&f[int(xs[Fr-1])-dst], &ph[int(xs[Fr-1])-dst], 0, HWid, fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph);
|
xue@7
|
1800 }
|
xue@7
|
1801 else
|
xue@7
|
1802 Sinusoid(Offst, &f[int(xs[fren-1])-dst], &ph[int(xs[fren-1])-dst], fa[fren-1], fb[fren-1], fc[fren-1], fd[fren-1], phs[fren-1], phs[fren]);
|
xue@1
|
1803
|
xue@1
|
1804 cdouble* y=new cdouble[Wid];
|
xue@1
|
1805 AllocateFFTBuffer(Wid, Amp, W, X);
|
xue@1
|
1806 double* win=NewWindow(wtHann, Wid);
|
xue@1
|
1807 int M; double c[10], iH2; windowspec(wtHann, Wid, &M, c, &iH2);
|
xue@1
|
1808 for (int fr=frst; fr<fren; fr++)
|
xue@1
|
1809 {
|
xue@1
|
1810 __int16* lx=&x[Offst*fr];
|
xue@1
|
1811 double* lph=&ph[Offst*(fr-frst)];
|
xue@1
|
1812 for (int i=0; i<Wid; i++) y[i]=cdouble(lx[i]).rotate(-lph[i]+i*0.15*2*M_PI);
|
xue@11
|
1813 CFFTCW(y, win, Amp, 0, Log2(Wid), W, X);
|
xue@1
|
1814 int pf=0.15*Wid, mpf=pf;
|
xue@1
|
1815 for (int k=pf-4; k<=pf+4; k++) if (Amp[k]>Amp[mpf]) mpf=k;
|
xue@1
|
1816 if (mpf>pf-4 && mpf<pf+4) pf=mpf;
|
xue@1
|
1817 double lfs=pf, lphs;
|
xue@1
|
1818 LSESinusoid(lfs, pf-3, pf+3, X, Wid, 3, M, c, iH2, as[fr], lphs, 1e-3);
|
xue@1
|
1819 fs[fr]=fs[fr]+lfs/Wid-0.15;
|
xue@1
|
1820 phs[fr]+=lphs-0.15*Wid*M_PI;
|
xue@1
|
1821 as[fr]*=2;
|
xue@1
|
1822 }
|
xue@1
|
1823
|
xue@1
|
1824 delete[] y;
|
xue@1
|
1825 delete[] f;
|
xue@1
|
1826 delete[] win;
|
xue@1
|
1827 delete[] fa;
|
xue@1
|
1828 FreeFFTBuffer(Amp);
|
xue@1
|
1829 }//MultiplicativeUpdateF
|
xue@1
|
1830
|
xue@1
|
1831 //---------------------------------------------------------------------------
|
xue@1
|
1832 /*
|
xue@1
|
1833 Earlier reestimation method routines.
|
xue@1
|
1834
|
xue@1
|
1835 Further reading: Wen X. and M. Sandler, "Evaluating parameters of time-varying
|
xue@1
|
1836 sinusoids by demodulation," in Proc. DAFx'08, Espoo, 2008.
|
xue@1
|
1837 */
|
xue@1
|
1838
|
Chris@5
|
1839 /**
|
xue@1
|
1840 function ReEstFreq: sinusoid reestimation by demodulating frequency.
|
xue@1
|
1841
|
xue@1
|
1842 In: x[Wid+Offst*(FrCount-1)]: waveform data
|
xue@1
|
1843 FrCount, Wid, Offst: frame count, frame size and hop size
|
xue@1
|
1844 fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
|
xue@1
|
1845 win[Wid]: window function for estimating demodulated sinusoid
|
xue@1
|
1846 M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
|
xue@1
|
1847 Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, optional
|
xue@1
|
1848 w[Wid/2], ps[Wid], xs[Wid], xc[Wid], fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
|
xue@1
|
1849 Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
|
xue@1
|
1850
|
xue@1
|
1851 No return value.
|
xue@1
|
1852 */
|
xue@1
|
1853 void ReEstFreq(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* fa, double* fb, double* fc, double* fd, double* ns, int* Wids)
|
xue@1
|
1854 {
|
xue@1
|
1855 int hWid=Wid/2;
|
xue@1
|
1856 //reestimate using frequency track
|
xue@1
|
1857 CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
|
xue@1
|
1858 for (int fr=0; fr<FrCount; fr++)
|
xue@1
|
1859 {
|
xue@1
|
1860 //find ps
|
xue@1
|
1861 if (fr==0)
|
xue@1
|
1862 {
|
xue@1
|
1863 double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0];
|
xue@1
|
1864 for (int j=0; j<Wid; j++)
|
xue@1
|
1865 {
|
xue@1
|
1866 double lx=j-hWid;
|
xue@1
|
1867 ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
|
xue@1
|
1868 }
|
xue@1
|
1869 // memset(ps, 0, sizeof(double)*hWid);
|
xue@1
|
1870 }
|
xue@1
|
1871 else if (fr==FrCount-1)
|
xue@1
|
1872 {
|
xue@1
|
1873 int lfr=FrCount-2;
|
xue@1
|
1874 double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
1875 double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
|
xue@1
|
1876 ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
|
xue@1
|
1877 for (int j=1; j<Wid; j++)
|
xue@1
|
1878 {
|
xue@1
|
1879 ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
1880 }
|
xue@1
|
1881 // memset(&ps[hWid], 0, sizeof(double)*hWid);
|
xue@1
|
1882 }
|
xue@1
|
1883 else
|
xue@1
|
1884 {
|
xue@1
|
1885 int lfr=fr-1;
|
xue@1
|
1886 double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
1887 ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
|
xue@1
|
1888 for (int j=1; j<hWid+1; j++)
|
xue@1
|
1889 {
|
xue@1
|
1890 ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
1891 }
|
xue@1
|
1892 lfr=fr;
|
xue@1
|
1893 lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
1894 for (int j=1; j<hWid; j++)
|
xue@1
|
1895 {
|
xue@1
|
1896 ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
1897 }
|
xue@1
|
1898 }
|
xue@1
|
1899 double* ldata=&x[fr*Offst];
|
xue@1
|
1900 for (int j=0; j<Wid; j++)
|
xue@1
|
1901 {
|
xue@1
|
1902 xs[j].x=ldata[j]*cos(-ps[j]);
|
xue@1
|
1903 xs[j].y=ldata[j]*sin(-ps[j]);
|
xue@1
|
1904 }
|
xue@1
|
1905
|
xue@1
|
1906 if (Wids)
|
xue@1
|
1907 {
|
xue@1
|
1908 int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
|
xue@1
|
1909 SetTwiddleFactors(lWid, w);
|
xue@1
|
1910 double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
|
xue@1
|
1911 windowspec(wtHann, lWid, &lM, lc, &liH2);
|
xue@11
|
1912 CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, Log2(lWid), w, xc);
|
xue@1
|
1913 delete[] lwin;
|
xue@1
|
1914 double lf=fbuf[fr]*lWid, la, lp;
|
xue@1
|
1915 LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
|
xue@1
|
1916 if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
|
xue@1
|
1917 }
|
xue@1
|
1918 else
|
xue@1
|
1919 {
|
xue@11
|
1920 CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
|
xue@1
|
1921 double lf=fbuf[fr]*Wid, la, lp;
|
xue@1
|
1922 LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
|
xue@1
|
1923 if (la*2>abuf[fr])
|
xue@1
|
1924 fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
|
xue@1
|
1925 }
|
xue@1
|
1926 }
|
xue@1
|
1927 }//ReEstFreq
|
xue@1
|
1928
|
Chris@5
|
1929 /**
|
xue@1
|
1930 function ReEstFreq_2: sinusoid reestimation by demodulating frequency. This is that same as ReEstFreq(...)
|
xue@1
|
1931 except that it calls Sinusoid(...) to synthesize the phase track used for demodulation and that it
|
xue@1
|
1932 does not allow variable window sizes for estimating demodulated sinusoid.
|
xue@1
|
1933
|
xue@1
|
1934 In: x[Wid+Offst*(FrCount-1)]: waveform data
|
xue@1
|
1935 FrCount, Wid, Offst: frame count, frame size and hop size
|
xue@1
|
1936 fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
|
xue@1
|
1937 win[Wid]: window function for LSE sinusoid estimation
|
xue@1
|
1938 M, c[], iH2: cosine-family window specification parameters, must be consistent with M, c, iH2
|
xue@1
|
1939 w[Wid/2], xs[Wid], xc[Wid], f3[FrCount-1], f2[FrCount-1], f1[FrCount-1], f0[FrCount-1]: buffers
|
xue@1
|
1940 Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
|
xue@1
|
1941
|
xue@1
|
1942 No return value.
|
xue@1
|
1943 */
|
xue@1
|
1944 void ReEstFreq_2(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* f3, double* f2, double* f1, double* f0, double* ns)
|
xue@1
|
1945 {
|
xue@1
|
1946 int hWid=Wid/2;
|
xue@1
|
1947 //reestimate using frequency track
|
xue@1
|
1948 CubicSpline(FrCount-1, f3, f2, f1, f0, ns, fbuf, 1, 1);
|
xue@1
|
1949 double *refcos=(double*)malloc8(sizeof(double)*Wid), *refsin=&refcos[hWid], ph=0, centralph;
|
xue@1
|
1950
|
xue@1
|
1951 memset(f0, 0, sizeof(double)*FrCount);
|
xue@1
|
1952
|
xue@1
|
1953 int N=Wid+Offst*(FrCount-1);
|
xue@1
|
1954 double* cosine=new double[N], *sine=new double[N];
|
xue@7
|
1955 CosSin(&cosine[hWid], &sine[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
|
xue@1
|
1956 for (int fr=0; fr<FrCount-1; fr++)
|
xue@1
|
1957 {
|
xue@1
|
1958 int ncentre=hWid+Offst*fr;
|
xue@7
|
1959 if (fr==FrCount-2) CosSin(&cosine[ncentre], &sine[ncentre], 0, Wid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
|
xue@7
|
1960 else CosSin(&cosine[ncentre], &sine[ncentre], 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
|
xue@1
|
1961 }
|
xue@1
|
1962 double err=0;
|
xue@1
|
1963 for (int n=0; n<N; n++) {double tmp=cosine[n]-x[n-hWid]; err+=tmp*tmp; tmp=cosine[n]*cosine[n]+sine[n]*sine[n]-1; err+=tmp*tmp;}
|
xue@1
|
1964
|
xue@1
|
1965 ph=0;
|
xue@1
|
1966 for (int fr=0; fr<FrCount; fr++)
|
xue@1
|
1967 {
|
xue@1
|
1968 double* ldata=&x[fr*Offst-hWid];
|
xue@1
|
1969
|
xue@1
|
1970 //store first half of demodulated frame to xs[0:hWid-1]
|
xue@1
|
1971 if (fr==0)
|
xue@1
|
1972 {
|
xue@7
|
1973 CosSin(&refcos[hWid], &refsin[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
|
xue@1
|
1974 for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
|
xue@1
|
1975 }
|
xue@1
|
1976 else
|
xue@1
|
1977 {
|
xue@1
|
1978 ph=0;
|
xue@7
|
1979 CosSin(refcos, refsin, 0, hWid, f3[fr-1], f2[fr-1], f1[fr-1], f0[fr-1], ph);
|
xue@1
|
1980 for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
|
xue@1
|
1981 }
|
xue@1
|
1982
|
xue@1
|
1983 //taking care of phase angles
|
xue@1
|
1984 if (fr==FrCount-1) {double tmp=ph; ph=centralph; centralph=tmp;}
|
xue@1
|
1985 else centralph=ph;
|
xue@1
|
1986
|
xue@1
|
1987 double *lrefcos=&refcos[-hWid], *lrefsin=&refsin[-hWid];
|
xue@1
|
1988 //store second half of demodulated frame to xs[hWid:Wid-1]
|
xue@1
|
1989 if (fr==FrCount-1)
|
xue@1
|
1990 {
|
xue@7
|
1991 CosSin(lrefcos, lrefsin, hWid, Wid, f3[FrCount-2], f2[FrCount-2], f1[FrCount-2], f0[FrCount-2], ph);
|
xue@1
|
1992 for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
|
xue@1
|
1993 }
|
xue@1
|
1994 else
|
xue@1
|
1995 {
|
xue@7
|
1996 CosSin(refcos, refsin, 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
|
xue@1
|
1997 for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
|
xue@1
|
1998 }
|
xue@1
|
1999
|
xue@11
|
2000 CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
|
xue@1
|
2001 double lf=fbuf[fr]*Wid, la, lp;
|
xue@1
|
2002 LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
|
xue@1
|
2003 if (la*2>abuf[fr])
|
xue@1
|
2004 fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp+centralph;
|
xue@1
|
2005 }
|
xue@1
|
2006 }//ReEstFreq_2
|
xue@1
|
2007
|
Chris@5
|
2008 /**
|
xue@1
|
2009 function ReEstFreqAmp: sinusoid reestimation by demodulating frequency and amplitude.
|
xue@1
|
2010
|
xue@1
|
2011 In: x[Wid+Offst*(FrCount-1)]: waveform data
|
xue@1
|
2012 FrCount, Wid, Offst: frame count, frame size and hop size
|
xue@1
|
2013 fbuf[FrCount], abuf[FrCount], ns[FrCount]: initial frequency and amplitude estiamtes and their
|
xue@1
|
2014 timing
|
xue@1
|
2015 win[Wid]: window function for estimating demodulated sinusoid
|
xue@1
|
2016 M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
|
xue@1
|
2017 Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
|
xue@1
|
2018 optional
|
xue@1
|
2019 w[Wid/2], ps[Wid], xs[Wid], xc[Wid]: buffers
|
xue@1
|
2020 fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
|
xue@1
|
2021 aa[FrCount-1], ab[FrCount-1], ac[FrCount-1], ad[FrCount-1]: buffers
|
xue@1
|
2022 Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
|
xue@1
|
2023
|
xue@1
|
2024 No return value.
|
xue@1
|
2025 */
|
xue@1
|
2026 void ReEstFreqAmp(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* as, double* fa, double* fb, double* fc, double* fd, double* aa, double* ab, double* ac, double* ad, double* ns, int* Wids)
|
xue@1
|
2027 {
|
xue@1
|
2028 int hWid=Wid/2;
|
xue@1
|
2029 //reestimate using amplitude and frequency track
|
xue@1
|
2030 CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
|
xue@1
|
2031 CubicSpline(FrCount-1, aa, ab, ac, ad, ns, abuf, 0, 1);
|
xue@1
|
2032 for (int fr=0; fr<FrCount; fr++)
|
xue@1
|
2033 {
|
xue@1
|
2034 if (fr==0)
|
xue@1
|
2035 {
|
xue@1
|
2036 double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0],
|
xue@1
|
2037 lad=ad[0], lac=ac[0], lab=ab[0], laa=aa[0];
|
xue@1
|
2038 for (int j=0; j<Wid; j++)
|
xue@1
|
2039 {
|
xue@1
|
2040 double lx=j-hWid;
|
xue@1
|
2041 ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
|
xue@1
|
2042 }
|
xue@1
|
2043 for (int j=0; j<Wid; j++)
|
xue@1
|
2044 {
|
xue@1
|
2045 double lx=j-hWid;
|
xue@1
|
2046 as[j]=lad+lx*(lac+lx*(lab+lx*laa));
|
xue@1
|
2047 }
|
xue@1
|
2048 }
|
xue@1
|
2049 else if (fr==FrCount-1)
|
xue@1
|
2050 {
|
xue@1
|
2051 int lfr=FrCount-2;
|
xue@1
|
2052 double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
2053 double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
|
xue@1
|
2054 double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
|
xue@1
|
2055 ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
|
xue@1
|
2056 for (int j=1; j<Wid; j++)
|
xue@1
|
2057 {
|
xue@1
|
2058 ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
2059 }
|
xue@1
|
2060 as[0]=ad[lfr];
|
xue@1
|
2061 for (int j=0; j<Wid; j++)
|
xue@1
|
2062 {
|
xue@1
|
2063 as[j]=lad+j*(lac+j*(lab+j*laa));
|
xue@1
|
2064 }
|
xue@1
|
2065 }
|
xue@1
|
2066 else
|
xue@1
|
2067 {
|
xue@1
|
2068 int lfr=fr-1;
|
xue@1
|
2069 double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
2070 double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
|
xue@1
|
2071 ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
|
xue@1
|
2072 for (int j=0; j<hWid+1; j++)
|
xue@1
|
2073 {
|
xue@1
|
2074 ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
2075 as[j]=lad+j*(lac+j*(lab+j*laa));
|
xue@1
|
2076 }
|
xue@1
|
2077 lfr=fr;
|
xue@1
|
2078 lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
|
xue@1
|
2079 lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
|
xue@1
|
2080 for (int j=1; j<hWid; j++)
|
xue@1
|
2081 {
|
xue@1
|
2082 ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
|
xue@1
|
2083 as[j+hWid]=lad+j*(lac+j*(lab+j*laa));
|
xue@1
|
2084 }
|
xue@1
|
2085 }
|
xue@1
|
2086 double *ldata=&x[fr*Offst];
|
xue@1
|
2087 for (int j=0; j<Wid; j++)
|
xue@1
|
2088 {
|
xue@1
|
2089 double tmp;
|
xue@1
|
2090 if ((fr==0 && j<hWid) || (fr==FrCount-1 && j>=hWid)) tmp=1;
|
xue@1
|
2091 else if (as[hWid]>100*as[j]) tmp=100;
|
xue@1
|
2092 else tmp=as[hWid]/as[j];
|
xue@1
|
2093 tmp=tmp*ldata[j];
|
xue@1
|
2094 xs[j].x=tmp*cos(-ps[j]);
|
xue@1
|
2095 xs[j].y=tmp*sin(-ps[j]);
|
xue@1
|
2096 }
|
xue@1
|
2097
|
xue@1
|
2098 if (Wids)
|
xue@1
|
2099 {
|
xue@1
|
2100 int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
|
xue@1
|
2101 SetTwiddleFactors(lWid, w);
|
xue@1
|
2102 double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
|
xue@1
|
2103 windowspec(wtHann, lWid, &lM, lc, &liH2);
|
xue@11
|
2104 CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, Log2(lWid), w, xc);
|
xue@1
|
2105 delete[] lwin;
|
xue@1
|
2106 double lf=fbuf[fr]*lWid, la, lp;
|
xue@1
|
2107 LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
|
xue@1
|
2108 if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
|
xue@1
|
2109 }
|
xue@1
|
2110 else
|
xue@1
|
2111 {
|
xue@11
|
2112 CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
|
xue@1
|
2113 double lf=fbuf[fr]*Wid, la, lp;
|
xue@1
|
2114 LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
|
xue@1
|
2115 if (la*2>abuf[fr]) fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
|
xue@1
|
2116 }
|
xue@1
|
2117 }
|
xue@1
|
2118 }//ReEstFreqAmp
|
xue@1
|
2119
|
Chris@5
|
2120 /**
|
xue@1
|
2121 function Reestimate2: iterative demodulation method for sinusoid parameter reestimation.
|
xue@1
|
2122
|
xue@1
|
2123 In: x[(FrCount-1)*Offst+Wid]: waveform data
|
xue@1
|
2124 FrCount, Wid, Offst: frame count, frame size and hop size
|
xue@1
|
2125 win[Wid]: window function
|
xue@1
|
2126 M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
|
xue@1
|
2127 Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
|
xue@1
|
2128 optional
|
xue@1
|
2129 maxiter: maximal number of iterates
|
xue@1
|
2130 ae[FrCount], fe[FrCount], pe[FrCount]: initial amplitude, frequency and phase estimates
|
xue@1
|
2131 Out: aret[FrCount], fret[FrCount], pret[FrCount]: reestimated amplitudes, frequencies and phase angles
|
xue@1
|
2132
|
xue@1
|
2133 Returns the number of unused iterates left of the total of maxiter.
|
xue@1
|
2134 */
|
xue@1
|
2135 int Reestimate2(int FrCount, int Wid, int Offst, double* win, int M, double* c, double iH2, double* x, double* ae, double* fe, double* pe, double* aret, double* fret, double *pret, int maxiter, int* Wids)
|
xue@1
|
2136 {
|
xue@1
|
2137 AllocateFFTBuffer(Wid, fft, w, xc);
|
xue@1
|
2138 double convep=1e-4, dif=0, lastdif=0; //convep is the hard-coded threshold that stops the iteration
|
xue@1
|
2139 int iter=1, hWid=Wid/2;
|
xue@1
|
2140
|
xue@1
|
2141 double *ns=new double[FrCount*12], *as=new double[Wid*5];
|
xue@1
|
2142 double *fbuf=&ns[FrCount], *abuf=&ns[FrCount*2],
|
xue@1
|
2143 *aa=&ns[FrCount*3], *ab=&ns[FrCount*4], *ac=&ns[FrCount*5], *ad=&ns[FrCount*6],
|
xue@1
|
2144 *fa=&ns[FrCount*7], *fb=&ns[FrCount*8], *fc=&ns[FrCount*9], *fd=&ns[FrCount*10],
|
xue@1
|
2145 *pbuf=&ns[FrCount*11];
|
xue@1
|
2146 double *ps=&as[Wid];
|
xue@1
|
2147 cdouble *xs=(cdouble*)&as[Wid*3];
|
xue@1
|
2148
|
xue@1
|
2149 memcpy(fbuf, fe, sizeof(double)*FrCount);
|
xue@1
|
2150 memcpy(abuf, ae, sizeof(double)*FrCount);
|
xue@1
|
2151 memcpy(pbuf, pe, sizeof(double)*FrCount);
|
xue@1
|
2152 for (int i=0; i<FrCount; i++)
|
xue@1
|
2153 {
|
xue@1
|
2154 ns[i]=hWid+i*Offst;
|
xue@1
|
2155 }
|
xue@1
|
2156
|
xue@1
|
2157 while (iter<=maxiter)
|
xue@1
|
2158 {
|
xue@1
|
2159 ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
|
xue@1
|
2160 ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
|
xue@1
|
2161 ReEstFreqAmp(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, as, fa, fb, fc, fd, aa, ab, ac, ad, ns, Wids);
|
xue@1
|
2162
|
xue@1
|
2163 if (iter>1) lastdif=dif;
|
xue@1
|
2164 dif=0;
|
xue@1
|
2165 if (iter==1)
|
xue@1
|
2166 {
|
xue@1
|
2167 for (int fr=0; fr<FrCount; fr++)
|
xue@1
|
2168 {
|
xue@1
|
2169 if (fabs(abuf[fr])>fabs(ae[fr]))
|
xue@1
|
2170 dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/abuf[fr]);
|
xue@1
|
2171 else
|
xue@1
|
2172 dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/ae[fr]);
|
xue@1
|
2173 }
|
xue@1
|
2174 }
|
xue@1
|
2175 else
|
xue@1
|
2176 {
|
xue@1
|
2177 for (int fr=0; fr<FrCount; fr++)
|
xue@1
|
2178 {
|
xue@1
|
2179 if (fabs(abuf[fr])>fabs(aret[fr]))
|
xue@1
|
2180 dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/abuf[fr]);
|
xue@1
|
2181 else
|
xue@1
|
2182 dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/aret[fr]);
|
xue@1
|
2183 }
|
xue@1
|
2184 }
|
xue@1
|
2185 memcpy(fret, fbuf, sizeof(double)*FrCount);
|
xue@1
|
2186 memcpy(aret, abuf, sizeof(double)*FrCount);
|
xue@1
|
2187 dif/=FrCount;
|
xue@1
|
2188 if (fabs(dif)<convep || (iter>1 && fabs(dif-lastdif)<convep*lastdif)) break;
|
xue@1
|
2189 iter++;
|
xue@1
|
2190 }
|
xue@1
|
2191
|
xue@1
|
2192 memcpy(pret, pbuf, sizeof(double)*FrCount);
|
xue@1
|
2193
|
xue@1
|
2194 delete[] ns;
|
xue@1
|
2195 delete[] as;
|
xue@1
|
2196 delete[] fft;
|
xue@1
|
2197
|
xue@1
|
2198 return maxiter-iter;
|
xue@1
|
2199 }//Reestimate2
|
xue@1
|
2200
|
xue@1
|
2201 //---------------------------------------------------------------------------
|
xue@1
|
2202 /*
|
xue@1
|
2203 Derivative method as proposed in DAFx09
|
xue@1
|
2204
|
xue@1
|
2205 Further reading: Wen X. and M. Sandler, "Notes on model-based non-stationary sinusoid estimation methods
|
xue@1
|
2206 using derivatives," in Proc. DAFx'09, Como, 2009.
|
xue@1
|
2207 */
|
xue@1
|
2208
|
Chris@5
|
2209 /**
|
xue@1
|
2210 function Derivative: derivative method for estimating amplitude derivative, frequency, and frequency derivative given
|
xue@1
|
2211 signal and its derivatives.
|
xue@1
|
2212
|
xue@1
|
2213 In: x[Wid], dx[Wid], ddx[Wid]: waveform and its derivatives
|
xue@1
|
2214 win[Wid]: window function
|
xue@1
|
2215 f0: initial digital frequency estimate
|
xue@1
|
2216 Out: f0: new estimate of digital frequency
|
xue@1
|
2217 f1, a1: estimates of frequency and amplitude derivatives
|
xue@1
|
2218
|
xue@1
|
2219 No return value.
|
xue@1
|
2220 */
|
xue@1
|
2221 void Derivative(int Wid, double* win, cdouble* x, cdouble* dx, cdouble* ddx, double& f0, double* f1, double* a0, double* a1, double* ph)
|
xue@1
|
2222 {
|
xue@1
|
2223 AllocateFFTBuffer(Wid, fft, W, X);
|
xue@11
|
2224 CFFTCW(x, win, fft, NULL, Log2(Wid), W, X);
|
xue@1
|
2225 int m=f0*Wid, m0=m-10, m1=m+10, hWid=Wid/2;
|
xue@1
|
2226 if (m0<0) m0=0; if (m1>hWid) m1=hWid;
|
xue@1
|
2227 for (int n=m0; n<=m1; n++) if (fft[n]>fft[m]) m=n;
|
xue@1
|
2228 cdouble Sw=0, S1w=0, S2w=0;
|
xue@1
|
2229 for (int n=0; n<Wid; n++)
|
xue@1
|
2230 {
|
xue@1
|
2231 cdouble tmp=x[n]*win[n];
|
xue@1
|
2232 Sw+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
|
xue@1
|
2233 tmp=dx[n]*win[n];
|
xue@1
|
2234 S1w+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
|
xue@1
|
2235 }
|
xue@1
|
2236 double omg0=(S1w/Sw).y;
|
xue@1
|
2237 Sw=0, S1w=0;
|
xue@1
|
2238 for (int n=0; n<Wid; n++)
|
xue@1
|
2239 {
|
xue@1
|
2240 cdouble tmp=x[n]*win[n];
|
xue@1
|
2241 Sw+=tmp.rotate(-omg0*(n-hWid)/Wid);
|
xue@1
|
2242 tmp=dx[n]*win[n];
|
xue@1
|
2243 S1w+=tmp.rotate(-omg0*(n-hWid)/Wid);
|
xue@1
|
2244 tmp=ddx[n]*win[n];
|
xue@1
|
2245 S2w+=tmp.rotate(-omg0*(n-hWid)/Wid);
|
xue@1
|
2246 }
|
xue@1
|
2247 omg0=(S1w/Sw).y;
|
xue@1
|
2248 double miu0=(S1w/Sw).x;
|
xue@1
|
2249 double psi0=(S2w/Sw).y-2*miu0*omg0;
|
xue@1
|
2250
|
xue@1
|
2251 f0=omg0/(2*M_PI);
|
xue@1
|
2252 *f1=psi0/(2*M_PI);
|
xue@1
|
2253 *a1=miu0;
|
xue@1
|
2254
|
xue@1
|
2255 FreeFFTBuffer(fft);
|
xue@1
|
2256 }//Derivative
|
xue@1
|
2257
|
Chris@5
|
2258 /**
|
xue@1
|
2259 function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency omg,
|
xue@1
|
2260 from x using window w and its derivatives.
|
xue@1
|
2261
|
xue@1
|
2262 In: x[Wid]: waveform data
|
xue@1
|
2263 w[K+1][Wid]: window functions and its derivatives up to order K
|
xue@1
|
2264 omg: angular frequency
|
xue@1
|
2265 Out: X[K+1]: windowed spectrum and its derivatives up to order K
|
xue@1
|
2266
|
xue@1
|
2267 No return value. This function is for internal use.
|
xue@1
|
2268 */
|
xue@1
|
2269 void Xkw(cdouble* X, int K, int Wid, double* x, double** w, double omg)
|
xue@1
|
2270 {
|
xue@1
|
2271 int hWid=Wid/2;
|
xue@1
|
2272 //calculate the first row
|
xue@1
|
2273 memset(X, 0, sizeof(cdouble)*(K+1));
|
xue@1
|
2274 for (int i=0; i<Wid; i++)
|
xue@1
|
2275 {
|
xue@1
|
2276 double n=i-hWid;
|
xue@1
|
2277 double ph=omg*n;
|
xue@1
|
2278 for (int k=0; k<=K; k++)
|
xue@1
|
2279 {
|
xue@1
|
2280 cdouble tmp=x[i]*w[k][i];
|
xue@1
|
2281 X[k]+=tmp.rotate(-ph);
|
xue@1
|
2282 }
|
xue@1
|
2283 }
|
xue@1
|
2284 //calculate the rest rows
|
xue@1
|
2285 for (int k=1; k<=K; k++)
|
xue@1
|
2286 {
|
xue@1
|
2287 cdouble *thisX=&X[k], *lastX=&X[k-1];
|
xue@1
|
2288 for (int kk=K-k; kk>=0; kk--) thisX[kk]=-lastX[kk+1]+cdouble(0, omg)*lastX[kk];
|
xue@1
|
2289 }
|
xue@1
|
2290 }//Xkw
|
xue@1
|
2291
|
Chris@5
|
2292 /**
|
xue@1
|
2293 function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency
|
xue@1
|
2294 omg, from x and its derivatives using window w.
|
xue@1
|
2295
|
xue@1
|
2296 In: x[K+1][Wid]: waveform data and its derivatives up to order K.
|
xue@1
|
2297 w[Wid]: window function
|
xue@1
|
2298 omg: angular frequency
|
xue@1
|
2299 Out: X[K+1]: windowed spectrum and its derivatives up to order K
|
xue@1
|
2300
|
xue@1
|
2301 No return value. This function is for testing only.
|
xue@1
|
2302 */
|
xue@1
|
2303 void Xkw(cdouble* X, int K, int Wid, double** x, double* w, double omg)
|
xue@1
|
2304 {
|
xue@1
|
2305 int hWid=Wid/2;
|
xue@1
|
2306 memset(X, 0, sizeof(cdouble)*(K+1));
|
xue@1
|
2307 for (int i=0; i<Wid; i++)
|
xue@1
|
2308 {
|
xue@1
|
2309 double n=i-hWid;
|
xue@1
|
2310 double ph=omg*n;
|
xue@1
|
2311 for (int k=0; k<=K; k++)
|
xue@1
|
2312 {
|
xue@1
|
2313 cdouble tmp=x[k][i]*w[i];
|
xue@1
|
2314 X[k]+=tmp.rotate(-ph);
|
xue@1
|
2315 }
|
xue@1
|
2316 }
|
xue@1
|
2317 }//Xkw
|
xue@1
|
2318
|
Chris@5
|
2319 /**
|
xue@1
|
2320 function Derivative: derivative method for estimating the model log(s)=h[M]'r[M], by discarding extra
|
xue@1
|
2321 equations
|
xue@1
|
2322
|
xue@1
|
2323 In: s[Wid]: waveform data
|
xue@1
|
2324 win[][Wid]: window function and its derivatives
|
xue@1
|
2325 h[M], dh[M]: pointers to basis functions and their derivatives
|
xue@1
|
2326 harg: pointer argument to be used by calls to functions in h[] amd dh[].
|
xue@1
|
2327 p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
|
xue@1
|
2328 q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
|
xue@1
|
2329 omg: initial angular frequency
|
xue@1
|
2330 Out: r[M]: estimated coefficients to h[M].
|
xue@1
|
2331
|
xue@1
|
2332 No return value.
|
xue@1
|
2333 */
|
xue@1
|
2334 void Derivative(int M, double (**h)(double t, void*), double (**dh)(double t, void*), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg)
|
xue@1
|
2335 {
|
xue@1
|
2336 int hWid=Wid/2, M1=M-1;
|
xue@1
|
2337 int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
|
xue@1
|
2338 int Kc=ceil(Kr/2.0); //number of derivatives required
|
xue@1
|
2339
|
xue@1
|
2340 //ind marks the 2*M1 real elements of an M1-array of complex unknowns with
|
xue@1
|
2341 // numerical indices (0-based) or -1 if it is not a real unknown variable
|
xue@1
|
2342 //uind marks the Kr real unknowns with their positions in ind
|
xue@1
|
2343 int *uind=new int[Kr], *ind=new int[2*M1];
|
xue@1
|
2344 memset(ind, 0, sizeof(int)*2*M1);
|
xue@1
|
2345 for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
|
xue@1
|
2346 for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
|
xue@1
|
2347 {
|
xue@1
|
2348 int p=0, up=0;
|
xue@1
|
2349 while (p<2*M1)
|
xue@1
|
2350 {
|
xue@1
|
2351 if (ind[p]>=0)
|
xue@1
|
2352 {
|
xue@1
|
2353 uind[up]=p;
|
xue@1
|
2354 ind[p]=up;
|
xue@1
|
2355 up++;
|
xue@1
|
2356 }
|
xue@1
|
2357 p++;
|
xue@1
|
2358 }
|
xue@1
|
2359 if (up!=Kr) throw("");
|
xue@1
|
2360 }
|
xue@1
|
2361
|
xue@1
|
2362 cdouble* Skw=new cdouble[M];
|
xue@1
|
2363 Xkw(Skw, Kc, Wid, s, win, omg);
|
xue@1
|
2364
|
xue@1
|
2365 double* x=new double[Wid];
|
xue@1
|
2366 cdouble** Allocate2(cdouble, M, Kc, Smkw);
|
xue@1
|
2367 for (int m=1; m<M; m++)
|
xue@1
|
2368 {
|
xue@1
|
2369 for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
|
xue@1
|
2370 Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
|
xue@1
|
2371 }
|
xue@1
|
2372
|
xue@1
|
2373 //allocate buffer for linear system A(pq)=b
|
xue@1
|
2374 Alloc2(2*Kc+2, Kr, A); double** AA; double *bb, *pqpq;
|
xue@1
|
2375 double *b=A[2*Kc], *pq=A[2*Kc+1];
|
xue@1
|
2376 for (int k=0; k<Kr; k++) b[k]=((double*)(&Skw[1]))[k];
|
xue@1
|
2377 // *pq=(double*)(&r[1]);
|
xue@1
|
2378 for (int k=0; k<Kc; k++) //looping through rows of A
|
xue@1
|
2379 {
|
xue@1
|
2380 //columns of A includes rows of Smkw corresponding to real unknowns
|
xue@1
|
2381 for (int m=0; m<M1; m++)
|
xue@1
|
2382 {
|
xue@1
|
2383 int lind;
|
xue@1
|
2384 if ((lind=ind[2*m])>=0) //the real part being unknown
|
xue@1
|
2385 {
|
xue@1
|
2386 A[2*k][lind]=Smkw[m+1][k].x;
|
xue@1
|
2387 A[2*k+1][lind]=Smkw[m+1][k].y;
|
xue@1
|
2388 }
|
xue@1
|
2389 if ((lind=ind[2*m+1])>=0) //the imag part being unknown
|
xue@1
|
2390 {
|
xue@1
|
2391 A[2*k+1][lind]=Smkw[m+1][k].x;
|
xue@1
|
2392 A[2*k][lind]=-Smkw[m+1][k].y;
|
xue@1
|
2393 }
|
xue@1
|
2394 }
|
xue@1
|
2395 }
|
xue@1
|
2396
|
xue@1
|
2397 bool dropeq=(2*Kc-1==Kr);
|
xue@1
|
2398 if (dropeq)
|
xue@1
|
2399 {
|
xue@1
|
2400 Allocate2(double, Kr+2, Kr, AA);
|
xue@1
|
2401 bb=AA[Kr], pqpq=AA[Kr+1];
|
xue@1
|
2402 memcpy(AA[0], A[0], sizeof(double)*Kr*(Kr-1));
|
xue@1
|
2403 memcpy(AA[Kr-1], A[Kr], sizeof(double)*Kr);
|
xue@1
|
2404 memcpy(bb, b, sizeof(double)*(Kr-1));
|
xue@1
|
2405 bb[Kr-1]=((double*)(&Skw[1]))[Kr];
|
xue@1
|
2406 }
|
xue@1
|
2407
|
xue@1
|
2408 double det;
|
xue@1
|
2409 GECP(Kr, pq, A, b, &det);
|
xue@1
|
2410 if (dropeq)
|
xue@1
|
2411 {
|
xue@1
|
2412 double det2;
|
xue@1
|
2413 GECP(Kr, pqpq, AA, bb, &det2);
|
xue@1
|
2414 if (fabs(det2)>fabs(det)) memcpy(pq, pqpq, sizeof(double)*Kr);
|
xue@1
|
2415 DeAlloc2(AA);
|
xue@1
|
2416 }
|
xue@1
|
2417 memset(&r[1], 0, sizeof(double)*M1*2);
|
xue@1
|
2418 for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
|
xue@1
|
2419
|
xue@1
|
2420 //estiamte r0
|
xue@1
|
2421 cdouble e0=0;
|
xue@1
|
2422 for (int i=0; i<Wid; i++)
|
xue@1
|
2423 {
|
xue@1
|
2424 cdouble expo=0;
|
xue@1
|
2425 double n=i-hWid;
|
xue@1
|
2426 for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
|
xue@1
|
2427 cdouble tmp=exp(expo)*win[0][i];
|
xue@1
|
2428 e0+=tmp.rotate(-omg*n);
|
xue@1
|
2429 }
|
xue@1
|
2430 r[0]=log(Skw[0]/e0);
|
xue@1
|
2431
|
xue@1
|
2432 delete[] x;
|
xue@1
|
2433 delete[] Skw;
|
xue@1
|
2434 delete[] uind;
|
xue@1
|
2435 delete[] ind;
|
xue@1
|
2436 DeAlloc2(Smkw);
|
xue@1
|
2437 DeAlloc2(A);
|
xue@1
|
2438 }//Derivative*/
|
xue@1
|
2439
|
Chris@5
|
2440 /**
|
xue@1
|
2441 function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M], least-square
|
xue@1
|
2442 implementation
|
xue@1
|
2443
|
xue@1
|
2444 In: s[Wid]: waveform data
|
xue@1
|
2445 win[][Wid]: window function and its derivatives
|
xue@1
|
2446 h[M], dh[M]: pointers to basis functions and their derivatives
|
xue@1
|
2447 harg: pointer argument to be used by calls to functions in h[] amd dh[].
|
xue@1
|
2448 K: number of derivatives to take
|
xue@1
|
2449 p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
|
xue@1
|
2450 q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
|
xue@1
|
2451 omg: initial angular frequency
|
xue@1
|
2452 Out: r[M]: estimated coefficients to h[M].
|
xue@1
|
2453
|
xue@1
|
2454 No return value.
|
xue@1
|
2455 */
|
xue@1
|
2456 void DerivativeLS(int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0)
|
xue@1
|
2457 {
|
xue@1
|
2458 int hWid=Wid/2, M1=M-1;
|
xue@1
|
2459 int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
|
xue@1
|
2460 int Kc=ceil(Kr/2.0); //number of derivatives required
|
xue@1
|
2461 if (Kc<K) Kc=K;
|
xue@1
|
2462
|
xue@1
|
2463 int *uind=new int[Kr], *ind=new int[2*M1];
|
xue@1
|
2464 memset(ind, 0, sizeof(int)*2*M1);
|
xue@1
|
2465 for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
|
xue@1
|
2466 for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
|
xue@1
|
2467 {int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;} if (up!=Kr) throw("");}
|
xue@1
|
2468
|
xue@1
|
2469 //allocate buffer for linear system A(pq)=b
|
xue@1
|
2470 cdouble* Skw=new cdouble[Kc+1];
|
xue@1
|
2471 double* x=new double[Wid];
|
xue@1
|
2472 cdouble** Allocate2(cdouble, M, Kc, Smkw);
|
xue@1
|
2473
|
xue@1
|
2474 Alloc2(2*Kc+2, 2*Kc, A);
|
xue@1
|
2475 double *b=A[2*Kc], *pq=A[2*Kc+1];
|
xue@1
|
2476
|
xue@1
|
2477 Xkw(Skw, Kc, Wid, s, win, omg);
|
xue@1
|
2478 for (int m=1; m<M; m++)
|
xue@1
|
2479 {
|
xue@1
|
2480 for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
|
xue@1
|
2481 Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
|
xue@1
|
2482 }
|
xue@1
|
2483
|
xue@1
|
2484 for (int k=0; k<2*Kc; k++) b[k]=((double*)(&Skw[1]))[k];
|
xue@1
|
2485 for (int k=0; k<Kc; k++)
|
xue@1
|
2486 {
|
xue@1
|
2487 for (int m=0; m<M1; m++)
|
xue@1
|
2488 {
|
xue@1
|
2489 int lind;
|
xue@1
|
2490 if ((lind=ind[2*m])>=0)
|
xue@1
|
2491 {
|
xue@1
|
2492 A[2*k][lind]=Smkw[m+1][k].x;
|
xue@1
|
2493 A[2*k+1][lind]=Smkw[m+1][k].y;
|
xue@1
|
2494 }
|
xue@1
|
2495 if ((lind=ind[2*m+1])>=0)
|
xue@1
|
2496 {
|
xue@1
|
2497 A[2*k+1][lind]=Smkw[m+1][k].x;
|
xue@1
|
2498 A[2*k][lind]=-Smkw[m+1][k].y;
|
xue@1
|
2499 }
|
xue@1
|
2500 }
|
xue@1
|
2501 }
|
xue@1
|
2502
|
xue@1
|
2503 if (2*Kc==Kr) GECP(Kr, pq, A, b);
|
xue@1
|
2504 else LSLinear2(2*Kc, Kr, pq, A, b);
|
xue@1
|
2505
|
xue@1
|
2506 memset(&r[1], 0, sizeof(double)*M1*2);
|
xue@1
|
2507 for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
|
xue@1
|
2508 //estiamte r0
|
xue@1
|
2509 if (r0)
|
xue@1
|
2510 {
|
xue@1
|
2511 cdouble e0=0;
|
xue@1
|
2512 for (int i=0; i<Wid; i++)
|
xue@1
|
2513 {
|
xue@1
|
2514 cdouble expo=0;
|
xue@1
|
2515 double n=i-hWid;
|
xue@1
|
2516 for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
|
xue@1
|
2517 cdouble tmp=exp(expo)*win[0][i];
|
xue@1
|
2518 e0+=tmp.rotate(-omg*n);
|
xue@1
|
2519 }
|
xue@1
|
2520 r[0]=log(Skw[0]/e0);
|
xue@1
|
2521 }
|
xue@1
|
2522 delete[] x;
|
xue@1
|
2523 delete[] Skw;
|
xue@1
|
2524 delete[] uind;
|
xue@1
|
2525 delete[] ind;
|
xue@1
|
2526 DeAlloc2(Smkw);
|
xue@1
|
2527 DeAlloc2(A);
|
xue@1
|
2528 }//DerivativeLS
|
xue@1
|
2529
|
Chris@5
|
2530 /**
|
xue@1
|
2531 function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M] using Fr
|
xue@1
|
2532 measurement points a quarter of Wid apart from each other, implemented by least-square.
|
xue@1
|
2533
|
xue@1
|
2534 In: s[Wid+(Fr-1)*Wid/4]: waveform data
|
xue@1
|
2535 win[][Wid]: window function and its derivatives
|
xue@1
|
2536 h[M], dh[M]: pointers to basis functions and their derivatives
|
xue@1
|
2537 harg: pointer argument to be used by calls to functions in h[] amd dh[].
|
xue@1
|
2538 Fr: number of measurement points
|
xue@1
|
2539 K: number of derivatives to take at each measurement point
|
xue@1
|
2540 p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
|
xue@1
|
2541 q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
|
xue@1
|
2542 omg: initial angular frequency
|
xue@1
|
2543 r0: specifies if r[0] is to be computed.
|
xue@1
|
2544 Out: r[M]: estimated coefficients to h[M].
|
xue@1
|
2545
|
xue@1
|
2546 No return value.
|
xue@1
|
2547 */
|
xue@1
|
2548 void DerivativeLS(int Fr, int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0)
|
xue@1
|
2549 {
|
xue@1
|
2550 int hWid=Wid/2, qWid=Wid/4, M1=M-1;
|
xue@1
|
2551 int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
|
xue@1
|
2552 int Kc=ceil(Kr/2.0/Fr); //number of derivatives required
|
xue@1
|
2553 if (Kc<K) Kc=K;
|
xue@1
|
2554
|
xue@1
|
2555 int *uind=new int[Kr], *ind=new int[2*M1];
|
xue@1
|
2556 memset(ind, 0, sizeof(int)*2*M1);
|
xue@1
|
2557 for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
|
xue@1
|
2558 for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
|
xue@1
|
2559 {int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;}}
|
xue@1
|
2560
|
xue@1
|
2561 //allocate buffer for linear system A(pq)=b
|
xue@1
|
2562 cdouble* Skw=new cdouble[Kc+1], Skw00;
|
xue@1
|
2563 double* x=new double[Wid];
|
xue@1
|
2564 cdouble** Allocate2(cdouble, M, Kc, Smkw);
|
xue@1
|
2565
|
xue@1
|
2566 Alloc2(2*Fr*Kc, 2*Fr*Kc, A);
|
xue@1
|
2567 double *pq=new double[2*Fr*Kc], *b=new double[2*Fr*Kc];
|
xue@1
|
2568
|
xue@1
|
2569 for (int fr=0; fr<Fr; fr++)
|
xue@1
|
2570 {
|
xue@1
|
2571 int Offst=qWid*fr; double* ss=&s[Offst];
|
xue@1
|
2572
|
xue@1
|
2573 Xkw(Skw, Kc, Wid, ss, win, omg); if (fr==0) Skw00=Skw[0];
|
xue@1
|
2574 for (int m=1; m<M; m++)
|
xue@1
|
2575 {
|
xue@1
|
2576 for (int i=0; i<Wid; i++) x[i]=dh[m](i+Offst-hWid, harg)*ss[i];
|
xue@1
|
2577 Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
|
xue@1
|
2578 }
|
xue@1
|
2579
|
xue@1
|
2580 for (int k=0; k<2*Kc; k++) b[2*fr*Kc+k]=((double*)(&Skw[1]))[k];
|
xue@1
|
2581 for (int k=0; k<Kc; k++)
|
xue@1
|
2582 {
|
xue@1
|
2583 for (int m=0; m<M1; m++)
|
xue@1
|
2584 {
|
xue@1
|
2585 int lind;
|
xue@1
|
2586 if ((lind=ind[2*m])>=0)
|
xue@1
|
2587 {
|
xue@1
|
2588 A[2*fr*Kc+2*k][lind]=Smkw[m+1][k].x;
|
xue@1
|
2589 A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].y;
|
xue@1
|
2590 }
|
xue@1
|
2591 if ((lind=ind[2*m+1])>=0)
|
xue@1
|
2592 {
|
xue@1
|
2593 A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].x;
|
xue@1
|
2594 A[2*fr*Kc+2*k][lind]=-Smkw[m+1][k].y;
|
xue@1
|
2595 }
|
xue@1
|
2596 }
|
xue@1
|
2597 }
|
xue@1
|
2598 }
|
xue@1
|
2599 if (2*Fr*Kc==Kr) GECP(Kr, pq, A, b);
|
xue@1
|
2600 else LSLinear2(2*Fr*Kc, Kr, pq, A, b);
|
xue@1
|
2601
|
xue@1
|
2602 memset(&r[1], 0, sizeof(double)*M1*2);
|
xue@1
|
2603 for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
|
xue@1
|
2604 //estiamte r0
|
xue@1
|
2605 if (r0)
|
xue@1
|
2606 {
|
xue@1
|
2607 cdouble e0=0;
|
xue@1
|
2608 for (int i=0; i<Wid; i++)
|
xue@1
|
2609 {
|
xue@1
|
2610 cdouble expo=0;
|
xue@1
|
2611 double n=i-hWid;
|
xue@1
|
2612 for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
|
xue@1
|
2613 cdouble tmp=exp(expo)*win[0][i];
|
xue@1
|
2614 e0+=tmp.rotate(-omg*n);
|
xue@1
|
2615 }
|
xue@1
|
2616 r[0]=log(Skw00/e0);
|
xue@1
|
2617 }
|
xue@1
|
2618 delete[] x;
|
xue@1
|
2619 delete[] Skw;
|
xue@1
|
2620 delete[] uind;
|
xue@1
|
2621 delete[] ind;
|
xue@1
|
2622 DeAlloc2(Smkw);
|
xue@1
|
2623 DeAlloc2(A);
|
xue@1
|
2624 delete[] pq; delete[] b;
|
xue@1
|
2625 }//DerivativeLS
|
xue@1
|
2626
|
xue@1
|
2627 //---------------------------------------------------------------------------
|
xue@1
|
2628 /*
|
xue@1
|
2629 Abe-Smith sinusoid estimator 2005
|
xue@1
|
2630
|
xue@1
|
2631 Further reading: M. Abe and J. O. Smith III, ¡°AM/FM rate estimation for time-varying sinusoidal
|
xue@1
|
2632 modeling,¡± in Proc. ICASSP'05, Philadelphia, 2005.
|
xue@1
|
2633 */
|
xue@1
|
2634
|
Chris@5
|
2635 /**
|
xue@1
|
2636 function RDFTW: windowed DTFT at frequency k bins
|
xue@1
|
2637
|
xue@1
|
2638 In: data[Wid]: waveform data
|
xue@1
|
2639 w[Wid]: window function
|
xue@1
|
2640 k: frequency, in bins
|
xue@1
|
2641 Out: Xr, Xi: real and imaginary parts of the DTFT of xw at frequency k bins
|
xue@1
|
2642
|
xue@1
|
2643 No return value.
|
xue@1
|
2644 */
|
xue@1
|
2645 void RDFTW(double& Xr, double& Xi, double k, int Wid, double* data, double* w)
|
xue@1
|
2646 {
|
xue@1
|
2647 Xr=Xi=0;
|
xue@1
|
2648 int hWid=Wid/2;
|
xue@1
|
2649 double* lw=&w[Wid];
|
xue@1
|
2650 for (int i=0; i<=Wid; i++)
|
xue@1
|
2651 {
|
xue@1
|
2652 double tmp;
|
xue@1
|
2653 tmp=*data**lw;
|
xue@1
|
2654 data++, lw--;
|
xue@1
|
2655 //*
|
xue@1
|
2656 double ph=-2*M_PI*(i-hWid)*k/Wid;
|
xue@1
|
2657 Xr+=tmp*cos(ph);
|
xue@1
|
2658 Xi+=tmp*sin(ph); //*/
|
xue@1
|
2659 }
|
xue@1
|
2660 }//RDFTW
|
xue@1
|
2661
|
Chris@5
|
2662 /**
|
xue@1
|
2663 function TFAS05: the Abe-Smith method 2005
|
xue@1
|
2664
|
xue@1
|
2665 In: data[Wid]: waveform data
|
xue@1
|
2666 w[Wid]: window function
|
xue@1
|
2667 res: resolution of frequency for QIFFT
|
xue@1
|
2668 Out: f, a, ph: frequency, amplitude and phase angle estimates
|
xue@1
|
2669 aesp, fslope: estimates of log amplitude and frequency derivatives
|
xue@1
|
2670
|
xue@1
|
2671 No return value.
|
xue@1
|
2672 */
|
xue@1
|
2673 void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
|
xue@1
|
2674 {
|
xue@1
|
2675 double fi=floor(f*Wid+0.5); //frequency (int) in bins
|
xue@1
|
2676 double xr0, xi0, xr_1, xi_1, xr1, xi1;
|
xue@1
|
2677 RDFTW(xr0, xi0, fi, Wid, data, w);
|
xue@1
|
2678 RDFTW(xr_1, xi_1, fi-res, Wid, data, w);
|
xue@1
|
2679 RDFTW(xr1, xi1, fi+res, Wid, data, w);
|
xue@1
|
2680 double winnorm=0; for (int i=0; i<=Wid; i++) winnorm+=w[i];
|
xue@1
|
2681 double y0=log(sqrt(xr0*xr0+xi0*xi0)/winnorm),
|
xue@1
|
2682 y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm),
|
xue@1
|
2683 y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
|
xue@1
|
2684 double df=0;
|
xue@1
|
2685 //*
|
xue@1
|
2686 if (y0<y1)
|
xue@1
|
2687 {
|
xue@1
|
2688 double newfi=fi+res;
|
xue@1
|
2689 while (y0<y1)
|
xue@1
|
2690 {
|
xue@1
|
2691 y_1=y0, xr_1=xr0, xi_1=xi0;
|
xue@1
|
2692 y0=y1, xr0=xr1, xi0=xi1;
|
xue@1
|
2693 newfi+=res;
|
xue@1
|
2694 RDFTW(xr1, xi1, newfi, Wid, data, w);
|
xue@1
|
2695 y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
|
xue@1
|
2696 fi+=res;
|
xue@1
|
2697 }
|
xue@1
|
2698 }
|
xue@1
|
2699 else if(y0<y_1)
|
xue@1
|
2700 {
|
xue@1
|
2701 double newfi=fi-res;
|
xue@1
|
2702 while (y0<y_1)
|
xue@1
|
2703 {
|
xue@1
|
2704 y1=y0, xr1=xr0, xi1=xi0;
|
xue@1
|
2705 y0=y_1, xr0=xr_1, xi0=xi_1;
|
xue@1
|
2706 newfi-=res;
|
xue@1
|
2707 RDFTW(xr_1, xi_1, newfi, Wid, data, w);
|
xue@1
|
2708 y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm);
|
xue@1
|
2709 fi-=res;
|
xue@1
|
2710 }
|
xue@1
|
2711 } //*/
|
xue@1
|
2712
|
xue@1
|
2713 double a2=(y1+y_1)*0.5-y0, a1=(y1-y_1)*0.5, a0=y0;
|
xue@1
|
2714 df=-a1*0.5/a2;
|
xue@1
|
2715 f=fi+df*res; //in bins
|
xue@1
|
2716 double y=a0-0.25*a1*a1/a2;
|
xue@1
|
2717 a=exp(y);
|
xue@1
|
2718 double ph0=(xi0==0 && xr0==0)?0:atan2(xi0, xr0),
|
xue@1
|
2719 ph_1=(xi_1==0 && xr_1==0)?0:atan2(xi_1, xr_1),
|
xue@1
|
2720 ph1=(xi1==0 && xr1==0)?0:atan2(xi1, xr1);
|
xue@1
|
2721 if (fabs(ph_1-ph0)>M_PI)
|
xue@1
|
2722 {
|
xue@1
|
2723 if (ph_1-ph0>0) ph_1-=M_PI*2;
|
xue@1
|
2724 else ph_1+=M_PI*2;
|
xue@1
|
2725 }
|
xue@1
|
2726 if (fabs(ph1-ph0)>M_PI)
|
xue@1
|
2727 {
|
xue@1
|
2728 if (ph1-ph0>0) ph1-=M_PI*2;
|
xue@1
|
2729 else ph1+=M_PI*2;
|
xue@1
|
2730 }
|
xue@1
|
2731 double b2=(ph1+ph_1)*0.5-ph0, b1=(ph1-ph_1)*0.5, b0=ph0;
|
xue@1
|
2732 ph=b0+b1*(df+b2*df);
|
xue@1
|
2733 //now we have the QI estimates
|
xue@1
|
2734 double uff=2*a2, vf=b1+2*b2*df, vff=2*b2;
|
xue@1
|
2735 double dfdp=Wid/(2*M_PI*res);
|
xue@1
|
2736 double upp=uff*dfdp*dfdp, vp=vf*dfdp, vpp=vff*dfdp*dfdp;
|
xue@1
|
2737 double p=-upp*0.5/(upp*upp+vpp*vpp);
|
xue@1
|
2738 double alf=-2*p*vp, beta=p*vpp/upp;
|
xue@1
|
2739 //*direct method
|
xue@1
|
2740 double beta_p=beta/p;
|
xue@1
|
2741 double feses=f-alf*beta/p /(2*M_PI)*Wid,
|
xue@1
|
2742 yeses=y-alf*alf*0.25/p+0.25*log(1+beta_p*beta_p),
|
xue@1
|
2743 pheses=ph+alf*alf*beta*0.25/p-0.5*atan(beta_p); //*/
|
xue@1
|
2744 /*adapted method
|
xue@1
|
2745 double zt[]={0, 0.995354, 0.169257, 1.393056, 0.442406, -0.717980, -0.251620, 0.177511, 0.158120, -0.503299};
|
xue@1
|
2746 double delt=res/Wid; double delt0=df*delt;
|
xue@1
|
2747 beta=zt[3]*beta+zt[4]*delt0*alf;
|
xue@1
|
2748 alf=(zt[1]+zt[2]*delt*delt)*alf;
|
xue@1
|
2749 double beta_p=beta/p;
|
xue@1
|
2750 double feses=f+zt[5]*alf*beta/p /(2*M_PI)*Wid,
|
xue@1
|
2751 yeses=y+zt[6]*alf*alf/p+zt[7]*log(1+beta_p*beta_p),
|
xue@1
|
2752 pheses=ph+zt[8]*alf*alf*beta/p+zt[9]*atan(beta_p); //*/
|
xue@1
|
2753 f=feses/Wid, a=exp(yeses), ph=pheses, fslope=2*beta/2/M_PI, aesp=alf;
|
xue@1
|
2754 }//TFAS05
|
xue@1
|
2755
|
Chris@5
|
2756 /**
|
xue@1
|
2757 function TFAS05_enh: the Abe-Smith method 2005 enhanced by LSE amplitude and phase estimation
|
xue@1
|
2758
|
xue@1
|
2759 In: data[Wid]: waveform data
|
xue@1
|
2760 w[Wid]: window function
|
xue@1
|
2761 res: resolution of frequency for QIFFT
|
xue@1
|
2762 Out: f, a, ph: frequency, amplitude and phase angle estimates
|
xue@1
|
2763 aesp, fslope: estimates of log amplitude and frequency derivatives
|
xue@1
|
2764
|
xue@1
|
2765 No return value.
|
xue@1
|
2766 */
|
xue@1
|
2767 void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
|
xue@1
|
2768 {
|
xue@1
|
2769 TFAS05(f, t, a, ph, aesp, fslope, Wid, data, w, res);
|
xue@1
|
2770 double xr=0, xi=0, p, win2=0;
|
xue@1
|
2771 for (int n=0; n<=Wid; n++)
|
xue@1
|
2772 {
|
xue@1
|
2773 double ni=n-Wid/2, tmp=data[n]*w[n]*w[n];//*exp(-aesp*(n-Wid/2)); if (IsInfinite(tmp)) continue;
|
xue@1
|
2774 p=-2*M_PI*(f+0.5*fslope*ni)*ni;
|
xue@1
|
2775 xr+=tmp*cos(p);
|
xue@1
|
2776 xi+=tmp*sin(p);
|
xue@1
|
2777 win2+=w[n]*w[n];
|
xue@1
|
2778 }
|
xue@1
|
2779 a=sqrt(xr*xr+xi*xi)/win2;
|
xue@1
|
2780 ph=(xr==0 && xi==0)?0:atan2(xi, xr);
|
xue@1
|
2781 }//TFAS05_enh
|
xue@1
|
2782 //version without returning aesp and fslope
|
xue@1
|
2783 void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res)
|
xue@1
|
2784 {
|
xue@1
|
2785 double aesp, fslope;
|
xue@1
|
2786 TFAS05_enh(f, t, a, ph, aesp, fslope, Wid, data, w, res);
|
xue@1
|
2787 }//TFAS05_enh
|
xue@1
|
2788
|
xue@1
|
2789 //---------------------------------------------------------------------------
|
Chris@5
|
2790 /**
|
xue@1
|
2791 function DerivativeLSv_AmpPh: estimate the constant-term in the local derivative method. This is used
|
xue@1
|
2792 by the local derivative algorithm, whose implementation is found in the header file as templates.
|
xue@1
|
2793
|
xue@1
|
2794 In: sv0: inner product <s, v0>, where s is the sinusoid being estimated.
|
xue@1
|
2795 integr_h[M][Wid]: M vectors containing samples of the integral of basis functions h[M].
|
xue@1
|
2796 v0[M]: a test function
|
xue@1
|
2797 lmd[M]: coefficients to h[M]
|
xue@1
|
2798
|
xue@1
|
2799 Returns coefficient of integr_h[0]=1.
|
xue@1
|
2800 */
|
xue@1
|
2801 cdouble DerivativeLSv_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, cdouble* v0, cdouble sv0)
|
xue@1
|
2802 {
|
xue@1
|
2803 cdouble e0=0;
|
xue@1
|
2804 for (int n=0; n<Wid; n++)
|
xue@1
|
2805 {
|
xue@1
|
2806 cdouble expo=0;
|
xue@1
|
2807 for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n];
|
xue@1
|
2808 e0+=exp(expo)**v0[n];
|
xue@1
|
2809 }
|
xue@1
|
2810 return log(sv0/e0);
|
xue@1
|
2811 }//DerivativeLSv_AmpPh
|
xue@1
|
2812
|
xue@1
|
2813 //---------------------------------------------------------------------------
|
xue@1
|
2814 /*
|
xue@1
|
2815 Piecewise derivative algorithm
|
xue@1
|
2816
|
xue@1
|
2817 Further reading: Wen X. and M. Sandler, "Spline exponential approximation of time-varying
|
xue@1
|
2818 sinusoids," under review.
|
xue@1
|
2819 */
|
xue@1
|
2820
|
Chris@5
|
2821 /**
|
xue@1
|
2822 function setv: computes I test functions v[I] by modulation u[I] to frequency f
|
xue@1
|
2823
|
xue@1
|
2824 In: u[I+1][Wid], du[I+1][Wid]: base-band test functions and their derivatives
|
xue@1
|
2825 f: carrier frequency
|
xue@1
|
2826 Out: v[I][Wid], dv[I][Wid]: test functions and their derivatives
|
xue@1
|
2827
|
xue@1
|
2828 No return value.
|
xue@1
|
2829 */
|
xue@1
|
2830 void setv(int I, int Wid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
|
xue@1
|
2831 {
|
xue@1
|
2832 double fbin=floor(f*Wid+0.5)/Wid;
|
xue@1
|
2833 double omg=fbin*2*M_PI;
|
xue@1
|
2834 cdouble jomg=cdouble(0, omg);
|
xue@1
|
2835 for (int c=0; c<Wid; c++)
|
xue@1
|
2836 {
|
xue@1
|
2837 double t=c;
|
xue@1
|
2838 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
2839 for (int i=0; i<I-1; i++) v[i][c]=u[i][c]*rot;
|
xue@1
|
2840 for (int i=0; i<I-1; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
|
xue@1
|
2841 //Here it is assumed that elements of u[] are modulated at 0, 1, -1, 2, -2, 3, -3, 4, ...;
|
xue@1
|
2842 //if f is under fbin then the closest ones are in order 0, -1, 1, -2, 3, -3, 3, .... This
|
xue@1
|
2843 //makes a difference to the whole of v[] only if I is even.
|
xue@1
|
2844 if (f>=fbin || I%2==1){v[I-1][c]=u[I-1][c]*rot; dv[I-1][c]=du[I-1][c]*rot+jomg*v[I-1][c];}
|
xue@1
|
2845 else{v[I-1][c]=u[I][c]*rot; dv[I-1][c]=du[I][c]*rot+jomg*v[I-1][c];}
|
xue@1
|
2846 }
|
xue@1
|
2847 }//setv
|
xue@1
|
2848
|
Chris@5
|
2849 /**
|
xue@1
|
2850 function setvhalf: computes I half-size test functions v[I] by modulation u[I] to frequency f.
|
xue@1
|
2851
|
xue@1
|
2852 In: u[I][hWid*2], du[I][Wid*2]: base-band test functions and their derivatives
|
xue@1
|
2853 f: carrier frequency
|
xue@1
|
2854 Out: v[I][hWid], dv[hWid]: half-size test functions and their derivatives
|
xue@1
|
2855
|
xue@1
|
2856 No return value.
|
xue@1
|
2857 */void setvhalf(int I, int hWid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
|
xue@1
|
2858 {
|
xue@1
|
2859 double fbin=floor(f*hWid)/hWid;
|
xue@1
|
2860 double omg=fbin*2*M_PI;
|
xue@1
|
2861 cdouble jomg=cdouble(0, omg);
|
xue@1
|
2862 for (int c=0; c<hWid; c++)
|
xue@1
|
2863 {
|
xue@1
|
2864 double t=c;
|
xue@1
|
2865 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
2866 for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
|
xue@1
|
2867 for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
|
xue@1
|
2868 }
|
xue@1
|
2869 }//setvhalf
|
xue@1
|
2870
|
xue@1
|
2871 //#define ERROR_CHECK
|
xue@1
|
2872
|
Chris@5
|
2873 /**
|
xue@1
|
2874 function DerivativePiecewise: Piecewise derivative algorithm. In this implementation of the piecewise
|
xue@1
|
2875 method the test functions v are constructed from I "basic" (single-frame) test functions, each
|
xue@1
|
2876 covering the same period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I
|
xue@1
|
2877 test functions are used.
|
xue@1
|
2878
|
xue@1
|
2879 In: s[LT+1]: waveform data
|
xue@1
|
2880 ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
|
xue@1
|
2881 L, T: number and length of pieces.
|
xue@1
|
2882 N: number of independent coefficients
|
xue@1
|
2883 h[M][T]: piecewise basis functions
|
xue@1
|
2884 A[L][M][N]: L matrices that map independent coefficients onto component coefficients over the L pieces
|
xue@1
|
2885 u[I][2T}, du[I][2T]: base-band test functions
|
xue@1
|
2886 f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
|
xue@1
|
2887 endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
2888 Out: aita[N]: independent coefficients
|
xue@1
|
2889
|
xue@1
|
2890 No return value.
|
xue@1
|
2891 */
|
xue@1
|
2892 void DerivativePiecewise(int N, cdouble* aita, int L, double* f, int T, cdouble* s, double*** A, int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds)
|
xue@1
|
2893 {
|
xue@1
|
2894 MList* mlist=new MList;
|
xue@1
|
2895 int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
|
xue@1
|
2896 cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
|
xue@1
|
2897 cdouble** Allocate2(cdouble, I, T*2, v);
|
xue@1
|
2898 cdouble** Allocate2(cdouble, I, T*2, dv);
|
xue@1
|
2899 //compute <sr, v>
|
xue@1
|
2900 cdouble*** Allocate3L(cdouble, L_1, I, N, srv, mlist);
|
xue@1
|
2901 cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
|
xue@1
|
2902 cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
|
xue@1
|
2903
|
xue@1
|
2904 #ifdef ERROR_CHECK
|
xue@1
|
2905 cdouble dsv1[128], dsv2[128];
|
xue@1
|
2906 #endif
|
xue@1
|
2907 for (int l=0; l<L-1; l++)
|
xue@1
|
2908 {
|
xue@1
|
2909 //v from u given f[l]
|
xue@1
|
2910 double fbin=floor(f[l+1]*T*2)/(T*2.0);
|
xue@1
|
2911 double omg=fbin*2*M_PI;
|
xue@1
|
2912 cdouble jomg=cdouble(0, omg);
|
xue@1
|
2913 for (int c=0; c<T*2; c++)
|
xue@1
|
2914 {
|
xue@1
|
2915 double t=c-T;
|
xue@1
|
2916 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
2917 for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
|
xue@1
|
2918 for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
|
xue@1
|
2919 }
|
xue@1
|
2920
|
xue@1
|
2921 //compute -<s, v'> over the lth frame
|
xue@1
|
2922 cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
|
xue@1
|
2923
|
xue@1
|
2924 //compute <sr, v> over the lth frame
|
xue@1
|
2925 cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
|
xue@1
|
2926 for (int i=0; i<I; i++)
|
xue@1
|
2927 for (int m=0; m<M; m++)
|
xue@1
|
2928 shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
|
xue@1
|
2929 //memset(srv[l][0], 0, sizeof(cdouble)*I*N);
|
xue@1
|
2930 MultiplyXY(I, M, N, srv[l], shv1, A[l]);
|
xue@1
|
2931 MultiAddXY(I, M, N, srv[l], shv2, A[l+1]);
|
xue@1
|
2932
|
xue@1
|
2933 #ifdef ERROR_CHECK
|
xue@1
|
2934 //error check: <s', v>=-<s, v'>
|
xue@1
|
2935 if (ds)
|
xue@1
|
2936 {
|
xue@1
|
2937 cdouble* lds=&ds[l*T];
|
xue@1
|
2938 for (int i=0; i<I && l*I+1<36; i++)
|
xue@1
|
2939 {
|
xue@1
|
2940 cdouble lsv=Inner(2*T, lds, v[i]); //compute <s', v[i]>
|
xue@1
|
2941 //cdouble* ls=&s[l*T];
|
xue@1
|
2942 //cdouble lsv2=Inner(2*T, ls, dv[i]);
|
xue@1
|
2943 dsv1[l*I+i]=lsv-sv[l][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
|
xue@1
|
2944 }
|
xue@1
|
2945
|
xue@1
|
2946 //error check: srv[l]*pq=<s',v>
|
xue@1
|
2947 for (int i=0; i<I && l*I+i<36; i++)
|
xue@1
|
2948 {
|
xue@1
|
2949 cdouble lsv=0;
|
xue@1
|
2950 for (int n=0; n<N; n++) lsv+=srv[l][i][n]*aita[n];
|
xue@1
|
2951 dsv2[l*I+i]=lsv-sv[l][i]-dsv1[l*I+i];
|
xue@1
|
2952 }
|
xue@1
|
2953 }
|
xue@1
|
2954 #endif
|
xue@1
|
2955 }
|
xue@1
|
2956 L_1=L-1;
|
xue@1
|
2957 if (endmode==1 || endmode==3)
|
xue@1
|
2958 {
|
xue@1
|
2959 //v from u given f[l]
|
xue@1
|
2960 int hT=T/2;
|
xue@1
|
2961 double fbin=floor((f[0]+f[1])*hT)/T;
|
xue@1
|
2962 double omg=fbin*2*M_PI;
|
xue@1
|
2963 cdouble jomg=cdouble(0, omg);
|
xue@1
|
2964 for (int c=0; c<T; c++)
|
xue@1
|
2965 {
|
xue@1
|
2966 double t=c-hT;
|
xue@1
|
2967 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
2968 for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
|
xue@1
|
2969 for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
|
xue@1
|
2970 }
|
xue@1
|
2971
|
xue@1
|
2972 //compute -<s, v'> over the lth frame
|
xue@1
|
2973 cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
2974
|
xue@1
|
2975 //compute <sr, v> over the lth frame
|
xue@1
|
2976 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
2977 //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
|
xue@1
|
2978 MultiplyXY(I, M, N, srv[L_1], shv1, A[0]);
|
xue@1
|
2979 #ifdef ERROR_CHECK
|
xue@1
|
2980 //error check: <s', v>=-<s, v'>
|
xue@1
|
2981 if (ds)
|
xue@1
|
2982 {
|
xue@1
|
2983 cdouble* lds=&ds[0];
|
xue@1
|
2984 for (int i=0; i<I && L_1*I+1<36; i++)
|
xue@1
|
2985 {
|
xue@1
|
2986 cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
|
xue@1
|
2987 //cdouble* ls=&s[l*T];
|
xue@1
|
2988 //cdouble lsv2=Inner(2*T, ls, dv[i]);
|
xue@1
|
2989 dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
|
xue@1
|
2990 }
|
xue@1
|
2991
|
xue@1
|
2992 //error check: srv[l]*pq=<s',v>
|
xue@1
|
2993 for (int i=0; i<I && L_1*I+i<36; i++)
|
xue@1
|
2994 {
|
xue@1
|
2995 cdouble lsv=0;
|
xue@1
|
2996 for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
|
xue@1
|
2997 dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
|
xue@1
|
2998 }
|
xue@1
|
2999 }
|
xue@1
|
3000 #endif
|
xue@1
|
3001 L_1++;
|
xue@1
|
3002 }
|
xue@1
|
3003 if (endmode==2 || endmode==3)
|
xue@1
|
3004 {
|
xue@1
|
3005 //v from u given f[l]
|
xue@1
|
3006 int hT=T/2;
|
xue@1
|
3007 double fbin=floor((f[L-1]+f[L])*hT)/T;
|
xue@1
|
3008 double omg=fbin*2*M_PI;
|
xue@1
|
3009 cdouble jomg=cdouble(0, omg);
|
xue@1
|
3010 for (int c=0; c<T; c++)
|
xue@1
|
3011 {
|
xue@1
|
3012 double t=c-hT;
|
xue@1
|
3013 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
3014 for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
|
xue@1
|
3015 for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
|
xue@1
|
3016 }
|
xue@1
|
3017
|
xue@1
|
3018 //compute -<s, v'> over the lth frame
|
xue@1
|
3019 cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
3020
|
xue@1
|
3021 //compute <sr, v> over the lth frame
|
xue@1
|
3022 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
3023 //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
|
xue@1
|
3024 MultiplyXY(I, M, N, srv[L_1], shv1, A[L-1]);
|
xue@1
|
3025 #ifdef ERROR_CHECK
|
xue@1
|
3026 //error check: <s', v>=-<s, v'>
|
xue@1
|
3027 if (ds)
|
xue@1
|
3028 {
|
xue@1
|
3029 cdouble* lds=&ds[(L-1)*T];
|
xue@1
|
3030 for (int i=0; i<I && L_1*I+1<36; i++)
|
xue@1
|
3031 {
|
xue@1
|
3032 cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
|
xue@1
|
3033 //cdouble* ls=&s[l*T];
|
xue@1
|
3034 //cdouble lsv2=Inner(2*T, ls, dv[i]);
|
xue@1
|
3035 dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
|
xue@1
|
3036 }
|
xue@1
|
3037
|
xue@1
|
3038 //error check: srv[l]*pq=<s',v>
|
xue@1
|
3039 for (int i=0; i<I && L_1*I+i<36; i++)
|
xue@1
|
3040 {
|
xue@1
|
3041 cdouble lsv=0;
|
xue@1
|
3042 for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
|
xue@1
|
3043 dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
|
xue@1
|
3044 }
|
xue@1
|
3045 }
|
xue@1
|
3046 #endif
|
xue@1
|
3047 L_1++;
|
xue@1
|
3048 }
|
xue@1
|
3049
|
xue@1
|
3050 if (L_1*2*I==2*N) GECP(N, aita, srv[0], sv[0]);
|
xue@1
|
3051 else LSLinear(L_1*I, N, aita, srv[0], sv[0]);
|
xue@1
|
3052
|
xue@1
|
3053 delete mlist;
|
xue@1
|
3054 }//DerivativePiecewise
|
xue@1
|
3055
|
Chris@5
|
3056 /**
|
xue@1
|
3057 function DerivativePiecewise2: Piecewise derivative algorithm in which the real and imaginary parts of
|
xue@1
|
3058 the exponent are modelled separately. In this implementation of the piecewise method the test
|
xue@1
|
3059 functions v are constructed from I "basic" (single-frame) test functions, each covering the same
|
xue@1
|
3060 period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
|
xue@1
|
3061 used.
|
xue@1
|
3062
|
xue@1
|
3063 In: s[LT+1]: waveform data
|
xue@1
|
3064 ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
|
xue@1
|
3065 L, T: number and length of pieces.
|
xue@1
|
3066 N: number of independent coefficients
|
xue@1
|
3067 h[M][T]: piecewise basis functions
|
xue@1
|
3068 A[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
|
xue@1
|
3069 B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
|
xue@1
|
3070 u[I][2T}, du[I][2T]: base-band test functions
|
xue@1
|
3071 f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
|
xue@1
|
3072 endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
3073 Out: p[Np], q[Nq]: independent coefficients
|
xue@1
|
3074
|
xue@1
|
3075 No return value.
|
xue@1
|
3076 */
|
xue@1
|
3077 void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B,
|
xue@1
|
3078 int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds)
|
xue@1
|
3079 {
|
xue@1
|
3080 MList* mlist=new MList;
|
xue@1
|
3081 int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
|
xue@1
|
3082 cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
|
xue@1
|
3083 cdouble** Allocate2(cdouble, I, T*2, v);
|
xue@1
|
3084 cdouble** Allocate2(cdouble, I, T*2, dv);
|
xue@1
|
3085 //compute <sr, v>
|
xue@1
|
3086 cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
|
xue@1
|
3087 cdouble*** srbv;
|
xue@1
|
3088 if (Np==Nq && B==A) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
|
xue@1
|
3089 cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
|
xue@1
|
3090 cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
|
xue@1
|
3091
|
xue@1
|
3092 for (int l=0; l<L-1; l++)
|
xue@1
|
3093 {
|
xue@1
|
3094 //v from u given f[l]
|
xue@1
|
3095 double fbin=floor(f[l+1]*T*2)/(T*2.0);
|
xue@1
|
3096 double omg=fbin*2*M_PI;
|
xue@1
|
3097 cdouble jomg=cdouble(0, omg);
|
xue@1
|
3098 for (int c=0; c<T*2; c++)
|
xue@1
|
3099 {
|
xue@1
|
3100 double t=c-T;
|
xue@1
|
3101 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
3102 for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
|
xue@1
|
3103 for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
|
xue@1
|
3104 }
|
xue@1
|
3105
|
xue@1
|
3106 //compute -<s, v'> over the lth frame
|
xue@1
|
3107 cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
|
xue@1
|
3108
|
xue@1
|
3109 //compute <sr, v> over the lth frame
|
xue@1
|
3110 cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
|
xue@1
|
3111 for (int i=0; i<I; i++)
|
xue@1
|
3112 for (int m=0; m<M; m++)
|
xue@1
|
3113 shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
|
xue@1
|
3114 memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3115 MultiplyXY(I, M, Np, srav[l], shv1, A[l]);
|
xue@1
|
3116 MultiAddXY(I, M, Np, srav[l], shv2, A[l+1]);
|
xue@1
|
3117 if (srbv!=srav) //so that either B!=A or Np!=Nq
|
xue@1
|
3118 {
|
xue@1
|
3119 //memset(srbv[l][0], 0, sizeof(cdouble)*I*Nq);
|
xue@1
|
3120 MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
|
xue@1
|
3121 MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
|
xue@1
|
3122 }
|
xue@1
|
3123 }
|
xue@1
|
3124 L_1=L-1;
|
xue@1
|
3125 if (endmode==1 || endmode==3)
|
xue@1
|
3126 {
|
xue@1
|
3127 //v from u given f[l]
|
xue@1
|
3128 int hT=T/2;
|
xue@1
|
3129 double fbin=floor((f[0]+f[1])*hT)/T;
|
xue@1
|
3130 double omg=fbin*2*M_PI;
|
xue@1
|
3131 cdouble jomg=cdouble(0, omg);
|
xue@1
|
3132 for (int c=0; c<T; c++)
|
xue@1
|
3133 {
|
xue@1
|
3134 double t=c-hT;
|
xue@1
|
3135 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
3136 for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
|
xue@1
|
3137 for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
|
xue@1
|
3138 }
|
xue@1
|
3139
|
xue@1
|
3140 //compute -<s, v'> over the lth frame
|
xue@1
|
3141 cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
3142
|
xue@1
|
3143 //compute <sr, v> over the lth frame
|
xue@1
|
3144 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
3145 //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3146 MultiplyXY(I, M, Np, srav[L_1], shv1, A[0]);
|
xue@1
|
3147 if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
|
xue@1
|
3148 L_1++;
|
xue@1
|
3149 }
|
xue@1
|
3150 if (endmode==2 || endmode==3)
|
xue@1
|
3151 {
|
xue@1
|
3152 //v from u given f[l]
|
xue@1
|
3153 int hT=T/2;
|
xue@1
|
3154 double fbin=floor((f[L-1]+f[L])*hT)/T;
|
xue@1
|
3155 double omg=fbin*2*M_PI;
|
xue@1
|
3156 cdouble jomg=cdouble(0, omg);
|
xue@1
|
3157 for (int c=0; c<T; c++)
|
xue@1
|
3158 {
|
xue@1
|
3159 double t=c-hT;
|
xue@1
|
3160 cdouble rot=polar(1.0, omg*t);
|
xue@1
|
3161 for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
|
xue@1
|
3162 for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
|
xue@1
|
3163 }
|
xue@1
|
3164
|
xue@1
|
3165 //compute -<s, v'> over the lth frame
|
xue@1
|
3166 cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
3167
|
xue@1
|
3168 //compute <sr, v> over the lth frame
|
xue@1
|
3169 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
3170 memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3171 MultiplyXY(I, M, Np, srav[L_1], shv1, A[L-1]);
|
xue@1
|
3172 if (srbv!=srav)
|
xue@1
|
3173 {
|
xue@1
|
3174 //memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);
|
xue@1
|
3175 MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
|
xue@1
|
3176 }
|
xue@1
|
3177 L_1++;
|
xue@1
|
3178 }
|
xue@1
|
3179
|
xue@1
|
3180 //real implementation of <sr,v>aita=<s',v>
|
xue@1
|
3181 double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
|
xue@1
|
3182 for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
|
xue@1
|
3183 {
|
xue@1
|
3184 int li=l*I+i, li_H=li+L_1*I;
|
xue@1
|
3185 for (int n=0; n<Np; n++)
|
xue@1
|
3186 {
|
xue@1
|
3187 AM[li][n]=srav[l][i][n].x;
|
xue@1
|
3188 AM[li_H][n]=srav[l][i][n].y;
|
xue@1
|
3189 }
|
xue@1
|
3190 for (int n=0; n<Nq; n++)
|
xue@1
|
3191 {
|
xue@1
|
3192 AM[li][Np+n]=-srbv[l][i][n].y;
|
xue@1
|
3193 AM[li_H][Np+n]=srbv[l][i][n].x;
|
xue@1
|
3194 }
|
xue@1
|
3195 }
|
xue@1
|
3196 //least-square solution of (srv)(aita)=(sv)
|
xue@1
|
3197 double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
|
xue@1
|
3198 double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y;
|
xue@1
|
3199
|
xue@1
|
3200 if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
|
xue@1
|
3201 else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);
|
xue@1
|
3202
|
xue@1
|
3203 memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);
|
xue@1
|
3204
|
xue@1
|
3205 delete mlist;
|
xue@1
|
3206 }//DerivativePiecewise2
|
xue@1
|
3207
|
xue@1
|
3208 /*
|
xue@1
|
3209 Error check: test that ds[LT] equals s[LT] times reconstructed R'. Notice that DA is D time A where D
|
xue@1
|
3210 is a pre-emphasis because p[Np] applies to log amplitude rather than its derivative.
|
xue@1
|
3211 */
|
xue@1
|
3212 double testds_pqA(int Np, double* p, int Nq, double* q, int L, int T, cdouble* s, cdouble* ds, int M, double** h, double** dh, double*** DA, double*** B, cdouble* errds=0)
|
xue@1
|
3213 {
|
xue@1
|
3214 double err=0, ene=0, *lamdax=new double[M*2], *lamday=&lamdax[M];
|
xue@1
|
3215 for (int l=0; l<L; l++)
|
xue@1
|
3216 {
|
xue@1
|
3217 MultiplyXy(M, Np, lamdax, DA[l], p);
|
xue@1
|
3218 MultiplyXy(M, Nq, lamday, B[l], q);
|
xue@1
|
3219 for (int t=0; t<T; t++)
|
xue@1
|
3220 {
|
xue@1
|
3221 double drtx=0; for (int m=0; m<M; m++) drtx+=lamdax[m]*h[m][t];
|
xue@1
|
3222 double drty=0; for (int m=0; m<M; m++) drty+=lamday[m]*h[m][t];
|
xue@1
|
3223 cdouble drt=cdouble(drtx, drty);
|
xue@1
|
3224 cdouble eds=ds[l*T+t]-s[l*T+t]*drt;
|
xue@1
|
3225 err+=~eds; ene+=~ds[l*T+t];
|
xue@1
|
3226 if (errds) errds[l*T+t]=eds;
|
xue@1
|
3227 }
|
xue@1
|
3228 }
|
xue@1
|
3229 delete[] lamdax;
|
xue@1
|
3230 return err/ene;
|
xue@1
|
3231 }//testds_pqA
|
xue@1
|
3232
|
xue@1
|
3233 /*
|
xue@1
|
3234 Error check: dsv1[I] tests that <s', v[I]> equals -<s, v[I]'>, dsv2[I] tests that <sr, v[I]>*pq=
|
xue@1
|
3235 <s',v[I]>
|
xue@1
|
3236 */
|
xue@1
|
3237 void testdsv(cdouble* dsv1, cdouble* dsv2, int Np, double* p, int Nq, double* q, int TT, cdouble* dsl, int I, cdouble** vl, cdouble* svl, cdouble** sravl, cdouble** srbvl)
|
xue@1
|
3238 {
|
xue@1
|
3239 for (int i=0; i<I; i++)
|
xue@1
|
3240 {
|
xue@1
|
3241 cdouble lsv=Inner(TT, dsl, vl[i]); //compute <s', v[i]>
|
xue@1
|
3242 //cdouble* ls=&s[l*T];
|
xue@1
|
3243 dsv1[i]=lsv-svl[i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
|
xue@1
|
3244 //sv[l][i]=lsv;
|
xue@1
|
3245 }
|
xue@1
|
3246 //error check: srv[l]*pq=<s',v>
|
xue@1
|
3247 for (int i=0; i<I; i++)
|
xue@1
|
3248 {
|
xue@1
|
3249 cdouble lsv=0;
|
xue@1
|
3250 for (int n=0; n<Np; n++) lsv+=sravl[i][n]*p[n];
|
xue@1
|
3251 for (int n=0; n<Nq; n++) lsv+=srbvl[i][n]*cdouble(0, q[n]);
|
xue@1
|
3252 dsv2[i]=lsv-svl[i]-dsv1[i];
|
xue@1
|
3253 }
|
xue@1
|
3254 }//testdsv
|
xue@1
|
3255
|
xue@1
|
3256 /*
|
xue@1
|
3257 Error check: tests A[MN]x[N1]=b[N1], returns square error
|
xue@1
|
3258 */
|
xue@1
|
3259 double testlinearsystem(int M, int N, double** A, double* x, double* b)
|
xue@1
|
3260 {
|
xue@1
|
3261 double err=0;
|
xue@1
|
3262 for (int m=0; m<M; m++)
|
xue@1
|
3263 {
|
xue@1
|
3264 double errli=Inner(N, A[m], x)-b[m];
|
xue@1
|
3265 err+=errli*errli;
|
xue@1
|
3266 }
|
xue@1
|
3267 return err;
|
xue@1
|
3268 }//testlinearsystem
|
xue@1
|
3269
|
xue@1
|
3270 /*
|
xue@1
|
3271 Error check: test the total square norm of <s, v>
|
xue@1
|
3272 */
|
xue@1
|
3273 double testsv(int L, double* f, int T, cdouble* s, int I, cdouble** u, cdouble** du, int endmode)
|
xue@1
|
3274 {
|
xue@1
|
3275 cdouble** Allocate2(cdouble, I, T*2, v);
|
xue@1
|
3276 cdouble** Allocate2(cdouble, I, T*2, dv);
|
xue@1
|
3277 double ene=0;
|
xue@1
|
3278 for (int l=0; l<L-1; l++)
|
xue@1
|
3279 {
|
xue@1
|
3280 //v from u given f[l]
|
xue@1
|
3281 setv(I, T*2, v, dv, f[l+1], u, du);
|
xue@1
|
3282 //compute -<s, v'> over the lth frame
|
xue@1
|
3283 cdouble* ls=&s[l*T];
|
xue@1
|
3284 for (int i=0; i<I; i++)
|
xue@1
|
3285 {
|
xue@1
|
3286 cdouble d=Inner(2*T, ls, v[i]);
|
xue@1
|
3287 ene+=~d;
|
xue@1
|
3288 }
|
xue@1
|
3289 }
|
xue@1
|
3290 if (endmode==1 || endmode==3)
|
xue@1
|
3291 {
|
xue@1
|
3292 //v from u given f[l]
|
xue@1
|
3293 setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
|
xue@1
|
3294 cdouble* ls=&s[0];
|
xue@1
|
3295 for (int i=0; i<I; i++)
|
xue@1
|
3296
|
xue@1
|
3297 ene+=~Inner(T, ls, v[i]);
|
xue@1
|
3298 }
|
xue@1
|
3299 if (endmode==2 || endmode==3)
|
xue@1
|
3300 {
|
xue@1
|
3301 //v from u given f[l]
|
xue@1
|
3302 setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
|
xue@1
|
3303 cdouble* ls=&s[(L-1)*T];
|
xue@1
|
3304 for (int i=0; i<I; i++)
|
xue@1
|
3305 ene+=~Inner(T, ls, v[i]);
|
xue@1
|
3306 }
|
xue@1
|
3307 DeAlloc2(v); DeAlloc2(dv);
|
xue@1
|
3308 return ene;
|
xue@1
|
3309 }//testsv
|
xue@1
|
3310
|
Chris@5
|
3311 /**
|
xue@1
|
3312 function DerivativePiecewise3: Piecewise derivative algorithm in which the log amplitude and frequeny
|
xue@1
|
3313 are modeled separately as piecewise functions. In this implementation of the piecewise method the test
|
xue@1
|
3314 functions v are constructed from I "basic" (single-frame) test functions, each covering the same
|
xue@1
|
3315 period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
|
xue@1
|
3316 used.
|
xue@1
|
3317
|
xue@1
|
3318 In: s[LT+1]: waveform data
|
xue@1
|
3319 ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
|
xue@1
|
3320 L, T: number and length of pieces.
|
xue@1
|
3321 N: number of independent coefficients
|
xue@1
|
3322 h[M][T]: piecewise basis functions
|
xue@1
|
3323 dh[M][T]: derivative of h[M][T], used only if ERROR_CHECK is defined.
|
xue@1
|
3324 DA[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
|
xue@1
|
3325 B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
|
xue@1
|
3326 u[I][2T}, du[I][2T]: base-band test functions
|
xue@1
|
3327 f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
|
xue@1
|
3328 endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
3329 Out: p[Np], q[Nq]: independent coefficients
|
xue@1
|
3330
|
xue@1
|
3331 No return value.
|
xue@1
|
3332 */
|
xue@1
|
3333 void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B,
|
xue@1
|
3334 int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds, double** dh)
|
xue@1
|
3335 {
|
xue@1
|
3336 MList* mlist=new MList;
|
xue@1
|
3337 int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
|
xue@1
|
3338 cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
|
xue@1
|
3339 cdouble** Allocate2L(cdouble, I, T*2, v, mlist);
|
xue@1
|
3340 cdouble** Allocate2L(cdouble, I, T*2, dv, mlist);
|
xue@1
|
3341 //compute <sr, v>
|
xue@1
|
3342 cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
|
xue@1
|
3343 cdouble*** srbv;
|
xue@1
|
3344 if (Np==Nq && B==DA) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
|
xue@1
|
3345 cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
|
xue@1
|
3346 cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
|
xue@1
|
3347
|
xue@1
|
3348 #ifdef ERROR_CHECK
|
xue@1
|
3349 cdouble dsv1in[128], dsv2in[128];
|
xue@1
|
3350 #endif
|
xue@1
|
3351
|
xue@1
|
3352 for (int l=0; l<L-1; l++)
|
xue@1
|
3353 {
|
xue@1
|
3354 //v from u given f[l]
|
xue@1
|
3355 setv(I, T*2, v, dv, f[l+1], u, du);
|
xue@1
|
3356 //compute -<s, v'> over the lth frame
|
xue@1
|
3357 cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
|
xue@1
|
3358
|
xue@1
|
3359 //compute <sr, v> over the lth frame
|
xue@1
|
3360 cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
|
xue@1
|
3361 for (int i=0; i<I; i++)
|
xue@1
|
3362 for (int m=0; m<M; m++)
|
xue@1
|
3363 shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
|
xue@1
|
3364 memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3365 MultiplyXY(I, M, Np, srav[l], shv1, DA[l]);
|
xue@1
|
3366 MultiAddXY(I, M, Np, srav[l], shv2, DA[l+1]);
|
xue@1
|
3367 if (srbv!=srav) //so that either B!=A or Np!=Nq
|
xue@1
|
3368 {
|
xue@1
|
3369 MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
|
xue@1
|
3370 MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
|
xue@1
|
3371 }
|
xue@1
|
3372 #ifdef ERROR_CHECK
|
xue@1
|
3373 //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
|
xue@1
|
3374 if (ds) testdsv(&dsv1in[l*I], &dsv2in[l*I], Np, p, Nq, q, T*2, &ds[l*T], I, v, sv[l], srav[l], srbv[l]);
|
xue@1
|
3375 #endif
|
xue@1
|
3376 }
|
xue@1
|
3377 L_1=L-1;
|
xue@1
|
3378 if (endmode==1 || endmode==3)
|
xue@1
|
3379 {
|
xue@1
|
3380 //v from u given f[l]
|
xue@1
|
3381 setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
|
xue@1
|
3382 //compute -<s, v'> over the lth frame
|
xue@1
|
3383 cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
3384 //compute <sr, v> over the lth frame
|
xue@1
|
3385 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
3386 //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3387 MultiplyXY(I, M, Np, srav[L_1], shv1, DA[0]);
|
xue@1
|
3388 if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
|
xue@1
|
3389 #ifdef ERROR_CHECK
|
xue@1
|
3390 //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
|
xue@1
|
3391 if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[0], I, v, sv[L_1], srav[L_1], srbv[L_1]);
|
xue@1
|
3392 #endif
|
xue@1
|
3393 L_1++;
|
xue@1
|
3394 }
|
xue@1
|
3395 if (endmode==2 || endmode==3)
|
xue@1
|
3396 {
|
xue@1
|
3397 //v from u given f[l]
|
xue@1
|
3398 setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
|
xue@1
|
3399 //compute -<s, v'> over the lth frame
|
xue@1
|
3400 cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
|
xue@1
|
3401 //compute <sr, v> over the lth frame
|
xue@1
|
3402 for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
|
xue@1
|
3403 memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
|
xue@1
|
3404 MultiplyXY(I, M, Np, srav[L_1], shv1, DA[L-1]);
|
xue@1
|
3405 if (srbv!=srav) MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
|
xue@1
|
3406 #ifdef ERROR_CHECK
|
xue@1
|
3407 //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
|
xue@1
|
3408 if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[(L-1)*T], I, v, sv[L_1], srav[L_1], srbv[L_1]);
|
xue@1
|
3409 #endif
|
xue@1
|
3410 L_1++;
|
xue@1
|
3411 }
|
xue@1
|
3412
|
xue@1
|
3413 //real implementation of <sr,v>aita=<s',v>
|
xue@1
|
3414 double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
|
xue@1
|
3415 for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
|
xue@1
|
3416 {
|
xue@1
|
3417 int li=l*I+i, li_H=li+L_1*I;
|
xue@1
|
3418 for (int n=0; n<Np; n++)
|
xue@1
|
3419 {
|
xue@1
|
3420 AM[li][n]=srav[l][i][n].x;
|
xue@1
|
3421 AM[li_H][n]=srav[l][i][n].y;
|
xue@1
|
3422 }
|
xue@1
|
3423 for (int n=0; n<Nq; n++)
|
xue@1
|
3424 {
|
xue@1
|
3425 AM[li][Np+n]=-srbv[l][i][n].y;
|
xue@1
|
3426 AM[li_H][Np+n]=srbv[l][i][n].x;
|
xue@1
|
3427 }
|
xue@1
|
3428 }
|
xue@1
|
3429 //least-square solution of (srv)(aita)=(sv)
|
xue@1
|
3430 double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
|
xue@1
|
3431 double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y; mlist->Add(b, 1);
|
xue@1
|
3432 #ifdef ERROR_CHECK
|
xue@1
|
3433 //tests that AM is invariant to a constant shift of p
|
xue@1
|
3434 double errAM=0, errAM2=0, err1, err2;
|
xue@1
|
3435 for (int l=0; l<L_1*I; l++){double errli=0; for (int n=0; n<Np; n++) errli+=AM[l][n]; errAM+=errli*errli; errli=0; for (int n=0; n<Np; n+=2) errli+=AM[l][n]; errAM2+=errli*errli;}
|
xue@1
|
3436 //test square error of the input pq
|
xue@1
|
3437 if (ds)
|
xue@1
|
3438 {
|
xue@1
|
3439 memcpy(pq, p, sizeof(double)*Np); memcpy(&pq[Np], q, sizeof(double)*Nq);
|
xue@1
|
3440 err1=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
|
xue@1
|
3441 }
|
xue@1
|
3442 //test error of s'-sR where R is synthesized from the input pq
|
xue@1
|
3443 double errdsin, errdsvin; cdouble* edsin;
|
xue@1
|
3444 if (ds && dh)
|
xue@1
|
3445 {
|
xue@1
|
3446 edsin=new cdouble[L*T]; mlist->Add(edsin, 1);
|
xue@1
|
3447 errdsin=testds_pqA(Np, p, Nq, q, L, T, s, ds, M, h, dh, DA, B, edsin);
|
xue@1
|
3448 errdsvin=testsv(L, f, T, edsin, I, u, du, endmode);
|
xue@1
|
3449 }
|
xue@1
|
3450 #endif
|
xue@1
|
3451 Alloc2L(L_1*I*2, Np+Nq-1, Am, mlist);
|
xue@1
|
3452 for (int l=0; l<L_1*I*2; l++) memcpy(Am[l], &AM[l][1], sizeof(double)*(Np+Nq-1));
|
xue@1
|
3453 pq[0]=0;
|
xue@1
|
3454 //if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
|
xue@1
|
3455 //else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);
|
xue@1
|
3456 if (L_1*2*I==Np+Nq-1) GECP(Np+Nq-1, &pq[1], Am, b);
|
xue@1
|
3457 else LSLinear(2*L_1*I, Np+Nq-1, &pq[1], Am, b);
|
xue@1
|
3458 #ifdef ERROR_CHECK
|
xue@1
|
3459 //test square error of output pq
|
xue@1
|
3460 if (ds) err2=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
|
xue@1
|
3461 //test error of s'-sR of the output pq
|
xue@1
|
3462 double errdsout, errdsvout; cdouble* edsout;
|
xue@1
|
3463 if (ds && dh)
|
xue@1
|
3464 {
|
xue@1
|
3465 edsout=new cdouble[L*T]; mlist->Add(edsout, 1);
|
xue@1
|
3466 errdsout=testds_pqA(Np, pq, Nq, &pq[Np], L, T, s, ds, M, h, dh, DA, B, edsout);
|
xue@1
|
3467 errdsvout=testsv(L, f, T, edsout, I, u, du, endmode);
|
xue@1
|
3468 }
|
xue@1
|
3469 #endif
|
xue@1
|
3470 memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);
|
xue@1
|
3471
|
xue@1
|
3472 delete mlist;
|
xue@1
|
3473 }//DerivativePiecewise3
|
xue@1
|
3474
|
xue@1
|
3475 //initialization routines for the piecewise derivative method
|
xue@1
|
3476
|
Chris@5
|
3477 /**
|
xue@1
|
3478 function seth: set h[M] to a series of power functions.
|
xue@1
|
3479
|
xue@1
|
3480 In: M, T.
|
xue@1
|
3481 Out: h[M][T], where h[m] is power function of order m.
|
xue@1
|
3482
|
xue@1
|
3483 No return value. h is allocated anew and must be freed by caller.
|
xue@1
|
3484 */
|
xue@1
|
3485 void seth(int M, int T, double**& h, MList* mlist)
|
xue@1
|
3486 {
|
xue@1
|
3487 if (M<=0){h=0; return;}
|
xue@1
|
3488 Allocate2L(double, M, T, h, mlist);
|
xue@1
|
3489 double* hm=h[0]; for (int t=0; t<T; t++) hm[t]=1;
|
xue@1
|
3490 for (int m=1; m<M; m++)
|
xue@1
|
3491 {
|
xue@1
|
3492 hm=h[m]; for (int t=0; t<T; t++) hm[t]=pow(t*1.0, m);
|
xue@1
|
3493 }
|
xue@1
|
3494 }//seth
|
xue@1
|
3495
|
Chris@5
|
3496 /**
|
xue@1
|
3497 function setdh: set dh[M] to the derivative of a series of power functions.
|
xue@1
|
3498
|
xue@1
|
3499 In: M, T.
|
xue@1
|
3500 Out: dh[M][T], where dh[m] is derivative of the power function of order m.
|
xue@1
|
3501
|
xue@1
|
3502 No return value. dh is allocated anew and must be freed by caller.
|
xue@1
|
3503 */
|
xue@1
|
3504 void setdh(int M, int T, double**& dh, MList* mlist)
|
xue@1
|
3505 {
|
xue@1
|
3506 if (M<=0){dh=0; return;}
|
xue@1
|
3507 Allocate2L(double, M, T, dh, mlist);
|
xue@1
|
3508 double* dhm=dh[0]; memset(dhm, 0, sizeof(double)*T);
|
xue@1
|
3509 if (M>1){dhm=dh[1]; for (int t=0; t<T; t++) dhm[t]=1;}
|
xue@1
|
3510 for (int m=2; m<M; m++)
|
xue@1
|
3511 {
|
xue@1
|
3512 dhm=dh[m]; for (int t=0; t<T; t++) dhm[t]=m*pow(t*1.0, m-1);
|
xue@1
|
3513 }
|
xue@1
|
3514 }//setdh
|
xue@1
|
3515
|
Chris@5
|
3516 /**
|
xue@1
|
3517 function setdih: set dih[M] to the difference of the integral of a series of power functions.
|
xue@1
|
3518
|
xue@1
|
3519 In: M, I
|
xue@1
|
3520 Out: dih[M][I], where the accumulation of dih[m] is the integral of the power function of order m.
|
xue@1
|
3521
|
xue@1
|
3522 No return value. dih is allocated anew and must be freed by caller.
|
xue@1
|
3523 */
|
xue@1
|
3524 void setdih(int M, int T, double**& dih, MList* mlist)
|
xue@1
|
3525 {
|
xue@1
|
3526 if (M<=0){dih=0; return;}
|
xue@1
|
3527 Allocate2L(double, M, T, dih, mlist);
|
xue@1
|
3528 double* dihm=dih[0]; for (int t=0; t<T; t++) dihm[t]=1;
|
xue@1
|
3529 for (int m=1; m<M; m++)
|
xue@1
|
3530 {
|
xue@1
|
3531 dihm=dih[m]; for (int t=0; t<T; t++) dihm[t]=(pow(t+1.0, m+1)-pow(t*1.0, m+1))/(m+1);
|
xue@1
|
3532 }
|
xue@1
|
3533 }//setdih
|
xue@1
|
3534
|
Chris@5
|
3535 /**
|
xue@1
|
3536 function sshLinear: sets M and h[M] for the linear spline model
|
xue@1
|
3537
|
xue@1
|
3538 In: T
|
xue@1
|
3539 Out: M=2, h[2][T] filled out for linear spline model.
|
xue@1
|
3540
|
xue@1
|
3541 No return value. h is allocated anew and must be freed by caller.
|
xue@1
|
3542 */
|
xue@1
|
3543 void sshLinear(int T, int& M, double** &h, MList* mlist)
|
xue@1
|
3544 {
|
xue@1
|
3545 M=2; Allocate2L(double, M, T, h, mlist);
|
xue@1
|
3546 for (int t=0; t<T; t++) h[0][t]=1, h[1][t]=t;
|
xue@1
|
3547 }//sshLinear
|
xue@1
|
3548
|
Chris@5
|
3549 /**
|
xue@1
|
3550 function sdihLinear: sets dih[M] for the linear spline model. For testing only.
|
xue@1
|
3551
|
xue@1
|
3552 In: T
|
xue@1
|
3553 Out: dih[2][T] filled out for linear spline model.
|
xue@1
|
3554
|
xue@1
|
3555 No return value. dih is allocated anew and must be freed by caller.
|
xue@1
|
3556 */
|
xue@1
|
3557 void sdihLinear(int T, double**& dih, MList* mlist)
|
xue@1
|
3558 {
|
xue@1
|
3559 Allocate2L(double, 2, T, dih, mlist);
|
xue@1
|
3560 for (int t=0; t<T; t++) dih[0][t]=1, dih[1][t]=t+0.5;
|
xue@1
|
3561 }//sdihLinear
|
xue@1
|
3562
|
Chris@5
|
3563 /**
|
xue@1
|
3564 function sshCubic: sets M and h[M] for cubic spline models.
|
xue@1
|
3565
|
xue@1
|
3566 In: T
|
xue@1
|
3567 Out: M=4 and h[M] filled out for cubic spline models, including cubic and cubic-Hermite.
|
xue@1
|
3568
|
xue@1
|
3569 No return value. h is allocated anew and must be freed by caller.
|
xue@1
|
3570 */
|
xue@1
|
3571 void sshCubic(int T, int& M, double** &h, MList* mlist)
|
xue@1
|
3572 {
|
xue@1
|
3573 M=4; Allocate2L(double, M, T, h, mlist);
|
xue@1
|
3574 for (int t=0; t<T; t++) h[3][t]=t*t*t, h[2][t]=t*t, h[1][t]=t, h[0][t]=1;
|
xue@1
|
3575 }//sshCubic
|
xue@1
|
3576
|
Chris@5
|
3577 /**
|
xue@1
|
3578 function sdihCubic: sets dih[M] for cubic spline models.
|
xue@1
|
3579
|
xue@1
|
3580 In: T
|
xue@1
|
3581 Out: dih[4] filled out for cubic spline models.
|
xue@1
|
3582
|
xue@1
|
3583 No return value. dih is allocated anew and must be freed by caller.
|
xue@1
|
3584 */
|
xue@1
|
3585 void sdihCubic(int T, double** &dih, MList* mlist)
|
xue@1
|
3586 {
|
xue@1
|
3587 Allocate2L(double, 4, T, dih, mlist);
|
xue@1
|
3588 for (int t=0; t<T; t++)
|
xue@1
|
3589 {
|
xue@1
|
3590 dih[3][t]=t*(t*(t+1.5)+1)+0.25, dih[2][t]=t*(t+1)+1.0/3, dih[1][t]=t+0.5, dih[0][t]=1;
|
xue@1
|
3591 }
|
xue@1
|
3592 }//sdihCubic*/
|
xue@1
|
3593
|
Chris@5
|
3594 /**
|
xue@1
|
3595 function ssALinearSpline: sets N and A[L] for the linear spline model
|
xue@1
|
3596
|
xue@1
|
3597 In: L, M, T
|
xue@1
|
3598 Out: N=L+1, A[L][M][N] filled out for the linear spline model
|
xue@1
|
3599
|
xue@1
|
3600 No return value. A is created anew and bust be freed by caller.
|
xue@1
|
3601 */
|
xue@1
|
3602 void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
|
xue@1
|
3603 {
|
xue@1
|
3604 N=L+1;
|
xue@1
|
3605 Allocate3L(double, L, M, N, A, mlist);
|
xue@1
|
3606 memset(A[0][0], 0, sizeof(double)*L*M*N);
|
xue@1
|
3607 double iT=1.0/T; for (int l=0; l<L; l++) A[l][0][l]=1, A[l][1][l]=-iT, A[l][1][l+1]=iT;
|
xue@1
|
3608 }//ssALinearSpline
|
xue@1
|
3609
|
Chris@5
|
3610 /**
|
xue@1
|
3611 function ssLinearSpline: sets M, N, h and A for the linear spline model
|
xue@1
|
3612
|
xue@1
|
3613 In: L, M, T
|
xue@1
|
3614 Out: N and h[][] and A[][][] filled out for the linear spline model
|
xue@1
|
3615
|
xue@1
|
3616 No reutrn value. A and h are created anew and bust be freed by caller.
|
xue@1
|
3617 */
|
xue@1
|
3618 void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode)
|
xue@1
|
3619 {
|
xue@1
|
3620 seth(M, T, h, mlist);
|
xue@1
|
3621 ssALinearSpline(L, T, M, N, A, mlist);
|
xue@1
|
3622 }//ssLinearSpline
|
xue@1
|
3623
|
Chris@5
|
3624 /**
|
xue@1
|
3625 function ssACubicHermite: sets N and A[L] for cubic Hermite spline model
|
xue@1
|
3626
|
xue@1
|
3627 In: L, M, T
|
xue@1
|
3628 Out: N=2(L+1), A[L][M][N] filled out for the cubic Hermite spline
|
xue@1
|
3629
|
xue@1
|
3630 No return value. A is created anew and must be freed by caller.
|
xue@1
|
3631 */
|
xue@1
|
3632 void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
|
xue@1
|
3633 {
|
xue@1
|
3634 N=2*(L+1);
|
xue@1
|
3635 Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
|
xue@1
|
3636 double iT=1.0/T, iT2=iT*iT, iT3=iT2*iT;
|
xue@1
|
3637 for (int l=0; l<L; l++)
|
xue@1
|
3638 {
|
xue@1
|
3639 A[l][3][2*l]=2*iT3; A[l][3][2*l+2]=-2*iT3; A[l][3][2*l+1]=A[l][3][2*l+3]=iT2;
|
xue@1
|
3640 A[l][2][2*l]=-3*iT2; A[l][2][2*l+1]=-2*iT; A[l][2][2*l+2]=3*iT2; A[l][2][2*l+3]=-iT;
|
xue@1
|
3641 A[l][1][2*l+1]=1;
|
xue@1
|
3642 A[l][0][2*l]=1;
|
xue@1
|
3643 }
|
xue@1
|
3644 }//ssACubicHermite
|
xue@1
|
3645
|
Chris@5
|
3646 /**
|
xue@1
|
3647 function ssLinearSpline: sets M, N, h and A for the cubic Hermite spline model
|
xue@1
|
3648
|
xue@1
|
3649 In: L, M, T
|
xue@1
|
3650 Out: N and h[][] and A[][][] filled out for the cubic Hermite spline model
|
xue@1
|
3651
|
xue@1
|
3652 No reutrn value. A and h are created anew and bust be freed by caller.
|
xue@1
|
3653 */
|
xue@1
|
3654 void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
|
xue@1
|
3655 {
|
xue@1
|
3656 seth(M, T, h, mlist);
|
xue@1
|
3657 ssACubicHermite(L, T, M, N, A, mlist);
|
xue@1
|
3658 }//ssCubicHermite
|
xue@1
|
3659
|
Chris@5
|
3660 /**
|
xue@1
|
3661 function ssACubicSpline: sets N and A[L] for cubic spline model
|
xue@1
|
3662
|
xue@1
|
3663 In: L, M, T
|
xue@1
|
3664 mode: boundary mode of cubic spline, 0=natural, 1=quadratic run-out, 2=cubic run-out
|
xue@1
|
3665 Out: N=2(L+1), A[L][M][N] filled out for the cubic spline
|
xue@1
|
3666
|
xue@1
|
3667 No return value. A is created anew and must be freed by caller.
|
xue@1
|
3668 */
|
xue@1
|
3669 void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
|
xue@1
|
3670 {
|
xue@1
|
3671 N=L+1;
|
xue@1
|
3672 Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
|
xue@1
|
3673 Alloc2(L+1, L+1, ML); memset(ML[0], 0, sizeof(double)*(L+1)*(L+1));
|
xue@1
|
3674 Alloc2(L+1, L+1, MR); memset(MR[0], 0, sizeof(double)*(L+1)*(L+1));
|
xue@1
|
3675 //fill in ML and MR. The only difference between various cubic splines are ML.
|
xue@1
|
3676 double _6iT2=6.0/(T*T);
|
xue@1
|
3677 ML[0][0]=ML[L][L]=1;
|
xue@1
|
3678 for (int l=1; l<L; l++) ML[l][l-1]=ML[l][l+1]=1, ML[l][l]=4,
|
xue@1
|
3679 MR[l][l-1]=MR[l][l+1]=_6iT2, MR[l][l]=-2*_6iT2;
|
xue@1
|
3680 if (mode==0){} //no more coefficients are needed for natural cubic spline
|
xue@1
|
3681 else if (mode==1) ML[0][1]=ML[L][L-1]=-1; //setting for quadratic run-out
|
xue@1
|
3682 else if (mode==2) ML[0][1]=ML[L][L-1]=-2, ML[0][2]=ML[L][L-2]=1; //setting for cubic run-out
|
xue@1
|
3683 GICP(L+1, ML);
|
xue@1
|
3684 double** MM=MultiplyXY(L+1, ML, ML, MR);
|
xue@1
|
3685 double iT=1.0/T;
|
xue@1
|
3686 Alloc2(4, 2, M42); M42[3][0]=-1.0/6/T, M42[3][1]=1.0/6/T, M42[2][0]=0.5, M42[2][1]=M42[0][0]=M42[0][1]=0, M42[1][0]=-T/3.0, M42[1][1]=-T/6.0;
|
xue@1
|
3687 for (int l=0; l<L; l++)
|
xue@1
|
3688 {
|
xue@1
|
3689 MultiplyXY(4, 2, N, A[l], M42, &MM[l]);
|
xue@1
|
3690 A[l][1][l]-=iT; A[l][1][l+1]+=iT; A[l][0][l]+=1;
|
xue@1
|
3691 }
|
xue@1
|
3692 DeAlloc2(ML); DeAlloc2(MR); DeAlloc2(M42);
|
xue@1
|
3693 }//ssACubicSpline
|
xue@1
|
3694
|
Chris@5
|
3695 /**
|
xue@1
|
3696 function ssLinearSpline: sets M, N, h and A for the cubic spline model
|
xue@1
|
3697
|
xue@1
|
3698 In: L, M, T
|
xue@1
|
3699 Out: N and h[][] and A[][][] filled out for the cubic spline model
|
xue@1
|
3700
|
xue@1
|
3701 No reutrn value. A and h are created anew and bust be freed by caller.
|
xue@1
|
3702 */
|
xue@1
|
3703 void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
|
xue@1
|
3704 {
|
xue@1
|
3705 seth(M, T, h, mlist);
|
xue@1
|
3706 ssACubicSpline(L, T, M, N, A, mlist, mode);
|
xue@1
|
3707 }//ssCubicSpline
|
xue@1
|
3708
|
Chris@5
|
3709 /**
|
xue@1
|
3710 function setu: sets u[I+1] as base-band windowed Fourier atoms, whose frequencies come in the order of
|
xue@1
|
3711 0, 1, -1, 2, -2, 3, -3, 4, etc, in bins.
|
xue@1
|
3712
|
xue@1
|
3713 In: I, Wid: number and size of atoms to generate.
|
xue@1
|
3714 WinOrder: order (=vanishing moment) of window function to use (2=Hann, 4=Hann^2, etc.)
|
xue@1
|
3715 Out: u[I+1][Wid], du[I+1]{Wid]: the I+1 atoms and their derivatives.
|
xue@1
|
3716
|
xue@1
|
3717 No return value. u and du are created anew and must be freed by caller.
|
xue@1
|
3718 */
|
xue@1
|
3719 void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder, MList* mlist)
|
xue@1
|
3720 {
|
xue@1
|
3721 Allocate2L(cdouble, I+1, Wid, u, mlist);
|
xue@1
|
3722 Allocate2L(cdouble, I+1, Wid, du, mlist);
|
xue@1
|
3723
|
xue@1
|
3724 double** wins=CosineWindows(WinOrder, Wid, (double**)0, 2);
|
xue@1
|
3725 double omg=2*M_PI/Wid; cdouble jomg=cdouble(0, omg);
|
xue@1
|
3726 for (int t=0; t<Wid; t++)
|
xue@1
|
3727 {
|
xue@1
|
3728 u[0][t]=wins[0][t], du[0][t]=wins[1][t];
|
xue@1
|
3729 int li=1;
|
xue@1
|
3730 for (int i=1; i<=I; i++)
|
xue@1
|
3731 {
|
xue@1
|
3732 cdouble rot=polar(1.0, li*omg*t);
|
xue@1
|
3733 u[i][t]=u[0][t]*rot; du[i][t]=du[0][t]*rot+jomg*li*u[i][t];
|
xue@1
|
3734 li=-li; if (li>0) li++;
|
xue@1
|
3735 }
|
xue@1
|
3736 }
|
xue@1
|
3737 DeAlloc2(wins);
|
xue@1
|
3738 }//setu
|
xue@1
|
3739
|
Chris@5
|
3740 /**
|
xue@1
|
3741 function DerivativePiecewiseI: wrapper for DerivativePiecewise(), doing the initialization ,etc.
|
xue@1
|
3742
|
xue@1
|
3743 In: L, T: number and length of pieces
|
xue@1
|
3744 s[LT]: waveform signal
|
xue@1
|
3745 ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
|
xue@1
|
3746 f[L+1]: reference frequencies at knots
|
xue@1
|
3747 M: polynomial degree of piecewise approximation
|
xue@1
|
3748 SpecifyA, ssmode: pointer to a function that fills A[L], and mode argument to call it
|
xue@1
|
3749 WinOrder: order(=vanishing moment) of window used for constructing test functions
|
xue@1
|
3750 I: number of test functions per frame.
|
xue@1
|
3751 endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
3752 Out: aita[N]: independent coefficients, where N is specified by SpecifyA.
|
xue@1
|
3753
|
xue@1
|
3754 No return vlue.
|
xue@1
|
3755 */
|
xue@1
|
3756 void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M,
|
xue@1
|
3757 void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode,
|
xue@1
|
3758 int WinOrder, int I, int endmode, cdouble* ds)
|
xue@1
|
3759 {
|
xue@1
|
3760 MList* mlist=new MList;
|
xue@1
|
3761 cdouble **u, **du;
|
xue@1
|
3762 setu(I, 2*T, u, du, WinOrder, mlist);
|
xue@1
|
3763
|
xue@1
|
3764 int N; double **h, ***A;
|
xue@1
|
3765 seth(M, T, h, mlist);
|
xue@1
|
3766 SpecifyA(L, T, M, N, A, mlist, ssmode);
|
xue@1
|
3767
|
xue@1
|
3768 DerivativePiecewise(N, aita, L, f, T, s, A, M, h, I, u, du, endmode, ds);
|
xue@1
|
3769 delete mlist;
|
xue@1
|
3770 }//DerivativePiecewiseI
|
xue@1
|
3771
|
Chris@5
|
3772 /**
|
xue@1
|
3773 function DerivativePiecewiseII: wrapper for DerivativePiecewise2(), doing the initialization ,etc.
|
xue@1
|
3774 This models the derivative of log ampltiude and frequency as separate piecewise polynomials, the first
|
xue@1
|
3775 specified by SpecifyA, the second by SpecifyB.
|
xue@1
|
3776
|
xue@1
|
3777 In: L, T: number and length of pieces
|
xue@1
|
3778 s[LT]: waveform signal
|
xue@1
|
3779 ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
|
xue@1
|
3780 f[L+1]: reference frequencies at knots
|
xue@1
|
3781 M: polynomial degree of piecewise approximation
|
xue@1
|
3782 SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
|
xue@1
|
3783 SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
|
xue@1
|
3784 WinOrder: order(=vanishing moment) of window used for constructing test functions
|
xue@1
|
3785 I: number of test functions per frame.
|
xue@1
|
3786 endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
3787 Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.
|
xue@1
|
3788
|
xue@1
|
3789 No reutrn value.
|
xue@1
|
3790 */
|
xue@1
|
3791 void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
|
xue@1
|
3792 void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
|
xue@1
|
3793 void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
|
xue@1
|
3794 int WinOrder, int I, int endmode, cdouble* ds)
|
xue@1
|
3795 {
|
xue@1
|
3796 MList* mlist=new MList;
|
xue@1
|
3797 cdouble **u, **du;
|
xue@1
|
3798 setu(I, 2*T, u, du, WinOrder, mlist);
|
xue@1
|
3799
|
xue@1
|
3800 int Np, Nq;
|
xue@1
|
3801 double **h, ***A, ***B;
|
xue@1
|
3802 seth(M, T, h, mlist);
|
xue@1
|
3803 SpecifyA(L, T, M, Np, A, mlist, ssAmode);
|
xue@1
|
3804 SpecifyB(L, T, M, Nq, B, mlist, ssBmode);
|
xue@1
|
3805
|
xue@1
|
3806 DerivativePiecewise2(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds);
|
xue@1
|
3807
|
xue@1
|
3808 delete mlist;
|
xue@1
|
3809 }//DerivativePiecewiseII
|
xue@1
|
3810
|
Chris@5
|
3811 /**
|
xue@1
|
3812 function DerivativePiecewiseIII: wrapper for DerivativePiecewise3(), doing the initialization ,etc.
|
xue@1
|
3813 Notice that this time the log amplitude, rather than its derivative, is modeled as a piecewise
|
xue@1
|
3814 polynomial specified by SpecifyA.
|
xue@1
|
3815
|
xue@1
|
3816 In: L, T: number and length of pieces
|
xue@1
|
3817 s[LT]: waveform signal
|
xue@1
|
3818 ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
|
xue@1
|
3819 f[L+1]: reference frequencies at knots
|
xue@1
|
3820 M: polynomial degree of piecewise approximation
|
xue@1
|
3821 SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
|
xue@1
|
3822 SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
|
xue@1
|
3823 WinOrder: order(=vanishing moment) of window used for constructing test functions
|
xue@1
|
3824 I: number of test functions per frame.
|
xue@1
|
3825 endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
|
xue@1
|
3826 Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.
|
xue@1
|
3827
|
xue@1
|
3828 No reutrn value.
|
xue@1
|
3829 */
|
xue@1
|
3830 void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
|
xue@1
|
3831 void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
|
xue@1
|
3832 void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
|
xue@1
|
3833 int WinOrder, int I, int endmode, cdouble* ds)
|
xue@1
|
3834 {
|
xue@1
|
3835 MList* mlist=new MList;
|
xue@1
|
3836 int Np, Nq;
|
xue@1
|
3837 double **h, ***A, ***B, **dh=0;
|
xue@1
|
3838 cdouble **u, **du;
|
xue@1
|
3839 setu(I, T*2, u, du, WinOrder, mlist);
|
xue@1
|
3840 seth(M, T, h, mlist);
|
xue@1
|
3841 if (ds) setdh(M, T, dh, mlist);
|
xue@1
|
3842 SpecifyA(L, T, M, Np, A, mlist, ssAmode);
|
xue@1
|
3843 SpecifyB(L, T, M, Nq, B, mlist, ssBmode);
|
xue@1
|
3844 Alloc2L(M, M, DM, mlist);
|
xue@1
|
3845 memset(DM[0], 0, sizeof(double)*M*M); for (int m=0; m<M-1; m++) DM[m][m+1]=m+1;
|
xue@1
|
3846 double** DA=0;
|
xue@1
|
3847
|
xue@1
|
3848 for (int l=0; l<L; l++)
|
xue@1
|
3849 {
|
xue@1
|
3850 DA=MultiplyXY(M, M, Np, DA, DM, A[l], mlist);
|
xue@1
|
3851 Copy(M, Np, A[l], DA);
|
xue@1
|
3852 }
|
xue@1
|
3853
|
xue@1
|
3854 DerivativePiecewise3(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds, dh);
|
xue@1
|
3855
|
xue@1
|
3856 delete mlist;
|
xue@1
|
3857 }//DerivativePiecewiseIII
|
xue@1
|
3858
|
Chris@5
|
3859 /**
|
xue@1
|
3860 function AmpPhCorrectionExpA: model-preserving amplitude and phase correction in piecewise derivative
|
xue@1
|
3861 method.
|
xue@1
|
3862
|
xue@1
|
3863 In: aita[N]: inital independent coefficients
|
xue@1
|
3864 L, T: number and size of pieces
|
xue@1
|
3865 sre[LT]: waveform data
|
xue@1
|
3866 h[M][T], dih[M][T]: piecewise basis functions and their difference-integrals
|
xue@1
|
3867 A[L][M][N]: L coefficient mapping matrices
|
xue@1
|
3868 SpecifyA: pointer to the function used for constructing A
|
xue@1
|
3869 WinOrder: order(=vanishing moment) of window used for constructing test functions
|
xue@1
|
3870 Out: aita[N]: corrected independent coefficients
|
xue@1
|
3871 s2[LT]: reconstruct sinusoid BEFORE correction
|
xue@1
|
3872
|
xue@1
|
3873 Returns the estimate of phase angle at 0.
|
xue@1
|
3874 */
|
xue@1
|
3875 double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A,
|
xue@1
|
3876 void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder)
|
xue@1
|
3877 {
|
xue@1
|
3878 MList* mlist=new MList;
|
xue@1
|
3879 //*amplitude and phase correction
|
xue@1
|
3880 //amplitude is done by updating p, i.e. Re(aita)
|
xue@1
|
3881 double *s2ph=new double[L+1]; mlist->Add(s2ph, 1);
|
xue@1
|
3882 double *phcor=new double[L+1]; mlist->Add(phcor, 1);
|
xue@1
|
3883 cdouble* lamda=new cdouble[M]; mlist->Add(lamda, 1);
|
xue@1
|
3884 double* lamdax=new double[M]; mlist->Add(lamdax, 1);
|
xue@1
|
3885 double* lamday=new double[M]; mlist->Add(lamday, 1);
|
xue@1
|
3886 {
|
xue@1
|
3887 double tmpph=0;
|
xue@1
|
3888 memset(s2ph, 0, sizeof(double)*(L+1));
|
xue@1
|
3889 s2ph[0]=tmpph;
|
xue@1
|
3890 for (int l=0; l<L; l++)
|
xue@1
|
3891 {
|
xue@1
|
3892 MultiplyXy(M, N, lamda, A[l], aita); for (int m=0; m<M; m++) lamdax[m]=lamda[m].x, lamday[m]=lamda[m].y;
|
xue@1
|
3893 SinusoidExpA(T, &s2[l*T], M, lamdax, lamday, h, dih, tmpph); s2ph[l+1]=tmpph;
|
xue@1
|
3894 }
|
xue@1
|
3895 double* win=new double[2*T+1]; CosineWindows(WinOrder, 2*T, &win, 1); mlist->Add(win, 1);
|
xue@1
|
3896 for (int l=1; l<L; l++)
|
xue@1
|
3897 {
|
xue@1
|
3898 cdouble inn=Inner(2*T, &sre[l*T-T], win, &s2[l*T-T])/Inner(2*T, &s2[l*T-T], win, &s2[l*T-T]);
|
xue@1
|
3899 cdouble loginn=log(inn);
|
xue@1
|
3900 if (SpecifyA==ssACubicHermite)
|
xue@1
|
3901 {
|
xue@1
|
3902 aita[l*2]+=loginn.x;
|
xue@1
|
3903 s2ph[l]+=loginn.y;
|
xue@1
|
3904 phcor[l]=loginn.y;
|
xue@1
|
3905 if (l==1) aita[0]+=loginn.x, phcor[0]=loginn.y, s2ph[0]+=loginn.y;
|
xue@1
|
3906 if (l==L-1) aita[L*2]+=loginn.x, phcor[L]=loginn.y, s2ph[L]+=loginn.y;
|
xue@1
|
3907 }
|
xue@1
|
3908 else
|
xue@1
|
3909 {
|
xue@1
|
3910 aita[l]+=loginn.x;
|
xue@1
|
3911 s2ph[l]+=loginn.y;
|
xue@1
|
3912 phcor[l]=loginn.y;
|
xue@1
|
3913 if (l==1)
|
xue@1
|
3914 {
|
xue@1
|
3915 inn=Inner(T, sre, &win[T], s2)/Inner(T, s2, &win[T], s2);
|
xue@1
|
3916 loginn=log(inn);
|
xue@1
|
3917 aita[0]+=loginn.x;
|
xue@1
|
3918 s2ph[0]+=loginn.y;
|
xue@1
|
3919 phcor[0]=loginn.y;
|
xue@1
|
3920 }
|
xue@1
|
3921 if (l==L-1)
|
xue@1
|
3922 {
|
xue@1
|
3923 inn=Inner(T, &sre[L*T-T], win, &s2[L*T-T])/Inner(T, &s2[L*T-T], win, &s2[L*T-T]);
|
xue@1
|
3924 loginn=log(inn);
|
xue@1
|
3925 aita[L]+=loginn.x;
|
xue@1
|
3926 s2ph[L]+=loginn.y;
|
xue@1
|
3927 phcor[L]=loginn.y;
|
xue@1
|
3928 }
|
xue@1
|
3929 }
|
xue@1
|
3930 }
|
xue@1
|
3931
|
xue@1
|
3932 for (int l=1; l<=L; l++)
|
xue@1
|
3933 {
|
xue@1
|
3934 int k=floor((phcor[l]-phcor[l-1])/(2*M_PI)+0.5);
|
xue@1
|
3935 if (k!=0)
|
xue@1
|
3936 phcor[l]+=2*M_PI*k;
|
xue@1
|
3937 }
|
xue@1
|
3938 //*
|
xue@1
|
3939 //now phcor[] contains phase corrector to be interpolated
|
xue@1
|
3940 double *b=new double[L], *zet=new double[L+1], *dzet=new double[L+1]; memset(zet, 0, sizeof(double)*(L+1)); memset(dzet, 0, sizeof(double)*(L+1));
|
xue@1
|
3941 mlist->Add(b, 1); mlist->Add(zet, 1); mlist->Add(dzet, 1);
|
xue@1
|
3942 double ihT[]={T, T/2.0*T, T/3.0*T*T, T/4.0*T*T*T};
|
xue@1
|
3943
|
xue@1
|
3944 Alloc2L(L, N, BB, mlist);
|
xue@1
|
3945 //prepare linear system (BB)(zet)=(b)
|
xue@1
|
3946 for (int l=0; l<L; l++)
|
xue@1
|
3947 {
|
xue@1
|
3948 MultiplyxY(N, 4, BB[l], ihT, A[l]);
|
xue@1
|
3949 b[l]=phcor[l+1]-phcor[l];
|
xue@1
|
3950 }
|
xue@1
|
3951 Alloc2L(L, L, copyA, mlist);
|
xue@1
|
3952 if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
|
xue@1
|
3953 else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2];
|
xue@1
|
3954 double* copyb=Copy(L, b, mlist);
|
xue@1
|
3955 zet[0]=0; GECP(L, &zet[1], copyA, copyb);
|
xue@1
|
3956 if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
|
xue@1
|
3957 else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2];
|
xue@1
|
3958 Copy(L, copyb, b); for (int l=0; l<L; l++) copyb[l]-=BB[l][0];
|
xue@1
|
3959 dzet[0]=1; GECP(L, &dzet[1], copyA, copyb);
|
xue@1
|
3960
|
xue@1
|
3961 #ifdef ERROR_CHECK
|
xue@1
|
3962 //Test that (BB)(zet)=b and (BB)(dzet)=b
|
xue@1
|
3963 double* bbzet=MultiplyXy(L, L+1, BB, zet, mlist);
|
xue@1
|
3964 MultiAdd(L, bbzet, bbzet, b, -1);
|
xue@1
|
3965 double err1=Inner(L, bbzet, bbzet);
|
xue@1
|
3966 double* bbdzet=MultiplyXy(L, L+1, BB, dzet, mlist);
|
xue@1
|
3967 MultiAdd(L, bbdzet, bbdzet, b, -1);
|
xue@1
|
3968 double err2=Inner(L, bbdzet, bbdzet);
|
xue@1
|
3969 MultiAdd(L+1, dzet, dzet, zet, -1);
|
xue@1
|
3970 //Test that (BB)dzet=0
|
xue@1
|
3971 MultiplyXy(L, L+1, bbdzet, BB, dzet);
|
xue@1
|
3972 double err3=Inner(L, bbzet, bbzet);
|
xue@1
|
3973 #endif
|
xue@1
|
3974 //now that (zet)+(miu)(dzet) is the general solution to (BB)(zet)=b,
|
xue@1
|
3975 // we look for (miu) that maximizes smoothness
|
xue@1
|
3976
|
xue@1
|
3977 double innuv=0, innvv=0, lmd0[4], lmdd[4], clmdd[4],
|
xue@1
|
3978 T2=T*T, T3=T2*T, T4=T3*T, T5=T4*T;
|
xue@1
|
3979 for (int l=0; l<L; l++)
|
xue@1
|
3980 {
|
xue@1
|
3981 MultiplyXy(4, L+1, lmd0, A[l], zet);
|
xue@1
|
3982 MultiplyXy(4, L+1, lmdd, A[l], dzet);
|
xue@1
|
3983 clmdd[1]=T*lmdd[1]+T2*lmdd[2]+T3*lmdd[3];
|
xue@1
|
3984 clmdd[2]=T2*lmdd[1]+(4.0/3)*T3*lmdd[2]+1.5*T4*lmdd[3];
|
xue@1
|
3985 clmdd[3]=T3*lmdd[1]+1.5*T4*lmdd[2]+1.8*T5*lmdd[3];
|
xue@1
|
3986 innuv+=Inner(3, &lmd0[1], &clmdd[1]);
|
xue@1
|
3987 innvv+=Inner(3, &lmdd[1], &clmdd[1]);
|
xue@1
|
3988 }
|
xue@1
|
3989 MultiAdd(L+1, zet, zet, dzet, -innuv/innvv);
|
xue@1
|
3990
|
xue@1
|
3991 if (SpecifyA==ssACubicHermite)
|
xue@1
|
3992 for (int l=0; l<=L; l++) aita[2*l].y+=zet[l];
|
xue@1
|
3993 else
|
xue@1
|
3994 for (int l=0; l<=L; l++) aita[l].y+=zet[l];
|
xue@1
|
3995 //*/
|
xue@1
|
3996 }
|
xue@1
|
3997 double result=s2ph[0];
|
xue@1
|
3998 delete mlist;
|
xue@1
|
3999 return result;
|
xue@1
|
4000 }//AmpPhCorrectionExpA
|
Chris@5
|
4001
|