view SinEst.h @ 1:6422640a802f

first upload

#ifndef SinEstH
#define SinEstH

/*
  SinEst.cpp - sinusoid estimation algorithms
*/


#include <mem.h>
#include "xcomplex.h"
#include "arrayalloc.h"
#include "matrix.h"
#ifdef I
#undef I
#endif

//--since function derivative------------------------------------------------
double ddsincd_unn(double x, int N);
double dsincd_unn(double x, int N);

//--window spectrum and derivatives------------------------------------------
cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2);
void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2);
void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2);

//--spectral projection routines---------------------------------------------
cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2);

double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude);
double IPWindow(double f, void* params);
double ddIPWindow(double f, void* params);
double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow);

double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* ej2ph=0);
double sIPWindow(double f, void* params);
double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip);
double dsIPWindow(double f, void* params);
double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip);
double ddsIPWindow(double f, void* params);
double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn=0);

double sIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* ej2ph=0);
double sIPWindowC(double f, void* params);
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);
double dsIPWindowC(double f, void* params);
double ddsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip);
double ddsIPWindowC(double f, void* params);

//--least-square sinusoid estimation routines--------------------------------
double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf=1e-6);
void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf=1e-6);
double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf);
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);
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);

//--multi-sinusoid spectral projection routines------------------------------
void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps=0);
void IPMulti(int I, double* f, cdouble* lmd, cfloat* x, int Wid, int K1, int K2, int M, double* c, double eps=0, double* sens=0, double* r1=0);
void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps=0);
double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B);
double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B);
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=0, double** Q=0);
cdouble* IPMulti(int I, int J, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, cdouble** wt=0, cdouble** Q=0, double** L=0, MList* RetList=0);

//--dual-sinusoid spectral projection routines-------------------------------
double WindowDuo(double df, int N, double* d, int M, cdouble* w);
double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w);
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);
double sIPWindowDuo(double f2, void* params);
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);
double ddsIPWindowDuo(double f2, void* params);
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);

//--time-frequency reassignment----------------------------------------------
void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope=0);
void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win=0);

//--additive and multiplicative reestimation routines------------------------
typedef double (*TBasicAnalyzer)(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, int reserved, bool LogA);
void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false);
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=false);
void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false);
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=false);
void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst);

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=0);
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);
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=0);
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=0);

//--local derivative algorithms - DAFx09-------------------------------------
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);
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=false);
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=false);

//--the Abe-Smith estimator--------------------------------------------------
void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res=1);
void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res=1);
void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res=1);

//--piecewise derivative algorithms and tools--------------------------------
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=0, cdouble* ds=0);
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=0, cdouble* ds=0);
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=0, cdouble* ds=0, double** dh=0);
void seth(int M, int T, double**& h, MList* mlist);
void setdh(int M, int T, double**& dh, MList* mlist);
void setdih(int M, int T, double**& dih, MList* mlist);
void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder=2, MList* mlist=0);
void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0);
void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0);
void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0);
void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode=0);
void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode=0);
void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode=0);
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=0,	int WinOrder=2, int I=2, int endmode=0, cdouble* ds=0);
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=2, int I=2, int endmode=0, cdouble* ds=0);
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=2, int I=2, int endmode=0, cdouble* ds=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);

