mdf.c

wince下著名的视频播放器源码

   /* 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;
   }
   
   /* This bit of code is optional and provides faster adaptation by doing a projection 
      of the previous gradient on the "MMSE surface" */
   if (1)
   {
      float Sge, Sgg, Syy;
      float gain;
      Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
      for (i=0;i<N;i++)
         st->Y2[i] = 0;
      for (j=0;j<M;j++)
         spectral_mul_accum(&st->X[j*N], &st->PHI[j*N], st->Y2, N);
      for (i=0;i<N;i++)
         st->y2[i] = st->Y2[i];
      spx_drft_backward(st->fft_lookup, st->y2);
      for (i=0;i<N;i++)
         st->y2[i] *= scale;
      Sge = inner_prod(st->y2+st->frame_size, st->E+st->frame_size, st->frame_size);
      Sgg = inner_prod(st->y2+st->frame_size, st->y2+st->frame_size, st->frame_size);
      /* Compute projection gain */
      gain = Sge/(N+.03*Syy+Sgg);
      if (gain>2)
         gain = 2;
      if (gain < -2)
         gain = -2;
      
      /* Apply gain to weights, echo estimates, output */
      for (i=0;i<N;i++)
         st->Y[i] += gain*st->Y2[i];
      for (i=0;i<st->frame_size;i++)
      {
         st->y[i+st->frame_size] += gain*st->y2[i+st->frame_size];
         st->E[i+st->frame_size] -= gain*st->y2[i+st->frame_size];
      }
      for (i=0;i<M*N;i++)
         st->W[i] += gain*st->PHI[i];
   }

   /* Compute power spectrum of output (D-Y) and filter response (Y) */
   for (i=0;i<N;i++)
      st->D[i] -= st->Y[i];
   power_spectrum(st->D, st->Rf, N);
   power_spectrum(st->Y, st->Yf, N);
   
   /* 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 a bunch of correlations */
   Sry = inner_prod(st->y+st->frame_size, st->d+st->frame_size, st->frame_size);
   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);
   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);

   /* 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) < -1)
   {
      /*fprintf (stderr, "reset at %d\n", st->cancel_count);*/
      speex_echo_state_reset(st);
      return;
   }

   SER = Srr / (1+Sxx);
   ESR = leak_estimate*Syy / (1+See);
   if (ESR>1)
      ESR = 1;
#if 1
   /* If over-cancellation (creating echo with 180 phase) damp filter */
   if (st->Sey/(1+st->Syy) < -.1 && (ESR > .3))
   {
      for (i=0;i<M*N;i++)
         st->W[i] *= .95;
      st->Sey *= .5;
      st->sum_adapt*= .95;
      /*fprintf (stderr, "corrected down\n");*/
   }
#endif
#if 1
   /* If under-cancellation (leaving echo with 0 phase) scale filter up */
   if (st->Sey/(1+st->Syy) > .1 && (ESR > .1 || SER < 10))
   {
      for (i=0;i<M*N;i++)
         st->W[i] *= 1.05;
      st->Sey *= .5;
      /*fprintf (stderr, "corrected up %d\n", st->cancel_count);*/
   }
#endif
   
   /* We consider that the filter is adapted if the following is true*/
   if (ESR>.6 && st->sum_adapt > .7 && !st->adapted)
   {
      /*fprintf(stderr, "Adapted at %d %f\n", st->cancel_count, st->sum_adapt);*/
      st->adapted = 1;
   } else if (st->sum_adapt < .5 && st->adapted)
   {
      /*fprintf(stderr, "Un-adapted at %d %f\n", st->cancel_count, st->sum_adapt);*/
      st->adapted = 0;
   }
   
   /* Update frequency-dependent energy ratio with the total energy ratio */
   for (i=0;i<=st->frame_size;i++)
   {
      st->fratio[i]  = (.2*ESR+.8*min(ESR,st->fratio[i]));
   }   

   if (st->adapted)
   {
      st->adapt_rate = .95f/(2+M);
      /* How much have we adapted so far? */
      st->sum_adapt = (1-st->adapt_rate)*st->sum_adapt + st->adapt_rate;
   } else {
      /* Temporary adaption rate if filter is not adapted correctly */
      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;
   }
   
   /* How much have we adapted so far? */
   /*st->sum_adapt += st->adapt_rate;*/

   /* Compute echo power in each frequency bin */
   {
      float ss = 1.0f/st->cancel_count;
      if (ss < .3/M)
         ss=.3/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]);
      }
   }

   
   /* Convert error to frequency domain */
   spx_drft_forward(st->fft_lookup, st->E);

   /* Do some regularization (prevents problems when system is ill-conditoned) */
   for (m=0;m<M;m++)
      for (i=0;i<N;i++)
         st->W[m*N+i] *= 1-st->regul[i]*ESR;
   
   /* 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 (st->cancel_count%M == 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] = 2*leak_estimate*st->Yps[i];
   }

}