Mercurial > hg > x
diff SinEst.cpp @ 1:6422640a802f
first upload
| author | Wen X <xue.wen@elec.qmul.ac.uk> |
|---|---|
| date | Tue, 05 Oct 2010 10:45:57 +0100 |
| parents | |
| children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SinEst.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,3970 @@ +//--------------------------------------------------------------------------- + +#include <stddef.h> +#include "SinEst.h" +#include "fft.h" +#include "opt.h" +#include "SinSyn.h" +#include "splines.h" +#include "WindowFunctions.h" + +//--------------------------------------------------------------------------- +/* + function dsincd_unn: derivative of unnormalized discrete sinc function + + In: x, scale N + + Returns the derivative of sincd_unn(x, N) +*/ +double dsincd_unn(double x, int N) +{ + double r=0; + double omg=M_PI*x; + double domg=omg/N; + if (fabs(x)>1e-6) + { + r=M_PI*(cos(omg)-sin(omg)*cos(domg)/sin(domg)/N)/sin(domg); + } + else + { + if (domg!=0) + { + double sindomg=sin(domg); + r=-omg*omg*omg*(1-1.0/(1.0*N*N))/3*M_PI/N/sindomg/sindomg; + } + else + r=0; + } + return r; +}//dsincd_unn + +/* + function ddsincd_unn: 2nd-order derivative of unnormalized discrete sinc function + + In: x, scale (equivalently, window size) N + + Returns the 2nd-order derivative of sincd_unn(x, N) +*/ +double ddsincd_unn(double x, int N) +{ + double r=0; + double omg=M_PI*x; + double domg=omg/N; + double PI2=M_PI*M_PI; + double NN=1.0/N/N-1; + if (domg==0) + { + r=PI2*N*NN/3; + } + else + { + if (fabs(x)>1e-5) + { + r=sin(domg)*cos(omg)-sin(omg)*cos(domg)/N; + } + else + { + r=omg*omg*omg/N*NN/3; + } + double ss=sin(omg)*NN; + r=-2.0/N*cos(domg)*r/sin(domg)/sin(domg)+ss; + r=r*PI2/sin(domg); + } + return r; +}//ddsincd_unn + +//--------------------------------------------------------------------------- +/* + function Window: calculates the cosine-family-windowed spectrum of a complex sinusoid on [0:N-1] at + frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2. + + Returns pointer to x. x is created anew if x=0 is specified on start. +*/ +cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + + if (!x) x=new cdouble[K2-K1+1]; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l)*M_PI; + double omg=ang/N; + long double si, co, sinn=sin(ang); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si); + double saco=sa*co; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + } + } + } + return x; +}//Window + +/* + function dWindow: calculates the cosine-family-windowed spectrum and its derivative of a complex + sinusoid on [0:N-1] at frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2, + dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2 + + No return value. +*/ +void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + memset(dx, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l), Omg=ang*M_PI, omg=Omg/N; + long double si, co, sinn=sin(Omg), cosn=cos(Omg); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N); + double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, cosnpi=cosn*M_PI; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + dx[kt].x-=c[m]*(-sinnpi_n+dsaco); + dx[kt].y+=c[m]*cosnpi; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + dx[kt].x+=c[m]*(-sinnpi_n+dsaco); + dx[kt].y-=c[m]*cosnpi; + } + } + } +}//dWindow + +/* + function ddWindow: calculates the cosine-family-windowed spectrum and its 1st and 2nd derivatives of + a complex sinusoid on [0:N-1] at frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2, + dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2 + ddx[0...K2-K1] containing the 2nd-order derivative spectrum at bins K1, ..., K2 + + No return value. +*/ +void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + memset(dx, 0, sizeof(cdouble)*(K2-K1+1)); + memset(ddx, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l), Omg=ang*M_PI, omg=Omg/N; + long double si, co, sinn=sin(Omg), cosn=cos(Omg); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N); + double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, sinnpipi=sinn*M_PI*M_PI, cosnpi=cosn*M_PI, + cosnpipi_n=cosnpi*M_PI/N, sipi_n=si*M_PI/N; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + dx[kt].x-=c[m]*(-sinnpi_n+dsaco); + dx[kt].y+=c[m]*cosnpi; + ddx[kt].x-=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n); + ddx[kt].y-=c[m]*sinnpipi; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + dx[kt].x+=c[m]*(-sinnpi_n+dsaco); + dx[kt].y-=c[m]*cosnpi; + ddx[kt].x+=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n); + ddx[kt].y+=c[m]*sinnpipi; + } + } + } +}//ddWindow + +//--------------------------------------------------------------------------- +/* + function IPWindow: computes the truncated inner product of a windowed spectrum with that of a sinusoid + at reference frequency f. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + returnamplitude: specifies return value, true for amplitude, false for angle + + Returns the amplitude or phase of the inner product, as specified by $returnamplitude. The return + value is interpreted as the actual amplitude/phase of a sinusoid being estimated at f. +*/ +double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude) +{ + cdouble r=IPWindowC(f, x, N, M, c, iH2, K1, K2); + double result; + if (returnamplitude) result=sqrt(r.x*r.x+r.y*r.y); + else result=arg(r); + return result; +}//IPWindow +//wrapper function +double IPWindow(double f, void* params) +{ + 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; + return IPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, true); +}//IPWindow + +/* + function ddIPWindow: computes the norm of the truncated inner product of a windowed spectrum with + that of a sinusoid at reference frequency f, as well as its 1st and 2nd derivatives. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: ipwindow and dipwindow: the truncated inner product norm and its derivative + + Returns the 2nd derivative of the norm of the truncated inner product. +*/ +double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2], *lx=&x[K1]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + delete[] w; + + double R2=~r, + R=sqrt(R2), + dR2=2*(r.x*dr.x+r.y*dr.y), + dR=dR2/(2*R), + ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr), + ddR=(R*ddR2-dR2*dR)/(2*R2); + ipwindow=R*iH2; + dipwindow=dR*iH2; + return ddR*iH2; +}//ddIPWindow +//wrapper function +double ddIPWindow(double f, void* params) +{ + 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; + return ddIPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow); +}//ddIPWindow + +//--------------------------------------------------------------------------- +/* + function IPWindowC: computes the truncated inner product of a windowed spectrum with that of a + sinusoid at reference frequency f. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + + Returns the inner product. The return value is interpreted as the actual amplitude-phase factor of a + sinusoid being estimated at f. +*/ +cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + cdouble *lx=&x[K1], result=0; + Window(w, f, N, M, c, K1, K2); + for (int k=0; k<K; k++) result+=lx[k]^w[k]; + delete[] w; + result*=iH2; + return result; +}//IPWindowC + +//--------------------------------------------------------------------------- +/* + function sIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f. This does not consider phase alignment + between the spectra, supposedly measured at a sequence of known instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional) + + Returns the energy of the vector of inner products. +*/ +double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd) +{ + double sip=0; + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + Window(w, f, N, M, c, K1, K2); + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w); + if (lmd) lmd[l]=r*iH2; + sip+=~r; + } + sip*=iH2; + delete[] w; + return sip; +}//sIPWindow +//wrapper function +double sIPWindow(double f, void* params) +{ + 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; + return sIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->lmd); +}//sIPWindow + +/* + function dsIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f, as well as its derivative. This does not + consider phase synchronization between the spectra, supposedly measured at a sequence of known + instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip, the energy of the vector of inner products. + + Returns the derivative of the energy of the vector of inner products. +*/ +double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*2], *dw=&w[K]; + dWindow(dw, w, f, N, M, c, K1, K2); + double dsip; sip=0; + for (int l=0; l<L; l++) + { + cdouble* lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw); + double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y); + sip+=R2, dsip+=dR2; + } + sip*=iH2, dsip*=iH2; + delete[] w; + return dsip; +}//dsIPWindow +//wrapper function +double dsIPWindow(double f, void* params) +{ + 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; + return dsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip); +}//dsIPWindow + +/* + function dsdIPWindow_unn: computes the energy of unnormalized truncated inner products between a given + windowed spectrum and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd + derivatives. "Unnormalized" indicates that the inner product cannot be taken as the actual amplitude- + phase factor of a sinusoid, but deviate from that by an unspecified factor. + + In: x[N]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sipwindow and dsipwindow, the energy and its derivative of the unnormalized inner product. + + Returns the 2nd derivative of the inner product. +*/ +double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + + double rr=0, ri=0, drr=0, dri=0, ddrr=0, ddri=0; + cdouble *lx=&x[K1]; + for (int k=0; k<K; k++) + { + rr+=lx[k].x*w[k].x+lx[k].y*w[k].y; + ri+=lx[k].y*w[k].x-lx[k].x*w[k].y; + drr+=lx[k].x*dw[k].x+lx[k].y*dw[k].y; + dri+=lx[k].y*dw[k].x-lx[k].x*dw[k].y; + ddrr+=lx[k].x*ddw[k].x+lx[k].y*ddw[k].y; + ddri+=lx[k].y*ddw[k].x-lx[k].x*ddw[k].y; + } + delete[] w; + + double R2=rr*rr+ri*ri, + dR2=2*(rr*drr+ri*dri), + ddR2=2*(rr*ddrr+ri*ddri+drr*drr+dri*dri); + sipwindow=R2; + dsipwindow=dR2; + if (w_unn) w_unn->x=rr, w_unn->y=ri; + return ddR2; +}//ddsIPWindow_unn + +/* + function ddsIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd derivatives. + This does not consider phase synchronization between the spectra, supposedly measured at a sequence + of known instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip and dsip, the energy of the vector of inner products and its derivative. + + Returns the 2nd derivative of the energy of the vector of inner products. +*/ +double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + double ddsip=0; dsip=sip=0; + for (int l=0; l<L; l++) + { + cdouble* lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y), ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr); + sip+=R2, dsip+=dR2, ddsip+=ddR2; + } + sip*=iH2, dsip*=iH2, ddsip*=iH2; + delete[] w; + return ddsip; +}//ddsIPWindow +//wrapper function +double ddsIPWindow(double f, void* params) +{ + 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; + return ddsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dsip, p->sip); +}//ddsIPWindow + +//--------------------------------------------------------------------------- +/* + function sIPWindowC: computes the total energy of truncated inner products between multiple frames of + a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional) + + Returns the energy of the vector of inner products. +*/ +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) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + double Cr=0; + cdouble Cc=0; + Window(w, f, N, M, c, K1, K2); + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w); + Cr+=~r; + double ph=-4*M_PI*f*offst_rel*l; + cdouble r2=r*r; + Cc+=r2.rotate(ph); + if (lmd) lmd[l]=r; + } + delete[] w; + double result=0.5*iH2*(Cr+abs(Cc)); + if (lmd) + { + double absCc=abs(Cc), hiH2=0.5*iH2; + cdouble ej2ph=Cc/absCc; + for (int l=0; l<L; l++) + { + double ph=4*M_PI*f*offst_rel*l; + lmd[l]=hiH2*(lmd[l]+(ej2ph**lmd[l]).rotate(ph)); + } + } + return result; +}//sIPWindowC +//wrapper function +double sIPWindowC(double f, void* params) +{ + 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; + return sIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2); +}//sIPWindowC + +/* + function dsIPWindowC: computes the total energy of truncated inner products between multiple frames of + a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, together + with its derivative. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip: energy of the vector of the inner products + + Returns the 1st derivative of the energy of the vector of inner products. +*/ +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) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*2], *dw=&w[K]; + dWindow(dw, w, f, N, M, c, K1, K2); + double Cr=0, dCr=0; + cdouble Cc=0, dCc=0; + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw); + Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); + int two=2; + cdouble r2=r*r, dr2=r*dr*two; + double lag=-4*M_PI*offst_rel*l, ph=lag*f; + Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph); + } + double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y); + double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1); + sip=0.5*iH2*(Cr+Cc1); + double dsip=0.5*iH2*(dCr+dCc1); + delete[] w; + return dsip; +}//dsIPWindowC +//wrapper function +double dsIPWindowC(double f, void* params) +{ + 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; + return dsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip); +}//dsIPWindowC + +/* + function ddsIPWindowC: computes the total energy of truncated inner products between multiple frames + of a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, + together with its 1st and 2nd derivatives. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sipwindow, dsipwindow: energy of the vector of the inner products and its derivative + + Returns the 2nd derivative of the energy of the vector of inner products. +*/ +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) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + double Cr=0, dCr=0, ddCr=0; + cdouble Cc=0, dCc=0, ddCc=0; + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); ddCr+=2*(r.x*ddr.x+r.y*ddr.y+~dr); + int two=2; + cdouble r2=r*r, dr2=r*dr*two, ddr2=(dr*dr+r*ddr)*two; + double lag=-4*M_PI*offst_rel*l, ph=lag*f; + 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); + } + double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y), ddCc2=2*(Cc.x*ddCc.x+Cc.y*ddCc.y+~dCc); + double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1), ddCc1=(Cc1*ddCc2-dCc2*dCc1)/(2*Cc2); + sipwindow=0.5*iH2*(Cr+Cc1); + dsipwindow=0.5*iH2*(dCr+dCc1); + double ddsipwindow=0.5*iH2*(ddCr+ddCc1); + delete[] w; + return ddsipwindow; +}//ddsIPWindowC +//wrapper function +double ddsIPWindowC(double f, void* params) +{ + 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; + 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); +}//ddsIPWindowC + +//-------------------------------------------------------------------------- +/* + Least-square-error sinusoid detection function + + version1: picking the highest peak and take measurement of a single sinusoid + version2: given a rough peak location and take measurement of a single sinusoid + + Complex spectrum x is calculated using N data points windowed by a window function that is specified + by the parameter set (M, c, iH2). c[0:M] is provided according to Table 3 in the transfer report, on + pp.11. iH2 is simply 1/H2, where H2 can be calculated using formula (2.17) on pp.12. + + f & epf are given/returned in bins. + + Further reading: "Least-square-error estimation of sinusoids.pdf" +*/ + +/* + function LSESinusoid: LSE estimation of the predominant stationary sinusoid. + + In: x[N]: windowed spectrum + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: a and pp: amplitude and phase estimates + + Returns the frequency estimate, in bins. +*/ +double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; //(l_hx *)¶ms; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int inp; + double minp=0; + for (int i=0; i<N; i++) + { + double lf=i, tmp; + p.k1=ceil(lf-B); if (p.k1<0) p.k1=0; + p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + tmp=IPWindow(lf, &p); + if (minp<tmp) inp=i, minp=tmp; + } + + double f=inp; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + return f; +}//LSESinusoid + +/*function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency. + + In: x[N]: windowed spectrum + f: initial frequency, in bins + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a and pp: frequency, amplitude and phase estimates + + No return value. +*/ +void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + 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}; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); +}//LSESinusoid + +/* + function LSESinusoid: LSE estimation of stationary sinusoid predominant within [f1, f2]. + + In: x[N]: windowed spectrum + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: a and pp: amplitude and phase estimates + + Returns the frequency estimate, in bins. +*/ +double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + 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}; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int inp; + double minp=0; + for (int i=f1; i<f2; i++) + { + double lf=i, tmp; + p.k1=ceil(lf-B); if (p.k1<0) p.k1=0; + p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + tmp=IPWindow(lf, &p); + if (minp<tmp) inp=i, minp=tmp; + } + + double f=inp; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + return f; +}//LSESinusoid + +/* + function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency within [f1, + f2]. + + In: x[N]: windowed spectrum + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a and pp: frequency, amplitude and phase estimates + + Returns 1 if managed to find a sinusoid, 0 if not, upon which $a and $pp are estimated at the initial + f. +*/ +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) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};//(l_hx *)¶ms; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int result=0; + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf, 100, 1e-256, f1, f2); + if (tmp!=-1 && f>f1 && f<f2) + { + result=1; + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + } + else + { + Search1DmaxEx(f, &p, IPWindow, f1, f2, &a, epf); + if (f<=f1 || f>=f2) + { + f=inp; + cdouble r=IPWindowC(f, x, N, M, c, iH2, p.k1, p.k2); + a=abs(r); + pp=arg(r); + } + else + { + result=1; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + } + } + return result; +}//LSESinusoid + +/* + function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without + considering phase-frequency consistency across frames. + + In: x[Fr][N]: spectrogram + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates + + Returns an error bound of the frequency estimate. +*/ +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) +{ + 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}; + int dfshift=int(&((l_ip1*)0)->dsip), fshift=int(&((l_ip1*)0)->sip); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double errf=Newton1dmax(f, f1, f2, ddsIPWindow, &p, dfshift, fshift, dsIPWindow, dfshift, epf); + if (errf<0) errf=Search1Dmax(f, &p, sIPWindow, f1, f2, a, epf); + if (a || ph) + { + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2); + if (a) a[fr]=abs(r); + if (ph) ph[fr]=arg(r); + } + } + return errf; +}//LSESinusoidMP + +/* + function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without + considering phase-frequency consistency across frames. + + In: x[Fr][N]: spectrogram + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates + + Returns an error bound of the frequency estimate. Although the frequencies are estimated assuming + cross-frame frequency-phase consistency, the final output phase angles are reestimated independently + for each frame using the frequency estimate. +*/ +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) +{ + 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;} + p={N, 0, 0, M, c,iH2, Fr, Offst*1.0/N, x, 0, 0}; + int dfshift=int(&((l_ip*)0)->sdip), fshift=int(&((l_ip*)0)->sip); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double errf=Newton1dmax(f, f1, f2, ddsIPWindowC, &p, dfshift, fshift, dsIPWindowC, dfshift, epf); + if (errf<0) errf=Search1Dmax(f, &p, sIPWindowC, f1, f2, a, epf); + if (a || ph) + { + cdouble* lmd=new cdouble[Fr]; + sIPWindowC(f, Fr, Offst*1.0/N, x, N, M, c, iH2, p.k1, p.k2, lmd); + for (int fr=0; fr<Fr; fr++) + { + lmd[fr]=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2); + + if (a) a[fr]=abs(lmd[fr]); + if (ph) ph[fr]=arg(lmd[fr]); + } + delete[] lmd; + } + return errf; +}//LSESinusoidMPC + +//--------------------------------------------------------------------------- +/* + function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy + suppression index of eps, i.e. the least square error is minimized with an additional eps*||lmd||^2 + term. + + In: x[Wid]: spectrum + f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: lmd[I]: amplitude-phase factors + + No return value. +*/ +void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + cdouble** whw=MultiplyXcXt(I, K, wt, List); + cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + GECP(I, lmd, whw, whx); + delete List; +}//IPMulti + +/* + function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy + suppression index of eps, and optionally returns residue and sensitivity indicators for each sinusoid. + + In: x[Wid]: spectrum + f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: lmd[I]: amplitude-phase factors + sens[I]: sensitivity indicators + r1[I]: residue indicators, measured by correlating residue with sinusoid spectra, optional + + No return value. Sensibitily is computed BEFORE applying eps. +*/ +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) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + cdouble** whw=MultiplyXcXt(I, K, wt, List); + + //*computes sensitivity if required + if (sens) + { + cdouble** iwhw=Copy(I, whw, List); + GICP(I, iwhw); + cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List); + for (int i=0; i<I; i++) + { + sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]); + } + } //*/ + cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + GECP(I, lmd, whw, whx); + //compute residue if required + if (r1) + { + cdouble* wlmd=MultiplyXty(K, I, wt, lmd, List); //reconstruct + for (int k=0; k<K; k++) wlmd[k]=wlmd[k]-x[K1+k]; //-residue + for (int i=0; i<I; i++) //r1[i]=Inner(K, wlmd, wt[i]).abs(); //-residue weighted by window + { + r1[i]=0; + for (int k=0; k<K; k++) r1[i]+=abs(wlmd[k])*abs(wt[i][k]); + } + } + delete List; +}//IPMulti + +/* + function IPMultiSens: computes the sensitivity of the least square estimation of multiple sinusoids given + their frequencies . + + In: f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: sens[I]: sensitivity indicators + + No return value. Sensibility is computed AFTER applying eps +*/ +void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + + cdouble** whw=MultiplyXcXt(I, K, wt, List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + + cdouble** iwhw=Copy(I, whw, List); + GICP(I, iwhw); + cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List); + for (int i=0; i<I; i++) + { + sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]); + } + delete List; +}//IPMultiSens + +/* + function IPMulti: least square estimation of multi-sinusoids with GIVEN frequencies. This version + operates in groups at least B bins from each other, rather than LSE all frequencies together. + + In: x[Wid]: spectrum + f[I]: frequencies, must be ordered low to high. + B: number of bins beyond which sinusoids are treated as non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: lmd[I]: amplitude-phase factors + + Returns 0. +*/ +double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B) +{ + int i=0, ist=0; + double Bw=B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; int K1=floor(fb-B+0.5), K2=floor(fb+B+0.5); + lmd[ist]=IPWindowC(fb, x, Wid, M, c, iH2, K1, K2); + } + else + { + MList* List=new MList; + int N=i-ist, K1=floor(f[ist]-B+0.5), K2=floor(f[i-1]+B+0.5), K=K2-K1+1; + cdouble** Allocate2L(cdouble, N, K, wt, List); + for (int n=0; n<N; n++) Window(wt[n], f[ist+n], Wid, M, c, K1, K2); + cdouble* whx=MultiplyXcy(N, K, wt, &x[K1], List); //w*'x=(wt*)x + cdouble** whw=MultiplyXcXt(N, K, wt, List); + /*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); + 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);*/ + GECP(N, &lmd[ist], whw, whx); //solving complex linear system (w*'w)a=w*'x + delete List; + } + ist=i; + } + i++; + } + return 0; +}//IPMulti + +/* + function IPMulti_Direct: LSE estimation of multiple sinusoids given frequencies AND PHASES (direct + method) + + In: x[Wid]: spectrum + f[I], ph[I]: frequencies and phase angles. + B: spectral truncation half width, in bins; sinusoids over 3B bins apart are regarded non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: a[I]: amplitudes + + Returns square norm of the residue. +*/ +double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B) +{ + MList* List=new MList; + int i=0, ist=0, hWid=Wid/2; + cdouble* r=Copy(hWid, x, List); //to store the residue + + double Bw=3.0*B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; + cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]); + double ip=Inner(2*hWid, (double*)x, (double*)w); + a[ist]=ip*iH2; + MultiAdd(hWid, r, r, w, -a[ist]); + delete[] w; + } + else + { + int N=i-ist; + cdouble** Allocate2L(cdouble, N, hWid, wt, List); + for (int n=0; n<N; n++) + { + Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]); + } + double* whxr=MultiplyXy(N, hWid*2, (double**)wt, (double*)x, List); //w*'x=(wt*)x + double** whwr=MultiplyXXt(N, hWid*2, (double**)wt, List); + GECP(N, &a[ist], whwr, whxr); //solving complex linear system (w*'w)a=w*'x + for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]); + } + ist=i; + } + i++; + } + double result=Inner(hWid, r, r).x; + delete List; + return result; +}//IPMulti_Direct + +/* + function IPMulti_GS: LSE estimation of multiple sinusoids given frequencies AND PHASES (Gram-Schmidt method) + + In: x[Wid]: spectrum + f[I], ph[I]: frequencies and phase angles. + B: spectral truncation, in bins; sinusoids over 3B bins apart are regarded non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: a[I]: amplitudes + + Returns square norm of the residue. +*/ +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) +{ + MList* List=new MList; + int i=0, ist=0, hWid=Wid/2; + cdouble* r=Copy(hWid, x, List); //to store the residue + double Bw=3.0*B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; + cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]); + double ip=Inner(2*hWid, (double*)x, (double*)w); + a[ist]=ip*iH2; + MultiAdd(hWid, r, r, w, -a[ist]); + delete[] w; + } + else + { + int N=i-ist; + cdouble** Allocate2L(cdouble, N, hWid, wt, List); + Alloc2L(N, N, L, List); Alloc2L(N, hWid*2, Q, List); + for (int n=0; n<N; n++) + { + Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]); + } + LQ_GS(N, hWid*2, (double**)wt, L, Q); + double* atl=MultiplyxYt(N, hWid*2, (double*)x, Q, List); + GExL(N, &a[ist], L, atl); + for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]); + } + ist=i; + } + i++; + } + double result=Inner(hWid, r, r).x; + delete List; + return result; +}//IPMulti_GS + +/* + function IPMulti: LSE estimation of I sinusoids given frequency and phase and J sinusoids given + frequency only + + In: x[Wid]: spectrum + f[I+J], ph[I]: frequencies and phase angles + M, c[], iH2: cosine-family window specification parameters + Out: a[I+J]: amplitudes + ph[I:I+J-1]: phase angles not given on start + wt[I+2J][hWid], Q[I+2J][hWid], L[I+2J][I+2J]: internal w matrix and its LQ factorization, optional + + Returns the residue vector, newly created and registered to RetList, if specified. On start a[] should + have valid storage no less than I+2J. +*/ +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) +{ + MList* List=new MList; + int hWid=Wid/2; + cdouble* r=Copy(hWid, x, RetList); //to store the residue + if (!wt){Allocate2L(cdouble, I+J*2, hWid, wt, List);} + if (!Q){Allocate2L(cdouble, I+J*2, hWid, Q, List);} + if (!L){Allocate2L(double, I+J*2, I+J*2, L, List);} + memset(wt[0], 0, sizeof(cdouble)*(I+J*2)*hWid); + memset(Q[0], 0, sizeof(cdouble)*(I+J*2)*hWid); + memset(L[0], 0, sizeof(double)*(I+J*2)*(I+J*2)); + + //*The direct form + for (int i=0; i<I; i++) + { + Window(wt[i], f[i], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[i][k].rotate(ph[i]); + } + for (int j=0; j<J; j++) + { + cdouble *w1=wt[I+j*2], *w2=wt[I+j*2+1]; + Window(w1, f[I+j], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w2[k].y=w1[k].x, w2[k].x=-w1[k].y; + } + + LQ_GS(I+J*2, hWid*2, (double**)wt, L, (double**)Q); + double *atl=MultiplyxYt(I+J*2, hWid*2, (double*)x, (double**)Q, List); + GExL(I+J*2, a, L, atl); + + for (int i=0; i<I+J*2; i++) MultiAdd(hWid, r, r, wt[i], -a[i]); + for (int j=0; j<J; j++) + { + double xx=a[I+j*2], yy=a[I+j*2+1]; + a[I+j]=sqrt(xx*xx+yy*yy); + ph[I+j]=atan2(yy, xx); + } + delete List; + return r; +}//IPMulti + +//--------------------------------------------------------------------------- +/* + Routines for estimation two sinusoids with 1 fixed and 1 flexible frequency + + Further reading: "LSE estimation for 2 sinusoids with 1 at a fixed frequency.pdf" +*/ + +/* + function WindowDuo: calcualtes the square norm of the inner product between windowed spectra of two + sinusoids at frequencies f1 and f2, df=f1-f2. + + In: df: frequency difference, in bins + N: DFT size + M, d[]: cosine-family window specification parameters (see "further reading"). + Out: w[0], the inner product, optional + + Returns square norm of the inner product. +*/ +double WindowDuo(double df, int N, double* d, int M, cdouble* w) +{ + double wr=0, wi=0; + for (int m=-2*M; m<=2*M; m++) + { + double ang=df+m, Omg=ang*M_PI, omg=Omg/N; + double si=sin(omg), co=cos(omg), sinn=sin(Omg); + double sa=(ang==0)?N:(sinn/si); + double dm; if (m<0) dm=d[-m]; else dm=d[m]; + wr+=dm*sa*co, wi+=-dm*sinn; + } + wr*=N, wi*=N; + if (w) w->x=wr, w->y=wi; + double result=wr*wr+wi*wi; + return result; +}//WindowDuo + +/* + function ddWindowDuo: calcualtes the square norm of the inner product between windowed spectra of two + sinusoids at frequencies f1 and f2, df=f1-f2, with its 1st and 2nd derivatives + + In: df: frequency difference, in bins + N: DFT size + M, d[]: cosine-family window specification parameters (see "further reading" for d[]). + Out: w[0], the inner product, optional + window, dwindow: square norm and its derivative, of the inner product + + Returns 2nd derivative of the square norm of the inner product. +*/ +double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w) +{ + 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; + for (int m=-2*M; m<=2*M; m++) + { + double ang=df+m, Omg=ang*M_PI, omg=Omg/N; + double si=sin(omg), co=cos(omg), sinn=sin(Omg), cosn=cos(Omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N); + double dm; if (m<0) dm=d[-m]; else dm=d[m]; + wr+=dm*sa*co, wi+=-dm*sinn; + dwr+=dm*(dsa*co-PI_N*sinn), dwi+=-dm*M_PI*cosn; + ddwr+=dm*(ddsa*co-PI_N*dsa*si-PIPI_N*cosn), ddwi+=dm*PIPI*sinn; + } + wr*=N, wi*=N, dwr*=N, dwi*=N, ddwr*=N, ddwi*=N; + window=wr*wr+wi*wi; + dwindow=2*(wr*dwr+wi*dwi); + if (w) w->x=wr, w->y=wi; + double ddwindow=2*(wr*ddwr+dwr*dwr+wi*ddwi+dwi*dwi); + return ddwindow; +}//ddWindowDuo + +/* + function sIPWindowDuo: calculates the square norm of the orthogonal projection of a windowed spectrum + onto the linear span of the windowed spectra of two sinusoids at reference frequencies f1 and f2. + + In: x[N]: spectrum + f1, f2: reference frequencies. + M, c[], d[], iH2: cosine-family window specification parameters. + K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored. + Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors + + Returns the square norm of the orthogonal projection. +*/ +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) +{ + int K=K2-K1+1; + cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K]; + Window(w1, f1, N, M, c, K1, K2); + double w1w1=0; + for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1; + for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k]; + Window(w1, f2, N, M, c, K1, K2); + cdouble r1w2=0, w12; for (int k=0; k<K; k++) r1w2+=(r1[k]^w1[k]); + double w=WindowDuo(f1-f2, N, d, M, &w12); + double v=1.0/iH2-w*iH2; + double result=~xw1/w1w1+~r1w2/v; + cdouble mu2=r1w2/v; + lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2; + delete[] w1; + return result; +}//sIPWindowDuo +//wrapper function +double sIPWindowDuo(double f2, void* params) +{ + 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; + cdouble r1, r2; + return sIPWindowDuo(p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2); +}//sIPWindowDuo + +/* + function ddsIPWindowDuo: calculates the square norm, and its 1st and 2nd derivatives against f2,, of + the orthogonal projection of a windowed spectrum onto the linear span of the windowed spectra of two + sinusoids at reference frequencies f1 and f2. + + In: x[N]: spectrum + f1, f2: reference frequencies. + M, c[], d[], iH2: cosine-family window specification parameters. + K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored. + + Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors + ddsip[3]: the 2nd, 1st and 0th derivatives (against f2) of the square norm. + + No return value. +*/ +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) +{ + int K=K2-K1+1; + cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K]; + Window(w1, f1, N, M, c, K1, K2); + double w1w1=0; + for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1; + for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k]; + + cdouble r1w2, w12; + double u, du, ddu=ddsIPWindow_unn(f2, &r1[-K1], N, M, c, K1, K2, du, u, &r1w2); + double w, dw, ddw=ddWindowDuo(f1-f2, N, d, M, dw, w, &w12); dw=-dw; + double v=1.0/iH2-w*iH2, dv=-iH2*dw, ddv=-iH2*ddw; + double iv=1.0/v;//, div=-dv*iv*iv, ddiv=(2*dv*dv-v*ddv)*iv*iv*iv; + + ddsip2[2]=~xw1/w1w1+u*iv; + ddsip2[1]=iv*(du-iv*u*dv); + ddsip2[0]=iv*(ddu-iv*(u*ddv+2*du*dv-2*iv*u*dv*dv)); + + cdouble mu2=r1w2*iv; + lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2; + + delete[] w1; +}//ddsIPWindowDuo +//wrapper function +double ddsIPWindowDuo(double f2, void* params) +{ + 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; + double ddsip2[3]; cdouble r1, r2; + ddsIPWindowDuo(ddsip2, p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2); + p->dipwindow=ddsip2[1], p->ipwindow=ddsip2[2]; + return ddsip2[0]; +}//ddsIPWindowDuo + +/* + function LSEDuo: least-square estimation of two sinusoids of which one has a fixed frequency + + In: x[N]: the windowed spectrum + f1: the fixed frequency + f2: initial value of the flexible frequency + fmin, fmax: search range for f2, the flexible frequency + B: spectral truncation half width + M, c[], d[], iH2: + epf: frequency error tolerance + Out: f2: frequency estimate + lmd1, lmd2: amplitude-phase factor estimates + Returns 1 if managed to find a good f2, 0 if not, upon which the initial f2 is used for estimating + + amplitudes and phase angles. +*/ +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) +{ + int result=0; + double inp=f2; + int k1=ceil(inp-B); if (k1<0) k1=0; + int k2=floor(inp+B); if (k2>=N/2) k2=N/2-1; + 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}; + int dfshift=int(&((l_hx*)0)->dipwindow);// fshift=int(&((l_hx*)0)->ipwindow); + + double tmp=Newton(f2, ddsIPWindowDuo, &p, dfshift, epf, 100, 1e-256, fmin, fmax); + if (tmp!=-1 && f2>fmin && f2<fmax) result=1; + else + { + Search1DmaxEx(f2, &p, sIPWindowDuo, fmin, fmax, NULL, epf); + if (f2<=fmin || f2>=fmax) f2=inp; + else result=1; + } + sIPWindowDuo(f1, f2, x, N, c, d, M, iH2, k1, k2, r1, r2); + return result; +}//LSEDuo + +//--------------------------------------------------------------------------- +/* + Time-frequency reassignment sinusoid estimation routines. + + Further reading: A. R?bel, ¡°Estimating partial frequency and frequency slope using reassignment + operators,¡± in Proc. ICMC¡¯02. G?teborg. 2002. +*/ + +/* + function CDFTW: single-frequency windowed DTFT, centre-aligned + + In: data[Wid]: waveform data x + win[Wid+1]: window function + k: frequency, in bins, where bin=1/Wid + Out: X: DTFT of xw at frequency k bins + + No return value. +*/ +void CDFTW(cdouble& X, double k, int Wid, cdouble* data, double* win) +{ + X=0; + int hWid=Wid/2; + for (int i=0; i<Wid; i++) + { + cdouble tmp=data[i]*win[Wid-i]; + double ph=-2*M_PI*(i-hWid)*k/Wid; + tmp.rotate(ph); + X+=tmp; + } +}//CDFTW + +/* + function CuDFTW: single-frequency windowed DTFT of t*data[t], centre-aligned + + In: data[Wid]: waveform data x + wid[Wid+1]: window function + k: frequency, in bins + Out: X: DTFT of txw at frequency k bins + + No return value. +*/ +void CuDFTW(cdouble& X, int k, int Wid, cdouble* data, double* win) +{ + X=0; + int hWid=Wid/2; + for (int i=0; i<Wid; i++) + { + double tw=((i-hWid)*win[Wid-i]); + cdouble tmp=data[i]*tw; + double ph=-2*M_PI*(i-hWid)*k/Wid; + tmp.rotate(ph); + X+=tmp; + } +}//CuDFTW + +/* + function TFReas: time-frequency reassignment + + In: data[Wid]: waveform data + win[Wid+1], dwin[Wid+1], ddwin[Wid+1]: window function and its derivatives + f, t: initial digital frequency and time + Out: f, t: reassigned digital frequency and time + fslope: estimate of frequency derivative + plogaslope[0]: estimate of the derivative of logarithmic amplitude, optional + + No return value. +*/ +void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope) +{ + int fi=floor(f*Wid+0.5); + + cdouble x, xt, xw; + CDFTW(x, fi, Wid, data, win); + CuDFTW(xw, fi, Wid, data, win); xt.x=xw.y; xw.y=-xw.x; xw.x=xt.x; + CDFTW(xt, fi, Wid, data, dwin); + double px=~x; + t=t-(xw.y*x.x-xw.x*x.y)/px; + f=1.0*fi/Wid+(xt.y*x.x-xt.x*x.y)/px/(2*M_PI); + if (plogaslope) plogaslope[0]=-(xt.x*x.x+xt.y*x.y)/px; + cdouble xtt, xtw; + CuDFTW(xtw, fi, Wid, data, dwin); xtt.x=xtw.y; xtw.y=-xtw.x; xtw.x=xtt.x; + CDFTW(xtt, fi, Wid, data, ddwin); + 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, + 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; + if (dtdt!=0) fslope=dwdt/dtdt/(2*M_PI); + else fslope=0; +} //TFReas*/ + +/* + function TFReas: sinusoid estimation using reassignment method + + In: data[Wid]: waveform data + w[Wid+1], dw[Wid+1], ddw[Wid+1]: window function and its derivatives + win[Wid]: window function used for estimating amplitude and phase by projection onto a chirp + t: time for which the parameters are estimated + f: initial frequency at t + Out: f, a, ph: digital frequency, amplitude and phase angle estimated at t + fslope: frequency derivative estimate + + No return value. +*/ +void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win) +{ + double localt=t, logaslope; + TFReas(f, localt, fslope, Wid, data, w, dw, ddw, &logaslope); + + if (logaslope*Wid>6) logaslope=6.0/Wid; + else if (logaslope*Wid<-6) logaslope=-6.0/Wid; + + f=f+fslope*(t-localt); //obtain frequency estimate at t + + cdouble x=0; + if (win==0) + { + for (int n=0; n<Wid; n++) + { + double ni=n-t; + cdouble tmp=data[n]; + double p=-2*M_PI*(f+0.5*fslope*ni)*ni; + tmp.rotate(p); + x+=tmp; + } + a=abs(x)/Wid; + } + else + { + double sumwin=0; + for (int n=0; n<Wid; n++) + { + double ni=n-t; + cdouble tmp=data[n]*win[n]; + double p=-2*M_PI*(f+0.5*fslope*ni)*ni; + tmp.rotate(p); + x+=tmp; sumwin+=win[n]; + } + a=abs(x)/sumwin; + } + ph=arg(x); +}//TFReas + +//--------------------------------------------------------------------------- +/* + Routines for additive and multiplicative reestimation of sinusoids. + + Further reading: Wen X. and M. Sandler, "Additive and multiplicative reestimation schemes + for the sinusoid modeling of audio," in Proc. EUSIPCO'09, Glasgow, 2009. +*/ + +/* + function AdditiveUpdate: additive reestimation of time-varying sinusoid + + In: x[Count]: waveform data + Wid, Offst: frame size and hop + fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters + das[Count]: initial estimate of amplitude derivative + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update + + No return value. +*/ +void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + int HWid=Wid/2, Fr=(Count-Wid)/Offst+1; + + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + + cdouble *y=new cdouble[Count]; + double *lf=new double[Count*4], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3]; + + __int16* ref=new __int16[Count]; + 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); + memcpy(lf, fs, sizeof(double)*Count); + BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, ref, reserved, LogA); + + //merge and interpolate + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3], + *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3]; + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + 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]; + xs[fr]=i; + if (fabs(f-g)*Wid>1) + { + afr[fr]=a, pfr[fr]=fai, ffr[fr]=f; + } + else + { + double rr=a*cos(fai)+b*cos(thet); + double ii=a*sin(fai)+b*sin(thet); + 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)); + afr[fr]=sqrt(rr*rr+ii*ii); + pfr[fr]=atan2(ii, rr); + } + if (LogA) afr[fr]=log(afr[fr]); + } + CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1); + CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1); + for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA); + Sinusoid(&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], pfr[0], pfr[1], LogA); + Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA); + delete[] fa; //*/ + /* + for (int i=0; i<Count; i++) + { + double rr=as[i]*cos(phs[i])+la[i]*cos(lp[i]); + double ii=as[i]*sin(phs[i])+la[i]*sin(lp[i]); + as[i]=sqrt(rr*rr+ii*ii); + phs[i]=atan2(ii, rr); + } //*/ + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + delete[] y; delete[] lf; delete[] ref; +}//AdditiveUpdate + +/* + function AdditiveAnalyzer: sinusoid analyzer with one additive update + + In: x[Count]: waveform data + Wid, Offst: frame size and hop size + BasicAnalyzer: pointer to a sinusoid analyzer + ref[Count]: reference frequencies, in bins, used by BasicAnalyzer + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates + das[Count]: estimate of amplitude derivative + + No return value. +*/ +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) +{ + BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA); + AdditiveUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved, LogA); +}//AdditiveAnalyzer + +/* + function MultiplicativeUpdate: multiplicative reestimation of time-varying sinusoid + + In: x[Count]: waveform data + Wid, Offst: frame size and hop + fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters + das[Count]: initial estimate of amplitude derivative + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update + + No return value. +*/ +void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + int HWid=Wid/2; + cdouble *y=new cdouble[Count]; + double *lf=new double[Count*8], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3], + *lf2=&lf[Count*4], *la2=&lf2[Count], *lp2=&lf2[Count*2], *lda2=&lf2[Count*3]; + __int16 *lref=new __int16[Count]; + + for (int i=0; i<Count; i++) y[i]=x[i]*(cdouble(1.0).rotate(-phs[i]+i*0.15*2*M_PI)), + lref[i]=0.15*Wid; + BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, lref, reserved, LogA); + 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; + BasicAnalyzer(lf2, la2, lp2, lda2, y, Count, Wid, Offst, lref, reserved, LogA); + + /* + for (int i=0; i<Count; i++) + { + as[i]=la[i]*la2[i]; + phs[i]=phs[i]+lp[i]+lp2[i]-0.3*2*M_PI*i; + fs[i]=fs[i]+lf[i]+lf2[i]-0.3; + } //*/ + + //merge + int Fr=(Count-Wid)/Offst+1; + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3], + *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3]; + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + xs[fr]=i; + afr[fr]=la[i]*la2[i]; + if (LogA) afr[fr]=log(afr[fr]); + ffr[fr]=fs[i]+lf[i]-0.15+lf2[i]-0.15; + pfr[fr]=phs[i]+lp[i]+lp2[i]-0.3*i*2*M_PI; + } + CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1); + CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1); + for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA); + Sinusoid(&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], pfr[0], pfr[1], LogA); + Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA); + delete[] fa; //*/ + + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + + delete[] y; delete[] lf; delete[] lref; +}//MultiplicativeUpdate + +/* + function MultiplicativeAnalyzer: sinusoid analyzer with one multiplicative update + + In: x[Count]: waveform data + Wid, Offst: frame size and hop size + BasicAnalyzer: pointer to a sinusoid analyzer + ref[Count]: reference frequencies, in bins, used by BasicAnalyzer + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates + das[Count]: estimate of amplitude derivative + + No return value. +*/ +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) +{ + BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA); + MultiplicativeUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved); +}//MultiplicativeAnalyzer + +/* + This is an earlier version of the multiplicative method without using a user-provided BasicAnalyzer. + This updates the sinusoid estimates at the selected consecutive FRAMES of x. Only frequency modulation + is included in the multiplier. The first frame (0) is centred at x[Wid/2]. fs, as, and phs are based + on frames rather than samples. Updates include frame frst, but not frame fren. +*/ +void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst) +{ + int HWid=Wid/2; + + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *xs=&fa[Fr*8]; + for (int fr=0; fr<Fr; fr++) xs[fr]=HWid+Offst*fr; + CubicSpline(Fr-1, fa, fb, fc, fd, xs, fs, 1, 1); + + int dst=Offst*frst, den=Offst*(fren-1)+Wid, dcount=den-dst; + double *f=new double[dcount*2], *ph=&f[dcount]; + for (int fr=frst; fr<fren-1; fr++) Sinusoid(&f[int(xs[fr])-dst], &ph[int(xs[fr])-dst], 0, Offst, fa[fr], fb[fr], fc[fr], fd[fr], phs[fr], phs[fr+1]); + if (frst==0) Sinusoid(&f[int(xs[0])-dst], &ph[int(xs[0])-dst], -HWid, 0, fa[0], fb[0], fc[0], fd[0], phs[0], phs[1]); + else Sinusoid(&f[int(xs[frst-1])-dst], &ph[int(xs[frst-1])-dst], 0, Offst, fa[frst-1], fb[frst-1], fc[frst-1], fd[frst-1], phs[frst-1], phs[frst]); + if (fren==Fr) Sinusoid(&f[int(xs[fren-2])-dst], &ph[int(xs[fren-2])-dst], Offst, Offst+HWid, fa[fren-2], fb[fren-2], fc[fren-2], fd[fren-2], phs[fren-2], phs[fren-1]); + else Sinusoid(&f[int(xs[fren-1])-dst], &ph[int(xs[fren-1])-dst], 0, Offst, fa[fren-1], fb[fren-1], fc[fren-1], fd[fren-1], phs[fren-1], phs[fren]); + + cdouble* y=new cdouble[Wid]; + AllocateFFTBuffer(Wid, Amp, W, X); + double* win=NewWindow(wtHann, Wid); + int M; double c[10], iH2; windowspec(wtHann, Wid, &M, c, &iH2); + for (int fr=frst; fr<fren; fr++) + { + __int16* lx=&x[Offst*fr]; + double* lph=&ph[Offst*(fr-frst)]; + for (int i=0; i<Wid; i++) y[i]=cdouble(lx[i]).rotate(-lph[i]+i*0.15*2*M_PI); + CFFTCW(y, win, Amp, 0, log2(Wid), W, X); + int pf=0.15*Wid, mpf=pf; + for (int k=pf-4; k<=pf+4; k++) if (Amp[k]>Amp[mpf]) mpf=k; + if (mpf>pf-4 && mpf<pf+4) pf=mpf; + double lfs=pf, lphs; + LSESinusoid(lfs, pf-3, pf+3, X, Wid, 3, M, c, iH2, as[fr], lphs, 1e-3); + fs[fr]=fs[fr]+lfs/Wid-0.15; + phs[fr]+=lphs-0.15*Wid*M_PI; + as[fr]*=2; + } + + delete[] y; + delete[] f; + delete[] win; + delete[] fa; + FreeFFTBuffer(Amp); +}//MultiplicativeUpdateF + +//--------------------------------------------------------------------------- +/* + Earlier reestimation method routines. + + Further reading: Wen X. and M. Sandler, "Evaluating parameters of time-varying + sinusoids by demodulation," in Proc. DAFx'08, Espoo, 2008. +*/ + +/* + function ReEstFreq: sinusoid reestimation by demodulating frequency. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing + win[Wid]: window function for estimating demodulated sinusoid + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, optional + w[Wid/2], ps[Wid], xs[Wid], xc[Wid], fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +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) +{ + int hWid=Wid/2; + //reestimate using frequency track + CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1); + for (int fr=0; fr<FrCount; fr++) + { + //find ps + if (fr==0) + { + double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0]; + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4))); + } +// memset(ps, 0, sizeof(double)*hWid); + } + else if (fr==FrCount-1) + { + int lfr=FrCount-2; + double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa))); + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<Wid; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } +// memset(&ps[hWid], 0, sizeof(double)*hWid); + } + else + { + int lfr=fr-1; + double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<hWid+1; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + lfr=fr; + lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + for (int j=1; j<hWid; j++) + { + ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + } + double* ldata=&x[fr*Offst]; + for (int j=0; j<Wid; j++) + { + xs[j].x=ldata[j]*cos(-ps[j]); + xs[j].y=ldata[j]*sin(-ps[j]); + } + + if (Wids) + { + int lWid=Wids[fr], lhWid=Wids[fr]/2, lM; + SetTwiddleFactors(lWid, w); + double *lwin=NewWindow(wtHann, lWid), lc[4], liH2; + windowspec(wtHann, lWid, &lM, lc, &liH2); + CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc); + delete[] lwin; + double lf=fbuf[fr]*lWid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp; + } + else + { + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) + fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp; + } + } +}//ReEstFreq + +/* + function ReEstFreq_2: sinusoid reestimation by demodulating frequency. This is that same as ReEstFreq(...) + except that it calls Sinusoid(...) to synthesize the phase track used for demodulation and that it + does not allow variable window sizes for estimating demodulated sinusoid. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing + win[Wid]: window function for LSE sinusoid estimation + M, c[], iH2: cosine-family window specification parameters, must be consistent with M, c, iH2 + w[Wid/2], xs[Wid], xc[Wid], f3[FrCount-1], f2[FrCount-1], f1[FrCount-1], f0[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +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) +{ + int hWid=Wid/2; + //reestimate using frequency track + CubicSpline(FrCount-1, f3, f2, f1, f0, ns, fbuf, 1, 1); + double *refcos=(double*)malloc8(sizeof(double)*Wid), *refsin=&refcos[hWid], ph=0, centralph; + + memset(f0, 0, sizeof(double)*FrCount); + + int N=Wid+Offst*(FrCount-1); + double* cosine=new double[N], *sine=new double[N]; + Sinusoid(&cosine[hWid], &sine[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph); + for (int fr=0; fr<FrCount-1; fr++) + { + int ncentre=hWid+Offst*fr; + if (fr==FrCount-2) Sinusoid(&cosine[ncentre], &sine[ncentre], 0, Wid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + else Sinusoid(&cosine[ncentre], &sine[ncentre], 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + } + double err=0; + 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;} + + ph=0; + for (int fr=0; fr<FrCount; fr++) + { + double* ldata=&x[fr*Offst-hWid]; + + //store first half of demodulated frame to xs[0:hWid-1] + if (fr==0) + { + Sinusoid(&refcos[hWid], &refsin[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph); + for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i]; + } + else + { + ph=0; + Sinusoid(refcos, refsin, 0, hWid, f3[fr-1], f2[fr-1], f1[fr-1], f0[fr-1], ph); + for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i]; + } + + //taking care of phase angles + if (fr==FrCount-1) {double tmp=ph; ph=centralph; centralph=tmp;} + else centralph=ph; + + double *lrefcos=&refcos[-hWid], *lrefsin=&refsin[-hWid]; + //store second half of demodulated frame to xs[hWid:Wid-1] + if (fr==FrCount-1) + { + Sinusoid(lrefcos, lrefsin, hWid, Wid, f3[FrCount-2], f2[FrCount-2], f1[FrCount-2], f0[FrCount-2], ph); + for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i]; + } + else + { + Sinusoid(refcos, refsin, 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i]; + } + + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) + fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp+centralph; + } +}//ReEstFreq_2 + +/* + function ReEstFreqAmp: sinusoid reestimation by demodulating frequency and amplitude. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], abuf[FrCount], ns[FrCount]: initial frequency and amplitude estiamtes and their + timing + win[Wid]: window function for estimating demodulated sinusoid + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, + optional + w[Wid/2], ps[Wid], xs[Wid], xc[Wid]: buffers + fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers + aa[FrCount-1], ab[FrCount-1], ac[FrCount-1], ad[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +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) +{ + int hWid=Wid/2; + //reestimate using amplitude and frequency track + CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1); + CubicSpline(FrCount-1, aa, ab, ac, ad, ns, abuf, 0, 1); + for (int fr=0; fr<FrCount; fr++) + { + if (fr==0) + { + double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0], + lad=ad[0], lac=ac[0], lab=ab[0], laa=aa[0]; + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4))); + } + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + as[j]=lad+lx*(lac+lx*(lab+lx*laa)); + } + } + else if (fr==FrCount-1) + { + int lfr=FrCount-2; + double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa))); + double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<Wid; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + as[0]=ad[lfr]; + for (int j=0; j<Wid; j++) + { + as[j]=lad+j*(lac+j*(lab+j*laa)); + } + } + else + { + int lfr=fr-1; + double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=0; j<hWid+1; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + as[j]=lad+j*(lac+j*(lab+j*laa)); + } + lfr=fr; + lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + for (int j=1; j<hWid; j++) + { + ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + as[j+hWid]=lad+j*(lac+j*(lab+j*laa)); + } + } + double *ldata=&x[fr*Offst]; + for (int j=0; j<Wid; j++) + { + double tmp; + if ((fr==0 && j<hWid) || (fr==FrCount-1 && j>=hWid)) tmp=1; + else if (as[hWid]>100*as[j]) tmp=100; + else tmp=as[hWid]/as[j]; + tmp=tmp*ldata[j]; + xs[j].x=tmp*cos(-ps[j]); + xs[j].y=tmp*sin(-ps[j]); + } + + if (Wids) + { + int lWid=Wids[fr], lhWid=Wids[fr]/2, lM; + SetTwiddleFactors(lWid, w); + double *lwin=NewWindow(wtHann, lWid), lc[4], liH2; + windowspec(wtHann, lWid, &lM, lc, &liH2); + CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc); + delete[] lwin; + double lf=fbuf[fr]*lWid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp; + } + else + { + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp; + } + } +}//ReEstFreqAmp + +/* + function Reestimate2: iterative demodulation method for sinusoid parameter reestimation. + + In: x[(FrCount-1)*Offst+Wid]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + win[Wid]: window function + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, + optional + maxiter: maximal number of iterates + ae[FrCount], fe[FrCount], pe[FrCount]: initial amplitude, frequency and phase estimates + Out: aret[FrCount], fret[FrCount], pret[FrCount]: reestimated amplitudes, frequencies and phase angles + + Returns the number of unused iterates left of the total of maxiter. +*/ +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) +{ + AllocateFFTBuffer(Wid, fft, w, xc); + double convep=1e-4, dif=0, lastdif=0; //convep is the hard-coded threshold that stops the iteration + int iter=1, hWid=Wid/2; + + double *ns=new double[FrCount*12], *as=new double[Wid*5]; + double *fbuf=&ns[FrCount], *abuf=&ns[FrCount*2], + *aa=&ns[FrCount*3], *ab=&ns[FrCount*4], *ac=&ns[FrCount*5], *ad=&ns[FrCount*6], + *fa=&ns[FrCount*7], *fb=&ns[FrCount*8], *fc=&ns[FrCount*9], *fd=&ns[FrCount*10], + *pbuf=&ns[FrCount*11]; + double *ps=&as[Wid]; + cdouble *xs=(cdouble*)&as[Wid*3]; + + memcpy(fbuf, fe, sizeof(double)*FrCount); + memcpy(abuf, ae, sizeof(double)*FrCount); + memcpy(pbuf, pe, sizeof(double)*FrCount); + for (int i=0; i<FrCount; i++) + { + ns[i]=hWid+i*Offst; + } + + while (iter<=maxiter) + { + ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids); + ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids); + 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); + + if (iter>1) lastdif=dif; + dif=0; + if (iter==1) + { + for (int fr=0; fr<FrCount; fr++) + { + if (fabs(abuf[fr])>fabs(ae[fr])) + dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/abuf[fr]); + else + dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/ae[fr]); + } + } + else + { + for (int fr=0; fr<FrCount; fr++) + { + if (fabs(abuf[fr])>fabs(aret[fr])) + dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/abuf[fr]); + else + dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/aret[fr]); + } + } + memcpy(fret, fbuf, sizeof(double)*FrCount); + memcpy(aret, abuf, sizeof(double)*FrCount); + dif/=FrCount; + if (fabs(dif)<convep || (iter>1 && fabs(dif-lastdif)<convep*lastdif)) break; + iter++; + } + + memcpy(pret, pbuf, sizeof(double)*FrCount); + + delete[] ns; + delete[] as; + delete[] fft; + + return maxiter-iter; +}//Reestimate2 + +//--------------------------------------------------------------------------- +/* + Derivative method as proposed in DAFx09 + + Further reading: Wen X. and M. Sandler, "Notes on model-based non-stationary sinusoid estimation methods + using derivatives," in Proc. DAFx'09, Como, 2009. +*/ + +/* + function Derivative: derivative method for estimating amplitude derivative, frequency, and frequency derivative given + signal and its derivatives. + + In: x[Wid], dx[Wid], ddx[Wid]: waveform and its derivatives + win[Wid]: window function + f0: initial digital frequency estimate + Out: f0: new estimate of digital frequency + f1, a1: estimates of frequency and amplitude derivatives + + No return value. +*/ +void Derivative(int Wid, double* win, cdouble* x, cdouble* dx, cdouble* ddx, double& f0, double* f1, double* a0, double* a1, double* ph) +{ + AllocateFFTBuffer(Wid, fft, W, X); + CFFTCW(x, win, fft, NULL, log2(Wid), W, X); + int m=f0*Wid, m0=m-10, m1=m+10, hWid=Wid/2; + if (m0<0) m0=0; if (m1>hWid) m1=hWid; + for (int n=m0; n<=m1; n++) if (fft[n]>fft[m]) m=n; + cdouble Sw=0, S1w=0, S2w=0; + for (int n=0; n<Wid; n++) + { + cdouble tmp=x[n]*win[n]; + Sw+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid); + tmp=dx[n]*win[n]; + S1w+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid); + } + double omg0=(S1w/Sw).y; + Sw=0, S1w=0; + for (int n=0; n<Wid; n++) + { + cdouble tmp=x[n]*win[n]; + Sw+=tmp.rotate(-omg0*(n-hWid)/Wid); + tmp=dx[n]*win[n]; + S1w+=tmp.rotate(-omg0*(n-hWid)/Wid); + tmp=ddx[n]*win[n]; + S2w+=tmp.rotate(-omg0*(n-hWid)/Wid); + } + omg0=(S1w/Sw).y; + double miu0=(S1w/Sw).x; + double psi0=(S2w/Sw).y-2*miu0*omg0; + + f0=omg0/(2*M_PI); + *f1=psi0/(2*M_PI); + *a1=miu0; + + FreeFFTBuffer(fft); +}//Derivative + +/* + function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency omg, + from x using window w and its derivatives. + + In: x[Wid]: waveform data + w[K+1][Wid]: window functions and its derivatives up to order K + omg: angular frequency + Out: X[K+1]: windowed spectrum and its derivatives up to order K + + No return value. This function is for internal use. +*/ +void Xkw(cdouble* X, int K, int Wid, double* x, double** w, double omg) +{ + int hWid=Wid/2; + //calculate the first row + memset(X, 0, sizeof(cdouble)*(K+1)); + for (int i=0; i<Wid; i++) + { + double n=i-hWid; + double ph=omg*n; + for (int k=0; k<=K; k++) + { + cdouble tmp=x[i]*w[k][i]; + X[k]+=tmp.rotate(-ph); + } + } + //calculate the rest rows + for (int k=1; k<=K; k++) + { + cdouble *thisX=&X[k], *lastX=&X[k-1]; + for (int kk=K-k; kk>=0; kk--) thisX[kk]=-lastX[kk+1]+cdouble(0, omg)*lastX[kk]; + } +}//Xkw + +/* + function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency + omg, from x and its derivatives using window w. + + In: x[K+1][Wid]: waveform data and its derivatives up to order K. + w[Wid]: window function + omg: angular frequency + Out: X[K+1]: windowed spectrum and its derivatives up to order K + + No return value. This function is for testing only. +*/ +void Xkw(cdouble* X, int K, int Wid, double** x, double* w, double omg) +{ + int hWid=Wid/2; + memset(X, 0, sizeof(cdouble)*(K+1)); + for (int i=0; i<Wid; i++) + { + double n=i-hWid; + double ph=omg*n; + for (int k=0; k<=K; k++) + { + cdouble tmp=x[k][i]*w[i]; + X[k]+=tmp.rotate(-ph); + } + } +}//Xkw + +/* + function Derivative: derivative method for estimating the model log(s)=h[M]'r[M], by discarding extra + equations + + In: s[Wid]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +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) +{ + int hWid=Wid/2, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0); //number of derivatives required + + //ind marks the 2*M1 real elements of an M1-array of complex unknowns with + // numerical indices (0-based) or -1 if it is not a real unknown variable + //uind marks the Kr real unknowns with their positions in ind + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + { + 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(""); + } + + cdouble* Skw=new cdouble[M]; + Xkw(Skw, Kc, Wid, s, win, omg); + + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + //allocate buffer for linear system A(pq)=b + Alloc2(2*Kc+2, Kr, A); double** AA; double *bb, *pqpq; + double *b=A[2*Kc], *pq=A[2*Kc+1]; + for (int k=0; k<Kr; k++) b[k]=((double*)(&Skw[1]))[k]; + // *pq=(double*)(&r[1]); + for (int k=0; k<Kc; k++) //looping through rows of A + { + //columns of A includes rows of Smkw corresponding to real unknowns + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) //the real part being unknown + { + A[2*k][lind]=Smkw[m+1][k].x; + A[2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) //the imag part being unknown + { + A[2*k+1][lind]=Smkw[m+1][k].x; + A[2*k][lind]=-Smkw[m+1][k].y; + } + } + } + + bool dropeq=(2*Kc-1==Kr); + if (dropeq) + { + Allocate2(double, Kr+2, Kr, AA); + bb=AA[Kr], pqpq=AA[Kr+1]; + memcpy(AA[0], A[0], sizeof(double)*Kr*(Kr-1)); + memcpy(AA[Kr-1], A[Kr], sizeof(double)*Kr); + memcpy(bb, b, sizeof(double)*(Kr-1)); + bb[Kr-1]=((double*)(&Skw[1]))[Kr]; + } + + double det; + GECP(Kr, pq, A, b, &det); + if (dropeq) + { + double det2; + GECP(Kr, pqpq, AA, bb, &det2); + if (fabs(det2)>fabs(det)) memcpy(pq, pqpq, sizeof(double)*Kr); + DeAlloc2(AA); + } + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + + //estiamte r0 + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw[0]/e0); + + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); +}//Derivative*/ + +/* + function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M], least-square + implementation + + In: s[Wid]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + K: number of derivatives to take + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +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) +{ + int hWid=Wid/2, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0); //number of derivatives required + if (Kc<K) Kc=K; + + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + {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("");} + + //allocate buffer for linear system A(pq)=b + cdouble* Skw=new cdouble[Kc+1]; + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + + Alloc2(2*Kc+2, 2*Kc, A); + double *b=A[2*Kc], *pq=A[2*Kc+1]; + + Xkw(Skw, Kc, Wid, s, win, omg); + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + for (int k=0; k<2*Kc; k++) b[k]=((double*)(&Skw[1]))[k]; + for (int k=0; k<Kc; k++) + { + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) + { + A[2*k][lind]=Smkw[m+1][k].x; + A[2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) + { + A[2*k+1][lind]=Smkw[m+1][k].x; + A[2*k][lind]=-Smkw[m+1][k].y; + } + } + } + + if (2*Kc==Kr) GECP(Kr, pq, A, b); + else LSLinear2(2*Kc, Kr, pq, A, b); + + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + //estiamte r0 + if (r0) + { + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw[0]/e0); + } + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); +}//DerivativeLS + +/* + function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M] using Fr + measurement points a quarter of Wid apart from each other, implemented by least-square. + + In: s[Wid+(Fr-1)*Wid/4]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + Fr: number of measurement points + K: number of derivatives to take at each measurement point + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + r0: specifies if r[0] is to be computed. + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +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) +{ + int hWid=Wid/2, qWid=Wid/4, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0/Fr); //number of derivatives required + if (Kc<K) Kc=K; + + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + {int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;}} + + //allocate buffer for linear system A(pq)=b + cdouble* Skw=new cdouble[Kc+1], Skw00; + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + + Alloc2(2*Fr*Kc, 2*Fr*Kc, A); + double *pq=new double[2*Fr*Kc], *b=new double[2*Fr*Kc]; + + for (int fr=0; fr<Fr; fr++) + { + int Offst=qWid*fr; double* ss=&s[Offst]; + + Xkw(Skw, Kc, Wid, ss, win, omg); if (fr==0) Skw00=Skw[0]; + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i+Offst-hWid, harg)*ss[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + for (int k=0; k<2*Kc; k++) b[2*fr*Kc+k]=((double*)(&Skw[1]))[k]; + for (int k=0; k<Kc; k++) + { + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) + { + A[2*fr*Kc+2*k][lind]=Smkw[m+1][k].x; + A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) + { + A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].x; + A[2*fr*Kc+2*k][lind]=-Smkw[m+1][k].y; + } + } + } + } + if (2*Fr*Kc==Kr) GECP(Kr, pq, A, b); + else LSLinear2(2*Fr*Kc, Kr, pq, A, b); + + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + //estiamte r0 + if (r0) + { + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw00/e0); + } + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); + delete[] pq; delete[] b; +}//DerivativeLS + +//--------------------------------------------------------------------------- +/* + Abe-Smith sinusoid estimator 2005 + + Further reading: M. Abe and J. O. Smith III, ¡°AM/FM rate estimation for time-varying sinusoidal + modeling,¡± in Proc. ICASSP'05, Philadelphia, 2005. +*/ + +/* + function RDFTW: windowed DTFT at frequency k bins + + In: data[Wid]: waveform data + w[Wid]: window function + k: frequency, in bins + Out: Xr, Xi: real and imaginary parts of the DTFT of xw at frequency k bins + + No return value. +*/ +void RDFTW(double& Xr, double& Xi, double k, int Wid, double* data, double* w) +{ + Xr=Xi=0; + int hWid=Wid/2; + double* lw=&w[Wid]; + for (int i=0; i<=Wid; i++) + { + double tmp; + tmp=*data**lw; + data++, lw--; +//* + double ph=-2*M_PI*(i-hWid)*k/Wid; + Xr+=tmp*cos(ph); + Xi+=tmp*sin(ph); //*/ + } +}//RDFTW + +/* + function TFAS05: the Abe-Smith method 2005 + + In: data[Wid]: waveform data + w[Wid]: window function + res: resolution of frequency for QIFFT + Out: f, a, ph: frequency, amplitude and phase angle estimates + aesp, fslope: estimates of log amplitude and frequency derivatives + + No return value. +*/ +void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res) +{ + double fi=floor(f*Wid+0.5); //frequency (int) in bins + double xr0, xi0, xr_1, xi_1, xr1, xi1; + RDFTW(xr0, xi0, fi, Wid, data, w); + RDFTW(xr_1, xi_1, fi-res, Wid, data, w); + RDFTW(xr1, xi1, fi+res, Wid, data, w); + double winnorm=0; for (int i=0; i<=Wid; i++) winnorm+=w[i]; + double y0=log(sqrt(xr0*xr0+xi0*xi0)/winnorm), + y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm), + y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm); + double df=0; +//* + if (y0<y1) + { + double newfi=fi+res; + while (y0<y1) + { + y_1=y0, xr_1=xr0, xi_1=xi0; + y0=y1, xr0=xr1, xi0=xi1; + newfi+=res; + RDFTW(xr1, xi1, newfi, Wid, data, w); + y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm); + fi+=res; + } + } + else if(y0<y_1) + { + double newfi=fi-res; + while (y0<y_1) + { + y1=y0, xr1=xr0, xi1=xi0; + y0=y_1, xr0=xr_1, xi0=xi_1; + newfi-=res; + RDFTW(xr_1, xi_1, newfi, Wid, data, w); + y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm); + fi-=res; + } + } //*/ + + double a2=(y1+y_1)*0.5-y0, a1=(y1-y_1)*0.5, a0=y0; + df=-a1*0.5/a2; + f=fi+df*res; //in bins + double y=a0-0.25*a1*a1/a2; + a=exp(y); + double ph0=(xi0==0 && xr0==0)?0:atan2(xi0, xr0), + ph_1=(xi_1==0 && xr_1==0)?0:atan2(xi_1, xr_1), + ph1=(xi1==0 && xr1==0)?0:atan2(xi1, xr1); + if (fabs(ph_1-ph0)>M_PI) + { + if (ph_1-ph0>0) ph_1-=M_PI*2; + else ph_1+=M_PI*2; + } + if (fabs(ph1-ph0)>M_PI) + { + if (ph1-ph0>0) ph1-=M_PI*2; + else ph1+=M_PI*2; + } + double b2=(ph1+ph_1)*0.5-ph0, b1=(ph1-ph_1)*0.5, b0=ph0; + ph=b0+b1*(df+b2*df); + //now we have the QI estimates + double uff=2*a2, vf=b1+2*b2*df, vff=2*b2; + double dfdp=Wid/(2*M_PI*res); + double upp=uff*dfdp*dfdp, vp=vf*dfdp, vpp=vff*dfdp*dfdp; + double p=-upp*0.5/(upp*upp+vpp*vpp); + double alf=-2*p*vp, beta=p*vpp/upp; + //*direct method + double beta_p=beta/p; + double feses=f-alf*beta/p /(2*M_PI)*Wid, + yeses=y-alf*alf*0.25/p+0.25*log(1+beta_p*beta_p), + pheses=ph+alf*alf*beta*0.25/p-0.5*atan(beta_p); //*/ + /*adapted method + double zt[]={0, 0.995354, 0.169257, 1.393056, 0.442406, -0.717980, -0.251620, 0.177511, 0.158120, -0.503299}; + double delt=res/Wid; double delt0=df*delt; + beta=zt[3]*beta+zt[4]*delt0*alf; + alf=(zt[1]+zt[2]*delt*delt)*alf; + double beta_p=beta/p; + double feses=f+zt[5]*alf*beta/p /(2*M_PI)*Wid, + yeses=y+zt[6]*alf*alf/p+zt[7]*log(1+beta_p*beta_p), + pheses=ph+zt[8]*alf*alf*beta/p+zt[9]*atan(beta_p); //*/ + f=feses/Wid, a=exp(yeses), ph=pheses, fslope=2*beta/2/M_PI, aesp=alf; +}//TFAS05 + +/* + function TFAS05_enh: the Abe-Smith method 2005 enhanced by LSE amplitude and phase estimation + + In: data[Wid]: waveform data + w[Wid]: window function + res: resolution of frequency for QIFFT + Out: f, a, ph: frequency, amplitude and phase angle estimates + aesp, fslope: estimates of log amplitude and frequency derivatives + + No return value. +*/ +void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res) +{ + TFAS05(f, t, a, ph, aesp, fslope, Wid, data, w, res); + double xr=0, xi=0, p, win2=0; + for (int n=0; n<=Wid; n++) + { + double ni=n-Wid/2, tmp=data[n]*w[n]*w[n];//*exp(-aesp*(n-Wid/2)); if (IsInfinite(tmp)) continue; + p=-2*M_PI*(f+0.5*fslope*ni)*ni; + xr+=tmp*cos(p); + xi+=tmp*sin(p); + win2+=w[n]*w[n]; + } + a=sqrt(xr*xr+xi*xi)/win2; + ph=(xr==0 && xi==0)?0:atan2(xi, xr); +}//TFAS05_enh +//version without returning aesp and fslope +void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res) +{ + double aesp, fslope; + TFAS05_enh(f, t, a, ph, aesp, fslope, Wid, data, w, res); +}//TFAS05_enh + +//--------------------------------------------------------------------------- +/* + function DerivativeLSv_AmpPh: estimate the constant-term in the local derivative method. This is used + by the local derivative algorithm, whose implementation is found in the header file as templates. + + In: sv0: inner product <s, v0>, where s is the sinusoid being estimated. + integr_h[M][Wid]: M vectors containing samples of the integral of basis functions h[M]. + v0[M]: a test function + lmd[M]: coefficients to h[M] + + Returns coefficient of integr_h[0]=1. +*/ +cdouble DerivativeLSv_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, cdouble* v0, cdouble sv0) +{ + cdouble e0=0; + for (int n=0; n<Wid; n++) + { + cdouble expo=0; + for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n]; + e0+=exp(expo)**v0[n]; + } + return log(sv0/e0); +}//DerivativeLSv_AmpPh + +//--------------------------------------------------------------------------- +/* + Piecewise derivative algorithm + + Further reading: Wen X. and M. Sandler, "Spline exponential approximation of time-varying + sinusoids," under review. +*/ + +/* + function setv: computes I test functions v[I] by modulation u[I] to frequency f + + In: u[I+1][Wid], du[I+1][Wid]: base-band test functions and their derivatives + f: carrier frequency + Out: v[I][Wid], dv[I][Wid]: test functions and their derivatives + + No return value. +*/ +void setv(int I, int Wid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du) +{ + double fbin=floor(f*Wid+0.5)/Wid; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<Wid; c++) + { + double t=c; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I-1; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I-1; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + //Here it is assumed that elements of u[] are modulated at 0, 1, -1, 2, -2, 3, -3, 4, ...; + //if f is under fbin then the closest ones are in order 0, -1, 1, -2, 3, -3, 3, .... This + //makes a difference to the whole of v[] only if I is even. + 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];} + else{v[I-1][c]=u[I][c]*rot; dv[I-1][c]=du[I][c]*rot+jomg*v[I-1][c];} + } +}//setv + +/* + function setvhalf: computes I half-size test functions v[I] by modulation u[I] to frequency f. + + In: u[I][hWid*2], du[I][Wid*2]: base-band test functions and their derivatives + f: carrier frequency + Out: v[I][hWid], dv[hWid]: half-size test functions and their derivatives + + No return value. +*/void setvhalf(int I, int hWid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du) +{ + double fbin=floor(f*hWid)/hWid; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<hWid; c++) + { + double t=c; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } +}//setvhalf + +//#define ERROR_CHECK + +/* + function DerivativePiecewise: Piecewise derivative algorithm. In this implementation of the piecewise + method the test functions v are constructed from I "basic" (single-frame) test functions, each + covering the same period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I + test functions are used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + A[L][M][N]: L matrices that map independent coefficients onto component coefficients over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: aita[N]: independent coefficients + + No return value. +*/ +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) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, N, srv, mlist); + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + +#ifdef ERROR_CHECK + cdouble dsv1[128], dsv2[128]; +#endif + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + double fbin=floor(f[l+1]*T*2)/(T*2.0); + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T*2; c++) + { + double t=c-T; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + //memset(srv[l][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[l], shv1, A[l]); + MultiAddXY(I, M, N, srv[l], shv2, A[l+1]); + +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[l*T]; + for (int i=0; i<I && l*I+1<36; i++) + { + cdouble lsv=Inner(2*T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[l*I+i]=lsv-sv[l][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && l*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[l][i][n]*aita[n]; + dsv2[l*I+i]=lsv-sv[l][i]-dsv1[l*I+i]; + } + } +#endif + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[0]+f[1])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[L_1], shv1, A[0]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[0]; + for (int i=0; i<I && L_1*I+1<36; i++) + { + cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && L_1*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n]; + dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i]; + } + } +#endif + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[L-1]+f[L])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[L_1], shv1, A[L-1]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[(L-1)*T]; + for (int i=0; i<I && L_1*I+1<36; i++) + { + cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && L_1*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n]; + dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i]; + } + } +#endif + L_1++; + } + + if (L_1*2*I==2*N) GECP(N, aita, srv[0], sv[0]); + else LSLinear(L_1*I, N, aita, srv[0], sv[0]); + + delete mlist; +}//DerivativePiecewise + +/* + function DerivativePiecewise2: Piecewise derivative algorithm in which the real and imaginary parts of + the exponent are modelled separately. In this implementation of the piecewise method the test + functions v are constructed from I "basic" (single-frame) test functions, each covering the same + period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are + used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + A[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces + B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients + + No return value. +*/ +void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B, + int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist); + cdouble*** srbv; + if (Np==Nq && B==A) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + double fbin=floor(f[l+1]*T*2)/(T*2.0); + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T*2; c++) + { + double t=c-T; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + memset(srav[l][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[l], shv1, A[l]); + MultiAddXY(I, M, Np, srav[l], shv2, A[l+1]); + if (srbv!=srav) //so that either B!=A or Np!=Nq + { + //memset(srbv[l][0], 0, sizeof(cdouble)*I*Nq); + MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]); + MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]); + } + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[0]+f[1])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, A[0]); + if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);} + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[L-1]+f[L])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, A[L-1]); + if (srbv!=srav) + { + //memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); + MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]); + } + L_1++; + } + + //real implementation of <sr,v>aita=<s',v> + double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist); + for (int l=0; l<L_1; l++) for (int i=0; i<I; i++) + { + int li=l*I+i, li_H=li+L_1*I; + for (int n=0; n<Np; n++) + { + AM[li][n]=srav[l][i][n].x; + AM[li_H][n]=srav[l][i][n].y; + } + for (int n=0; n<Nq; n++) + { + AM[li][Np+n]=-srbv[l][i][n].y; + AM[li_H][Np+n]=srbv[l][i][n].x; + } + } + //least-square solution of (srv)(aita)=(sv) + double* pq=new double[Np+Nq]; mlist->Add(pq, 1); + 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; + + if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b); + else LSLinear(2*L_1*I, Np+Nq, pq, AM, b); + + memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq); + + delete mlist; +}//DerivativePiecewise2 + +/* + Error check: test that ds[LT] equals s[LT] times reconstructed R'. Notice that DA is D time A where D + is a pre-emphasis because p[Np] applies to log amplitude rather than its derivative. +*/ +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) +{ + double err=0, ene=0, *lamdax=new double[M*2], *lamday=&lamdax[M]; + for (int l=0; l<L; l++) + { + MultiplyXy(M, Np, lamdax, DA[l], p); + MultiplyXy(M, Nq, lamday, B[l], q); + for (int t=0; t<T; t++) + { + double drtx=0; for (int m=0; m<M; m++) drtx+=lamdax[m]*h[m][t]; + double drty=0; for (int m=0; m<M; m++) drty+=lamday[m]*h[m][t]; + cdouble drt=cdouble(drtx, drty); + cdouble eds=ds[l*T+t]-s[l*T+t]*drt; + err+=~eds; ene+=~ds[l*T+t]; + if (errds) errds[l*T+t]=eds; + } + } + delete[] lamdax; + return err/ene; +}//testds_pqA + +/* + Error check: dsv1[I] tests that <s', v[I]> equals -<s, v[I]'>, dsv2[I] tests that <sr, v[I]>*pq= + <s',v[I]> +*/ +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) +{ + for (int i=0; i<I; i++) + { + cdouble lsv=Inner(TT, dsl, vl[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + dsv1[i]=lsv-svl[i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + //sv[l][i]=lsv; + } + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I; i++) + { + cdouble lsv=0; + for (int n=0; n<Np; n++) lsv+=sravl[i][n]*p[n]; + for (int n=0; n<Nq; n++) lsv+=srbvl[i][n]*cdouble(0, q[n]); + dsv2[i]=lsv-svl[i]-dsv1[i]; + } +}//testdsv + +/* + Error check: tests A[MN]x[N1]=b[N1], returns square error +*/ +double testlinearsystem(int M, int N, double** A, double* x, double* b) +{ + double err=0; + for (int m=0; m<M; m++) + { + double errli=Inner(N, A[m], x)-b[m]; + err+=errli*errli; + } + return err; +}//testlinearsystem + +/* + Error check: test the total square norm of <s, v> +*/ +double testsv(int L, double* f, int T, cdouble* s, int I, cdouble** u, cdouble** du, int endmode) +{ + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + double ene=0; + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + setv(I, T*2, v, dv, f[l+1], u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; + for (int i=0; i<I; i++) + { + cdouble d=Inner(2*T, ls, v[i]); + ene+=~d; + } + } + if (endmode==1 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du); + cdouble* ls=&s[0]; + for (int i=0; i<I; i++) + + ene+=~Inner(T, ls, v[i]); + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du); + cdouble* ls=&s[(L-1)*T]; + for (int i=0; i<I; i++) + ene+=~Inner(T, ls, v[i]); + } + DeAlloc2(v); DeAlloc2(dv); + return ene; +}//testsv + +/* + function DerivativePiecewise3: Piecewise derivative algorithm in which the log amplitude and frequeny + are modeled separately as piecewise functions. In this implementation of the piecewise method the test + functions v are constructed from I "basic" (single-frame) test functions, each covering the same + period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are + used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + dh[M][T]: derivative of h[M][T], used only if ERROR_CHECK is defined. + DA[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces + B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients + + No return value. +*/ +void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B, + int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds, double** dh) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2L(cdouble, I, T*2, v, mlist); + cdouble** Allocate2L(cdouble, I, T*2, dv, mlist); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist); + cdouble*** srbv; + if (Np==Nq && B==DA) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + +#ifdef ERROR_CHECK + cdouble dsv1in[128], dsv2in[128]; +#endif + + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + setv(I, T*2, v, dv, f[l+1], u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + memset(srav[l][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[l], shv1, DA[l]); + MultiAddXY(I, M, Np, srav[l], shv2, DA[l+1]); + if (srbv!=srav) //so that either B!=A or Np!=Nq + { + MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]); + MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]); + } +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + 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]); +#endif + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, DA[0]); + if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);} +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + 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]); +#endif + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, DA[L-1]); + if (srbv!=srav) MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + 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]); +#endif + L_1++; + } + + //real implementation of <sr,v>aita=<s',v> + double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist); + for (int l=0; l<L_1; l++) for (int i=0; i<I; i++) + { + int li=l*I+i, li_H=li+L_1*I; + for (int n=0; n<Np; n++) + { + AM[li][n]=srav[l][i][n].x; + AM[li_H][n]=srav[l][i][n].y; + } + for (int n=0; n<Nq; n++) + { + AM[li][Np+n]=-srbv[l][i][n].y; + AM[li_H][Np+n]=srbv[l][i][n].x; + } + } + //least-square solution of (srv)(aita)=(sv) + double* pq=new double[Np+Nq]; mlist->Add(pq, 1); + 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); +#ifdef ERROR_CHECK + //tests that AM is invariant to a constant shift of p + double errAM=0, errAM2=0, err1, err2; + 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;} + //test square error of the input pq + if (ds) + { + memcpy(pq, p, sizeof(double)*Np); memcpy(&pq[Np], q, sizeof(double)*Nq); + err1=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b); + } + //test error of s'-sR where R is synthesized from the input pq + double errdsin, errdsvin; cdouble* edsin; + if (ds && dh) + { + edsin=new cdouble[L*T]; mlist->Add(edsin, 1); + errdsin=testds_pqA(Np, p, Nq, q, L, T, s, ds, M, h, dh, DA, B, edsin); + errdsvin=testsv(L, f, T, edsin, I, u, du, endmode); + } +#endif + Alloc2L(L_1*I*2, Np+Nq-1, Am, mlist); + for (int l=0; l<L_1*I*2; l++) memcpy(Am[l], &AM[l][1], sizeof(double)*(Np+Nq-1)); + pq[0]=0; + //if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b); + //else LSLinear(2*L_1*I, Np+Nq, pq, AM, b); + if (L_1*2*I==Np+Nq-1) GECP(Np+Nq-1, &pq[1], Am, b); + else LSLinear(2*L_1*I, Np+Nq-1, &pq[1], Am, b); +#ifdef ERROR_CHECK + //test square error of output pq + if (ds) err2=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b); + //test error of s'-sR of the output pq + double errdsout, errdsvout; cdouble* edsout; + if (ds && dh) + { + edsout=new cdouble[L*T]; mlist->Add(edsout, 1); + errdsout=testds_pqA(Np, pq, Nq, &pq[Np], L, T, s, ds, M, h, dh, DA, B, edsout); + errdsvout=testsv(L, f, T, edsout, I, u, du, endmode); + } +#endif + memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq); + + delete mlist; +}//DerivativePiecewise3 + +//initialization routines for the piecewise derivative method + +/* + function seth: set h[M] to a series of power functions. + + In: M, T. + Out: h[M][T], where h[m] is power function of order m. + + No return value. h is allocated anew and must be freed by caller. +*/ +void seth(int M, int T, double**& h, MList* mlist) +{ + if (M<=0){h=0; return;} + Allocate2L(double, M, T, h, mlist); + double* hm=h[0]; for (int t=0; t<T; t++) hm[t]=1; + for (int m=1; m<M; m++) + { + hm=h[m]; for (int t=0; t<T; t++) hm[t]=pow(t*1.0, m); + } +}//seth + +/* + function setdh: set dh[M] to the derivative of a series of power functions. + + In: M, T. + Out: dh[M][T], where dh[m] is derivative of the power function of order m. + + No return value. dh is allocated anew and must be freed by caller. +*/ +void setdh(int M, int T, double**& dh, MList* mlist) +{ + if (M<=0){dh=0; return;} + Allocate2L(double, M, T, dh, mlist); + double* dhm=dh[0]; memset(dhm, 0, sizeof(double)*T); + if (M>1){dhm=dh[1]; for (int t=0; t<T; t++) dhm[t]=1;} + for (int m=2; m<M; m++) + { + dhm=dh[m]; for (int t=0; t<T; t++) dhm[t]=m*pow(t*1.0, m-1); + } +}//setdh + +/* + function setdih: set dih[M] to the difference of the integral of a series of power functions. + + In: M, I + Out: dih[M][I], where the accumulation of dih[m] is the integral of the power function of order m. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void setdih(int M, int T, double**& dih, MList* mlist) +{ + if (M<=0){dih=0; return;} + Allocate2L(double, M, T, dih, mlist); + double* dihm=dih[0]; for (int t=0; t<T; t++) dihm[t]=1; + for (int m=1; m<M; m++) + { + 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); + } +}//setdih + +/* + function sshLinear: sets M and h[M] for the linear spline model + + In: T + Out: M=2, h[2][T] filled out for linear spline model. + + No return value. h is allocated anew and must be freed by caller. +*/ +void sshLinear(int T, int& M, double** &h, MList* mlist) +{ + M=2; Allocate2L(double, M, T, h, mlist); + for (int t=0; t<T; t++) h[0][t]=1, h[1][t]=t; +}//sshLinear + +/* + function sdihLinear: sets dih[M] for the linear spline model. For testing only. + + In: T + Out: dih[2][T] filled out for linear spline model. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void sdihLinear(int T, double**& dih, MList* mlist) +{ + Allocate2L(double, 2, T, dih, mlist); + for (int t=0; t<T; t++) dih[0][t]=1, dih[1][t]=t+0.5; +}//sdihLinear + +/* + function sshCubic: sets M and h[M] for cubic spline models. + + In: T + Out: M=4 and h[M] filled out for cubic spline models, including cubic and cubic-Hermite. + + No return value. h is allocated anew and must be freed by caller. +*/ +void sshCubic(int T, int& M, double** &h, MList* mlist) +{ + M=4; Allocate2L(double, M, T, h, mlist); + 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; +}//sshCubic + +/* + function sdihCubic: sets dih[M] for cubic spline models. + + In: T + Out: dih[4] filled out for cubic spline models. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void sdihCubic(int T, double** &dih, MList* mlist) +{ + Allocate2L(double, 4, T, dih, mlist); + for (int t=0; t<T; t++) + { + 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; + } +}//sdihCubic*/ + +/* + function ssALinearSpline: sets N and A[L] for the linear spline model + + In: L, M, T + Out: N=L+1, A[L][M][N] filled out for the linear spline model + + No return value. A is created anew and bust be freed by caller. +*/ +void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=L+1; + Allocate3L(double, L, M, N, A, mlist); + memset(A[0][0], 0, sizeof(double)*L*M*N); + 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; +}//ssALinearSpline + +/* + function ssLinearSpline: sets M, N, h and A for the linear spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the linear spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssALinearSpline(L, T, M, N, A, mlist); +}//ssLinearSpline + +/* + function ssACubicHermite: sets N and A[L] for cubic Hermite spline model + + In: L, M, T + Out: N=2(L+1), A[L][M][N] filled out for the cubic Hermite spline + + No return value. A is created anew and must be freed by caller. +*/ +void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=2*(L+1); + Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N); + double iT=1.0/T, iT2=iT*iT, iT3=iT2*iT; + for (int l=0; l<L; l++) + { + 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; + 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; + A[l][1][2*l+1]=1; + A[l][0][2*l]=1; + } +}//ssACubicHermite + +/* + function ssLinearSpline: sets M, N, h and A for the cubic Hermite spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the cubic Hermite spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssACubicHermite(L, T, M, N, A, mlist); +}//ssCubicHermite + +/* + function ssACubicSpline: sets N and A[L] for cubic spline model + + In: L, M, T + mode: boundary mode of cubic spline, 0=natural, 1=quadratic run-out, 2=cubic run-out + Out: N=2(L+1), A[L][M][N] filled out for the cubic spline + + No return value. A is created anew and must be freed by caller. +*/ +void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=L+1; + Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N); + Alloc2(L+1, L+1, ML); memset(ML[0], 0, sizeof(double)*(L+1)*(L+1)); + Alloc2(L+1, L+1, MR); memset(MR[0], 0, sizeof(double)*(L+1)*(L+1)); + //fill in ML and MR. The only difference between various cubic splines are ML. + double _6iT2=6.0/(T*T); + ML[0][0]=ML[L][L]=1; + for (int l=1; l<L; l++) ML[l][l-1]=ML[l][l+1]=1, ML[l][l]=4, + MR[l][l-1]=MR[l][l+1]=_6iT2, MR[l][l]=-2*_6iT2; + if (mode==0){} //no more coefficients are needed for natural cubic spline + else if (mode==1) ML[0][1]=ML[L][L-1]=-1; //setting for quadratic run-out + 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 + GICP(L+1, ML); + double** MM=MultiplyXY(L+1, ML, ML, MR); + double iT=1.0/T; + 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; + for (int l=0; l<L; l++) + { + MultiplyXY(4, 2, N, A[l], M42, &MM[l]); + A[l][1][l]-=iT; A[l][1][l+1]+=iT; A[l][0][l]+=1; + } + DeAlloc2(ML); DeAlloc2(MR); DeAlloc2(M42); +}//ssACubicSpline + +/* + function ssLinearSpline: sets M, N, h and A for the cubic spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the cubic spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssACubicSpline(L, T, M, N, A, mlist, mode); +}//ssCubicSpline + +/* + function setu: sets u[I+1] as base-band windowed Fourier atoms, whose frequencies come in the order of + 0, 1, -1, 2, -2, 3, -3, 4, etc, in bins. + + In: I, Wid: number and size of atoms to generate. + WinOrder: order (=vanishing moment) of window function to use (2=Hann, 4=Hann^2, etc.) + Out: u[I+1][Wid], du[I+1]{Wid]: the I+1 atoms and their derivatives. + + No return value. u and du are created anew and must be freed by caller. +*/ +void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder, MList* mlist) +{ + Allocate2L(cdouble, I+1, Wid, u, mlist); + Allocate2L(cdouble, I+1, Wid, du, mlist); + + double** wins=CosineWindows(WinOrder, Wid, (double**)0, 2); + double omg=2*M_PI/Wid; cdouble jomg=cdouble(0, omg); + for (int t=0; t<Wid; t++) + { + u[0][t]=wins[0][t], du[0][t]=wins[1][t]; + int li=1; + for (int i=1; i<=I; i++) + { + cdouble rot=polar(1.0, li*omg*t); + u[i][t]=u[0][t]*rot; du[i][t]=du[0][t]*rot+jomg*li*u[i][t]; + li=-li; if (li>0) li++; + } + } + DeAlloc2(wins); +}//setu + +/* + function DerivativePiecewiseI: wrapper for DerivativePiecewise(), doing the initialization ,etc. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssmode: pointer to a function that fills A[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: aita[N]: independent coefficients, where N is specified by SpecifyA. + + No return vlue. +*/ +void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + cdouble **u, **du; + setu(I, 2*T, u, du, WinOrder, mlist); + + int N; double **h, ***A; + seth(M, T, h, mlist); + SpecifyA(L, T, M, N, A, mlist, ssmode); + + DerivativePiecewise(N, aita, L, f, T, s, A, M, h, I, u, du, endmode, ds); + delete mlist; +}//DerivativePiecewiseI + +/* + function DerivativePiecewiseII: wrapper for DerivativePiecewise2(), doing the initialization ,etc. + This models the derivative of log ampltiude and frequency as separate piecewise polynomials, the first + specified by SpecifyA, the second by SpecifyB. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it + SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB. + + No reutrn value. +*/ +void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, + void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + cdouble **u, **du; + setu(I, 2*T, u, du, WinOrder, mlist); + + int Np, Nq; + double **h, ***A, ***B; + seth(M, T, h, mlist); + SpecifyA(L, T, M, Np, A, mlist, ssAmode); + SpecifyB(L, T, M, Nq, B, mlist, ssBmode); + + DerivativePiecewise2(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds); + + delete mlist; +}//DerivativePiecewiseII + +/* + function DerivativePiecewiseIII: wrapper for DerivativePiecewise3(), doing the initialization ,etc. + Notice that this time the log amplitude, rather than its derivative, is modeled as a piecewise + polynomial specified by SpecifyA. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it + SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB. + + No reutrn value. +*/ +void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, + void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + int Np, Nq; + double **h, ***A, ***B, **dh=0; + cdouble **u, **du; + setu(I, T*2, u, du, WinOrder, mlist); + seth(M, T, h, mlist); + if (ds) setdh(M, T, dh, mlist); + SpecifyA(L, T, M, Np, A, mlist, ssAmode); + SpecifyB(L, T, M, Nq, B, mlist, ssBmode); + Alloc2L(M, M, DM, mlist); + memset(DM[0], 0, sizeof(double)*M*M); for (int m=0; m<M-1; m++) DM[m][m+1]=m+1; + double** DA=0; + + for (int l=0; l<L; l++) + { + DA=MultiplyXY(M, M, Np, DA, DM, A[l], mlist); + Copy(M, Np, A[l], DA); + } + + DerivativePiecewise3(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds, dh); + + delete mlist; +}//DerivativePiecewiseIII + +/* + function AmpPhCorrectionExpA: model-preserving amplitude and phase correction in piecewise derivative + method. + + In: aita[N]: inital independent coefficients + L, T: number and size of pieces + sre[LT]: waveform data + h[M][T], dih[M][T]: piecewise basis functions and their difference-integrals + A[L][M][N]: L coefficient mapping matrices + SpecifyA: pointer to the function used for constructing A + WinOrder: order(=vanishing moment) of window used for constructing test functions + Out: aita[N]: corrected independent coefficients + s2[LT]: reconstruct sinusoid BEFORE correction + + Returns the estimate of phase angle at 0. +*/ +double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder) +{ + MList* mlist=new MList; + //*amplitude and phase correction + //amplitude is done by updating p, i.e. Re(aita) + double *s2ph=new double[L+1]; mlist->Add(s2ph, 1); + double *phcor=new double[L+1]; mlist->Add(phcor, 1); + cdouble* lamda=new cdouble[M]; mlist->Add(lamda, 1); + double* lamdax=new double[M]; mlist->Add(lamdax, 1); + double* lamday=new double[M]; mlist->Add(lamday, 1); + { + double tmpph=0; + memset(s2ph, 0, sizeof(double)*(L+1)); + s2ph[0]=tmpph; + for (int l=0; l<L; l++) + { + MultiplyXy(M, N, lamda, A[l], aita); for (int m=0; m<M; m++) lamdax[m]=lamda[m].x, lamday[m]=lamda[m].y; + SinusoidExpA(T, &s2[l*T], M, lamdax, lamday, h, dih, tmpph); s2ph[l+1]=tmpph; + } + double* win=new double[2*T+1]; CosineWindows(WinOrder, 2*T, &win, 1); mlist->Add(win, 1); + for (int l=1; l<L; l++) + { + 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]); + cdouble loginn=log(inn); + if (SpecifyA==ssACubicHermite) + { + aita[l*2]+=loginn.x; + s2ph[l]+=loginn.y; + phcor[l]=loginn.y; + if (l==1) aita[0]+=loginn.x, phcor[0]=loginn.y, s2ph[0]+=loginn.y; + if (l==L-1) aita[L*2]+=loginn.x, phcor[L]=loginn.y, s2ph[L]+=loginn.y; + } + else + { + aita[l]+=loginn.x; + s2ph[l]+=loginn.y; + phcor[l]=loginn.y; + if (l==1) + { + inn=Inner(T, sre, &win[T], s2)/Inner(T, s2, &win[T], s2); + loginn=log(inn); + aita[0]+=loginn.x; + s2ph[0]+=loginn.y; + phcor[0]=loginn.y; + } + if (l==L-1) + { + inn=Inner(T, &sre[L*T-T], win, &s2[L*T-T])/Inner(T, &s2[L*T-T], win, &s2[L*T-T]); + loginn=log(inn); + aita[L]+=loginn.x; + s2ph[L]+=loginn.y; + phcor[L]=loginn.y; + } + } + } + + for (int l=1; l<=L; l++) + { + int k=floor((phcor[l]-phcor[l-1])/(2*M_PI)+0.5); + if (k!=0) + phcor[l]+=2*M_PI*k; + } + //* + //now phcor[] contains phase corrector to be interpolated + 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)); + mlist->Add(b, 1); mlist->Add(zet, 1); mlist->Add(dzet, 1); + double ihT[]={T, T/2.0*T, T/3.0*T*T, T/4.0*T*T*T}; + + Alloc2L(L, N, BB, mlist); + //prepare linear system (BB)(zet)=(b) + for (int l=0; l<L; l++) + { + MultiplyxY(N, 4, BB[l], ihT, A[l]); + b[l]=phcor[l+1]-phcor[l]; + } + Alloc2L(L, L, copyA, mlist); + if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L); + 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]; + double* copyb=Copy(L, b, mlist); + zet[0]=0; GECP(L, &zet[1], copyA, copyb); + if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L); + 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]; + Copy(L, copyb, b); for (int l=0; l<L; l++) copyb[l]-=BB[l][0]; + dzet[0]=1; GECP(L, &dzet[1], copyA, copyb); + +#ifdef ERROR_CHECK + //Test that (BB)(zet)=b and (BB)(dzet)=b + double* bbzet=MultiplyXy(L, L+1, BB, zet, mlist); + MultiAdd(L, bbzet, bbzet, b, -1); + double err1=Inner(L, bbzet, bbzet); + double* bbdzet=MultiplyXy(L, L+1, BB, dzet, mlist); + MultiAdd(L, bbdzet, bbdzet, b, -1); + double err2=Inner(L, bbdzet, bbdzet); + MultiAdd(L+1, dzet, dzet, zet, -1); + //Test that (BB)dzet=0 + MultiplyXy(L, L+1, bbdzet, BB, dzet); + double err3=Inner(L, bbzet, bbzet); +#endif + //now that (zet)+(miu)(dzet) is the general solution to (BB)(zet)=b, + // we look for (miu) that maximizes smoothness + + double innuv=0, innvv=0, lmd0[4], lmdd[4], clmdd[4], + T2=T*T, T3=T2*T, T4=T3*T, T5=T4*T; + for (int l=0; l<L; l++) + { + MultiplyXy(4, L+1, lmd0, A[l], zet); + MultiplyXy(4, L+1, lmdd, A[l], dzet); + clmdd[1]=T*lmdd[1]+T2*lmdd[2]+T3*lmdd[3]; + clmdd[2]=T2*lmdd[1]+(4.0/3)*T3*lmdd[2]+1.5*T4*lmdd[3]; + clmdd[3]=T3*lmdd[1]+1.5*T4*lmdd[2]+1.8*T5*lmdd[3]; + innuv+=Inner(3, &lmd0[1], &clmdd[1]); + innvv+=Inner(3, &lmdd[1], &clmdd[1]); + } + MultiAdd(L+1, zet, zet, dzet, -innuv/innvv); + + if (SpecifyA==ssACubicHermite) + for (int l=0; l<=L; l++) aita[2*l].y+=zet[l]; + else + for (int l=0; l<=L; l++) aita[l].y+=zet[l]; + //*/ + } + double result=s2ph[0]; + delete mlist; + return result; +}//AmpPhCorrectionExpA