annotate sinest.cpp @ 13:de3961f74f30 tip

Add Linux/gcc Makefile; build fix
author Chris Cannam
date Mon, 05 Sep 2011 15:22:35 +0100
parents 977f541d6683
children
rev   line source
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 *)&params;
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 *)&params;
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