//--local derivative algorithms - general------------------------------------
/*
  template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "v" version,
  i.e. using tuned test functions.

  In: s[Wid]: waveform data
      v[I][Wid], dv[I][Wid]: test functions and their derivatives
      h[M+1][Wid]: basis functions
      p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero.
  Out: lmd[1:M]: coefficients of h[1:M].

  Returns inner product of s and v[0].
*/
template<class Ts>cdouble DerivativeLSv(int Wid, Ts* s, int I, cdouble** v, cdouble** dv, int M, double **h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0)
{
  int Kr=M*2-p0s-q0s; //number of real unknowns apart from p0 and q0
  if (I<ceil(Kr/2.0)) throw("insufficient test functions"); //Kr/2 complex equations are needed to solve the unknowns

  //ind maps the real unknowns to their positions in physical buffer
  //uind maps them back
  int *uind=new int[Kr], *ind=new int[2*M];
  memset(ind, 0, sizeof(int)*2*M);
  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*M){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;}
    if (up!=Kr) throw("");
  }

  cdouble* sv1=new cdouble[I];
  for (int i=0; i<I; i++) sv1[i]=-Inner(Wid, s, dv[i]);

  double** Allocate2(double, 2*I, Kr, A);
  for (int m=1; m<=M; m++)
    for (int i=0; i<I; i++)
    {
      int lind;
      cdouble shv=Inner(Wid, s, h[m], v[i]);
      if ((lind=ind[2*(m-1)])>=0)
      {
        A[2*i][lind]=shv.x;
        A[2*i+1][lind]=shv.y;
      }
      if ((lind=ind[2*m-1])>=0)
      {
        A[2*i][lind]=-shv.y;
        A[2*i+1][lind]=shv.x;
      }
    }

  double* pq=new double[Kr];
  if (2*I==Kr) GECP(Kr, pq, A, (double*)sv1);
  else LSLinear(2*I, Kr, pq, A, (double*)sv1);

  memset(lmd, 0, sizeof(double)*(M+1)*2);
  for (int k=0; k<Kr; k++) ((double*)(&lmd[1]))[uind[k]]=pq[k];

  cdouble result=Inner(Wid, s, v[0]);
  delete[] pq;
  delete[] sv1;
  delete[] uind;
  delete[] ind;
  DeAlloc2(A);
  return result;
}//DerivativeLSv

/*
  template DerivativeLS: local derivative algorithm for estimating time-varying sinusoids, "u" version,
  i.e. using base-band test functions.

  In: s[Wid]: waveform data
      u[I][Wid], du[I][Wid]: base-band test functions and their derivatives
      omg: angular frequency onto which u[I] and du[I] are modulated to give the test functions
      h[M+1][Wid]: basis functions
      p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero.
  Out: lmd[1:M]: coefficients of h[1:M].

  Returns inner product of s and v[0].
*/
template<class Ts, class Tu>cdouble DerivativeLS(int Wid, Ts* s, int I, double omg, Tu** u, Tu** du, int M, double **h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0)
{
  cdouble** Allocate2(cdouble, I, Wid, v);
  cdouble** Allocate2(cdouble, I, Wid, dv);
  cdouble jomg=cdouble(0, omg); int hWid=Wid/2;
  for (int c=0; c<Wid; c++)
  {
    double t=c-hWid;
    cdouble rot=cdouble(1).rotate(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];
  }
  cdouble result=DerivativeLSv(Wid, s, I, v, dv, M, h, lmd, p0s, p0, q0s, q0);
  DeAlloc2(v); DeAlloc2(dv);
  return result;
}//DerivativeLS

/*
  template DerivativeLS_AmpPh: amplitude and phase estimation in the local derivative algorithm, "u"
  version

  In: sv0: inner product of signal s[Wid] and test function v0
      u0[Wid], omg: base-band test function and carrier frequency used for computing v0[]
      integr_h[M+1][Wid]: integrals of basis functions

  Returns coefficient to integr_h[0]=1.
*/
template<class Tu>cdouble DerivativeLS_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, double omg, Tu* u0, cdouble sv0)
{
  cdouble e0=0; double hWid=Wid/2.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];
    if (expo.x>300) expo.x=300;
    else if (expo.x<-300) expo.x=-300;
    e0+=exp(expo)**(cdouble(u0[n]).rotate(omg*(n-hWid)));
  }
  return log(sv0/e0);
}//DerivativeLS_AmpPh

/*
  template DerivativeLS_AmpPh: amplitude and phase estimation in the local derivative algorithm, "u"
  version.

  In: s[Wid]: waveform data
      u0[Wid], omg: base-band test function and carrier frequency used for computing v0[]
      integr_h[M+1][Wid]: integrals of basis functions

  Returns coefficient to integr_h[0]=1.
*/
template<class Tu, class Ts>cdouble DerivativeLS_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, double omg, Tu* u0, Ts* s)
{
  cdouble ss0=0, e0=0; double hWid=Wid/2.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];
    if (expo.x>300) expo.x=300;
    else if (expo.x<-300) expo.x=-300;
    e0+=~exp(expo)*abs(u0[n]);
    ss0+=s[n]**exp(expo)*abs(u0[n]);
  }
  return log(ss0/e0);
}//DerivativeLS_AmpPh

cdouble DerivativeLSv_AmpPh(int, int, double**, cdouble*, cdouble*, cdouble); //the "v" version is implemented as a normal function in SinEst.cpp.

