st->Pe = (1-alpha)*st->Pe + alpha*Pe; st->Py = (1-alpha)*st->Py + alpha*Py; if (st->Pey < Nscale*st->Pe*st->Py) st->Pey = Nscale*st->Pe*st->Py; if (st->Pyy < Nscale*st->Py*st->Py) st->Pyy = Nscale*st->Py*st->Py; leak_estimate = (st->Pey - Nscale*st->Pe*st->Py) / max(1.f,st->Pyy - Nscale*st->Py*st->Py);
if (st->Pey<0) st->Pey = 0; leak_estimate = st->Pey / (1+st->Pyy);
void speex_echo_cancel(SpeexEchoState *st, short *ref, short *echo, short *out, float *Yout){   int i,j,m;   int N,M;   float scale;   float Syy=0,Sey=0,See=0;   float leak_estimate;            N = st->window_size;   M = st->M;   scale = 1.0f/N;   st->cancel_count++;   /* Copy input data to buffer */   for (i=0;i<st->frame_size;i++)   {      st->x[i] = st->x[i+st->frame_size];      st->x[i+st->frame_size] = echo[i];      st->d[i] = st->d[i+st->frame_size];      st->d[i+st->frame_size] = ref[i];   }   /* Shift memory: this could be optimized eventually*/   for (i=0;i<N*(M-1);i++)      st->X[i]=st->X[i+N];   /* Copy new echo frame */   for (i=0;i<N;i++)      st->X[(M-1)*N+i]=st->x[i];   /* Convert x (echo input) to frequency domain */   spx_drft_forward(st->fft_lookup, &st->X[(M-1)*N]);   /* Compute filter response Y */   for (i=0;i<N;i++)      st->Y[i] = 0;   for (j=0;j<M;j++)      spectral_mul_accum(&st->X[j*N], &st->W[j*N], st->Y, N);      /* Convert Y (filter response) to time domain */   for (i=0;i<N;i++)      st->y[i] = st->Y[i];   spx_drft_backward(st->fft_lookup, st->y);   for (i=0;i<N;i++)      st->y[i] *= scale;   /* Transform d (reference signal) to frequency domain */   for (i=0;i<N;i++)      st->D[i]=st->d[i];   spx_drft_forward(st->fft_lookup, st->D);   /* Compute error signal (signal with echo removed) */    for (i=0;i<st->frame_size;i++)   {      float tmp_out;      tmp_out = (float)ref[i] - st->y[i+st->frame_size];            st->E[i] = 0;      st->E[i+st->frame_size] = tmp_out;            /* Saturation */      if (tmp_out>32767)         tmp_out = 32767;      else if (tmp_out<-32768)         tmp_out = -32768;      out[i] = tmp_out;   }      /* Compute a bunch of correlations */   Sey = inner_prod(st->y+st->frame_size, st->E+st->frame_size, st->frame_size);   See = inner_prod(st->E+st->frame_size, st->E+st->frame_size, st->frame_size);   Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);      /* Convert error to frequency domain */   spx_drft_forward(st->fft_lookup, st->E);      /* Compute power spectrum of error (E) and filter response (Y) */   power_spectrum(st->E, st->Rf, N);   power_spectrum(st->Y, st->Yf, N);   {      float Pey = 0, Pyy=0, Pe=0,Py=0;      float Nscale;      float alpha;      Nscale = 1./(st->frame_size+1);      for (j=0;j<=st->frame_size;j++)      {         float E, Y;         E = (st->Rf[j]);         Y = (st->Yf[j]);         Pey += E*Y;         Pyy += Y*Y;         Pe += E;         Py += Y;      }      alpha = .1*Syy / (1+See);      if (alpha > .1)         alpha = .1;      st->Pey = (1-alpha)*st->Pey + alpha*Pey;      st->Pyy = (1-alpha)*st->Pyy + alpha*Pyy;      st->Pe = (1-alpha)*st->Pe + alpha*Pe;      st->Py = (1-alpha)*st->Py + alpha*Py;      if (st->Pey < Nscale*st->Pe*st->Py)         st->Pey = Nscale*st->Pe*st->Py;      if (st->Pyy < Nscale*st->Py*st->Py)         st->Pyy = Nscale*st->Py*st->Py;      leak_estimate = (st->Pey - Nscale*st->Pe*st->Py) / max(1.f,st->Pyy - Nscale*st->Py*st->Py);      if (leak_estimate > 1)         leak_estimate = 1;      /*printf ("%f\n", leak_estimate);*/   }      /* Compute frequency-domain adaptation mask */   for (j=0;j<=st->frame_size;j++)   {      float r;      r = leak_estimate*st->Yf[j] / (1+st->Rf[j]);      if (r>1)         r = 1;      st->fratio[j] = r;   }   /* Compute smoothed cross-correlation and energy */      st->Sey = .98*st->Sey + .02*Sey;   st->Syy = .98*st->Syy + .02*Syy;   st->See = .98*st->See + .02*See;      /* Check if filter is completely mis-adapted (if so, reset filter) */   if (st->Sey/(1+st->Syy + .01*st->See) < -.9)   {      /*fprintf (stderr, "reset at %d\n", st->cancel_count);*/      speex_echo_state_reset(st);      return;   }      /* Update frequency-dependent energy ratio with the total energy ratio */   for (i=0;i<=st->frame_size;i++)   {      st->fratio[i]  = min(1.f,st->fratio[i]);      /*printf ("%f ", st->fratio[i]);*/   }   printf ("\n");   if (st->adapted)   {      st->adapt_rate = 1.f/M;   } else {      /* Temporary adaption rate if filter is not adapted correctly */      float ESR;      float SER;      float Srr, Sxx;            Srr = inner_prod(st->d+st->frame_size, st->d+st->frame_size, st->frame_size);      Sxx = inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);      SER = Srr / (1+Sxx);      ESR = leak_estimate*Syy / (1+See);      if (ESR>1)         ESR = 1;         /* We consider that the filter is adapted if the following is true*/      if (ESR>.05 && st->sum_adapt > .1 && !st->adapted)      {         /*fprintf(stderr, "Adapted at %d %f\n", st->cancel_count, st->sum_adapt);*/         st->adapted = 1;      }      if (SER<.1)         st->adapt_rate =.5/(2+M);      else if (SER<1)         st->adapt_rate =.3/(2+M);      else if (SER<10)         st->adapt_rate =.2/(2+M);      else if (SER<30)         st->adapt_rate =.08/(2+M);      else         st->adapt_rate = 0;      /* How much have we adapted so far? */      st->sum_adapt = (1-ESR*st->adapt_rate)*st->sum_adapt + ESR*st->adapt_rate;   }      /* Compute echo power in each frequency bin */   {      float ss = 1.0f/st->cancel_count;      if (ss < .4/M)         ss=.4/M;      power_spectrum(&st->X[(M-1)*N], st->Xf, N);      /* Smooth echo energy estimate over time */      for (j=0;j<=st->frame_size;j++)         st->power[j] = (1-ss)*st->power[j] + ss*st->Xf[j];                  /* Combine adaptation rate to the the inverse energy estimate */      if (st->adapted)      {         /* If filter is adapted, include the frequency-dependent ratio too */         for (i=0;i<=st->frame_size;i++)            st->power_1[i] = st->adapt_rate*st->fratio[i] /(1.f+st->power[i]);      } else {         for (i=0;i<=st->frame_size;i++)            st->power_1[i] = st->adapt_rate/(1.f+st->power[i]);      }   }      /* Compute weight gradient */   for (j=0;j<M;j++)   {      weighted_spectral_mul_conj(st->power_1, &st->X[j*N], st->E, st->PHI+N*j, N);   }   /* Gradient descent */   for (i=0;i<M*N;i++)      st->W[i] += st->PHI[i];      /* AUMDF weight constraint */   for (j=0;j<M;j++)   {      /* Remove the "if" to make this an MDF filter */      if (j==M-1 || st->cancel_count%(M-1) == j)      {         spx_drft_backward(st->fft_lookup, &st->W[j*N]);         for (i=0;i<N;i++)            st->W[j*N+i]*=scale;         for (i=st->frame_size;i<N;i++)         {            st->W[j*N+i]=0;         }         spx_drft_forward(st->fft_lookup, &st->W[j*N]);      }   }   /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/   if (Yout)   {      if (st->adapted)      {         /* If the filter is adapted, take the filtered echo */         for (i=0;i<st->frame_size;i++)            st->last_y[i] = st->last_y[st->frame_size+i];         for (i=0;i<st->frame_size;i++)            st->last_y[st->frame_size+i] = st->y[st->frame_size+i];      } else {         /* If filter isn't adapted yet, all we can do is take the echo signal directly */         for (i=0;i<N;i++)            st->last_y[i] = st->x[i];      }            /* Apply hanning window (should pre-compute it)*/      for (i=0;i<N;i++)         st->Yps[i] = (.5-.5*cos(2*M_PI*i/N))*st->last_y[i];            /* Compute power spectrum of the echo */      spx_drft_forward(st->fft_lookup, st->Yps);      power_spectrum(st->Yps, st->Yps, N);            /* Estimate residual echo */      for (i=0;i<=st->frame_size;i++)         Yout[i] = leak_estimate*st->Yps[i];   }}