/*
  template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "v" version.

  In: s[Wid]: waveform data
      v[I][Wid], dv[I][Wid]: test functions and their derivatives
      h[M+1][Wid], integr_h[M+1][Wid]: basis functions and their integrals
      p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero.
  Out: lmd[M+1]: coefficients of h[M+1], including lmd[0].

  No return value.
*/
template<class Ts> void DerivativeLSv(int Wid, Ts* s, int I, cdouble** v, cdouble** dv, int M, double **h, double **integr_h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0)
{
  cdouble sv0=DerivativeLSv(Wid, s, I, v, dv, M, h, lmd, p0s, p0, q0s, q0);
  lmd[0]=DerivativeLSv_AmpPh(Wid, M, integr_h, lmd, v[0], sv0);
}//DerivativeLSv_AmpPh

/*template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "u" version.

  In: s[Wid]: waveform data
      u[I][Wid], du[I][Wid]: base-band test functions and their derivatives
      omg: angular frequency onto which u[I] and du[I] are modulated to give the test functions
      h[M+1][Wid], integr_h[M+1][Wid]: basis functions and their integrals
      p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero.
  Out: lmd[M+1]: coefficients of h[M+1], including lmd[0].

  No return value.
*/
template<class Ts, class Tu>void DerivativeLS(int Wid, Ts* s, int I, double omg, Tu** u, Tu** du, int M, double **h, double **integr_h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0)
{
  cdouble sv0=DerivativeLS(Wid, s, I, omg, u, du, M, h, lmd, p0s, p0, q0s, q0);
  lmd[0]=DerivativeLS_AmpPh(Wid, M, integr_h, lmd, omg, u[0], s); //sv0);
}//DerivativeLSv

/*
  template CosineWindows: generates the Hann^(K/2) window and its L-1 derivatives as Result[L][Wid+1]

  In: K, L, Wid
  Out: w[L][Wid+1]: Hann^(K/2) window function and its derivatives up to order L-1

  Returns pointer to w. w is created anew if w=0 is specified on start.
*/
template<class T>T** CosineWindows(int K, int Wid, T **w, int L=0)
{
  if (L<=0) L=K;
  if (!w) {Allocate2(T, L, Wid+1, w);}
  memset(w[0], 0, sizeof(T)*L*(Wid+1));
  int hWid=Wid/2, dWid=Wid*2;
  double *s=new double[dWid+hWid], *c=&s[hWid]; //s[n]=sin(pi*n/N), n=0, ..., 2N-1
  double *C=new double[K+2], *lK=&C[K/2+1], piC=M_PI/Wid;
  //C[i]=C(K, i)(-1)^i*2^(-K+1), the combination number, i=0, ..., K/2
  //ik[i]=(K-2i)^k*(M_PI/Wid)^k, i=0, ..., K/2
  //calculate C(K,i)(-1)^i*2^(-K+1)
  C[0]=1.0/(1<<(K-1)); double lC=C[0]; for (int i=1; i+i<=K; i++){lC=lC*(K-i+1)/i; C[i]=(i%2)?(-lC):lC;}
  //calculate sin/cos functions
  for (int n=0; n<dWid; n++) s[n]=sin(n*piC); memcpy(&s[dWid], s, sizeof(double)*hWid);
  for (int k=0; k<L; k++)
  {
    if (k==0) for (int i=0; i+i<K; i++) lK[i]=C[i];
    else for (int i=0; i+i<K; i++) lK[i]*=(K-2*i)*piC;

    if ((K-k)%2) //K-k is odd
    {
      for (int i=0; i+i<K; i++)	for (int n=0; n<=Wid; n++) w[k][n]+=lK[i]*s[(K-2*i)*n%dWid];
      if ((K-k-1)/2%2) for (int n=0; n<=Wid; n++) w[k][n]=-w[k][n];
    }
    else
    {
      for (int i=0; i+i<K; i++) for (int n=0; n<=Wid; n++) w[k][n]+=lK[i]*c[(K-2*i)*n%dWid];
      if ((K-k)/2%2) for (int n=0; n<=Wid; n++) w[k][n]=-w[k][n];
    }
  }
  if (K%2==0){double tmp=C[K/2]*0.5; if (K/2%2) tmp=-tmp; for (int n=0; n<=Wid; n++) w[0][n]+=tmp;}
  delete[] s; delete[] C;
  return w;
}//CosineWindows


#endif