libspeex/preprocess.c

   1 /* Copyright (C) 2003 Epic Games
   2    Written by Jean-Marc Valin
   3
   4    File: preprocess.c
   5    Preprocessor with denoising based on the algorithm by Ephraim and Malah
   6
   7    Redistribution and use in source and binary forms, with or without
   8    modification, are permitted provided that the following conditions are
   9    met:
  10
  11    1. Redistributions of source code must retain the above copyright notice,
  12    this list of conditions and the following disclaimer.
  13
  14    2. Redistributions in binary form must reproduce the above copyright
  15    notice, this list of conditions and the following disclaimer in the
  16    documentation and/or other materials provided with the distribution.
  17
  18    3. The name of the author may not be used to endorse or promote products
  19    derived from this software without specific prior written permission.
  20
  21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
  25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
  29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  31    POSSIBILITY OF SUCH DAMAGE.
  32 */
  33
  34 #include <math.h>
  35 #include "speex_preprocess.h"
  36 #include "misc.h"
  37 #include "smallft.h"
  38
  39 #define max(a,b) ((a) > (b) ? (a) : (b))
  40 #define min(a,b) ((a) < (b) ? (a) : (b))
  41
  42 #ifndef M_PI
  43 #define M_PI 3.14159263
  44 #endif
  45
  46 #define SQRT_M_PI_2 0.88623
  47 #define LOUDNESS_EXP 2.5
  48
  49 #define NB_BANDS 8
  50
  51 #define ZMIN .1
  52 #define ZMAX .316
  53 #define ZMIN_1 10
  54 #define LOG_MIN_MAX_1 0.86859
  55
  56 static void conj_window(float *w, int len)
  57 {
  58    int i;
  59    for (i=0;i<len;i++)
  60    {
  61       float x=4*((float)i)/len;
  62       int inv=0;
  63       if (x<1)
  64       {
  65       } else if (x<2)
  66       {
  67          x=2-x;
  68          inv=1;
  69       } else if (x<3)
  70       {
  71          x=x-2;
  72          inv=1;
  73       } else {
  74          x=4-x;
  75       }
  76       x*=1.9979;
  77       w[i]=(.5-.5*cos(x))*(.5-.5*cos(x));
  78       if (inv)
  79          w[i]=1-w[i];
  80       w[i]=sqrt(w[i]);
  81    }
  82 }
  83
  84 /* This function approximates the gain function
  85    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)
  86    which multiplied by xi/(1+xi) is the optimal gain
  87    in the loudness domain ( sqrt[amplitude] )
  88 */
  89 static float hypergeom_gain(float x)
  90 {
  91    int ind;
  92    float integer, frac;
  93    static float table[21] = {
  94       0.82157, 1.02017, 1.20461, 1.37534, 1.53363, 1.68092, 1.81865,
  95       1.94811, 2.07038, 2.18638, 2.29688, 2.40255, 2.50391, 2.60144,
  96       2.69551, 2.78647, 2.87458, 2.96015, 3.04333, 3.12431, 3.20326};
  97
  98    if (x>9.5)
  99       return 1+.12/x;
 100
 101    integer = floor(x);
 102    frac = x-integer;
 103    ind = (int)integer;
 104
 105    return ((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001);
 106 }
 107
 108 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
 109 {
 110    int i;
 111    int N, N3, N4;
 112
 113    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
 114    st->frame_size = frame_size;
 115
 116    /* Round ps_size down to the nearest power of two */
 117 #if 0
 118    i=1;
 119    st->ps_size = st->frame_size;
 120    while(1)
 121    {
 122       if (st->ps_size & ~i)
 123       {
 124          st->ps_size &= ~i;
 125          i<<=1;
 126       } else {
 127          break;
 128       }
 129    }
 130
 131
 132    if (st->ps_size < 3*st->frame_size/4)
 133       st->ps_size = st->ps_size * 3 / 2;
 134 #else
 135    st->ps_size = st->frame_size;
 136 #endif
 137
 138    N = st->ps_size;
 139    N3 = 2*N - st->frame_size;
 140    N4 = st->frame_size - N3;
 141
 142    st->sampling_rate = sampling_rate;
 143    st->denoise_enabled = 1;
 144    st->agc_enabled = 0;
 145    st->agc_level = 8000;
 146    st->vad_enabled = 0;
 147
 148    st->frame = (float*)speex_alloc(2*N*sizeof(float));
 149    st->ps = (float*)speex_alloc(N*sizeof(float));
 150    st->gain2 = (float*)speex_alloc(N*sizeof(float));
 151    st->window = (float*)speex_alloc(2*N*sizeof(float));
 152    st->noise = (float*)speex_alloc(N*sizeof(float));
 153    st->old_ps = (float*)speex_alloc(N*sizeof(float));
 154    st->gain = (float*)speex_alloc(N*sizeof(float));
 155    st->prior = (float*)speex_alloc(N*sizeof(float));
 156    st->post = (float*)speex_alloc(N*sizeof(float));
 157    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
 158    st->inbuf = (float*)speex_alloc(N3*sizeof(float));
 159    st->outbuf = (float*)speex_alloc(N3*sizeof(float));
 160    st->echo_noise = (float*)speex_alloc(N*sizeof(float));
 161
 162    st->S = (float*)speex_alloc(N*sizeof(float));
 163    st->Smin = (float*)speex_alloc(N*sizeof(float));
 164    st->Stmp = (float*)speex_alloc(N*sizeof(float));
 165    st->update_prob = (float*)speex_alloc(N*sizeof(float));
 166
 167    st->zeta = (float*)speex_alloc(N*sizeof(float));
 168    st->Zpeak = 0;
 169    st->Zlast = 0;
 170
 171    st->noise_bands = (float*)speex_alloc(NB_BANDS*sizeof(float));
 172    st->noise_bands2 = (float*)speex_alloc(NB_BANDS*sizeof(float));
 173    st->speech_bands = (float*)speex_alloc(NB_BANDS*sizeof(float));
 174    st->speech_bands2 = (float*)speex_alloc(NB_BANDS*sizeof(float));
 175    st->noise_bandsN = st->speech_bandsN = 1;
 176
 177    conj_window(st->window, 2*N3);
 178    for (i=2*N3;i<2*st->ps_size;i++)
 179       st->window[i]=1;
 180
 181    if (N4>0)
 182    {
 183       for (i=N3-1;i>=0;i--)
 184       {
 185          st->window[i+N3+N4]=st->window[i+N3];
 186          st->window[i+N3]=1;
 187       }
 188    }
 189    for (i=0;i<N;i++)
 190    {
 191       st->noise[i]=1e4;
 192       st->old_ps[i]=1e4;
 193       st->gain[i]=1;
 194       st->post[i]=1;
 195       st->prior[i]=1;
 196    }
 197
 198    for (i=0;i<N3;i++)
 199    {
 200       st->inbuf[i]=0;
 201       st->outbuf[i]=0;
 202    }
 203
 204    for (i=0;i<N;i++)
 205    {
 206       float ff=((float)i)*.5*sampling_rate/((float)N);
 207       st->loudness_weight[i] = .35-.35*ff/16000+.73*exp(-.5*(ff-3800)*(ff-3800)/9e5);
 208       if (st->loudness_weight[i]<.01)
 209          st->loudness_weight[i]=.01;
 210       st->loudness_weight[i] *= st->loudness_weight[i];
 211    }
 212
 213    st->speech_prob = 0;
 214    st->last_speech = 1000;
 215    st->loudness = pow(6000,LOUDNESS_EXP);
 216    st->loudness2 = 6000;
 217    st->nb_loudness_adapt = 0;
 218
 219    st->fft_lookup = speex_alloc(sizeof(struct drft_lookup));
 220    drft_init(st->fft_lookup,2*N);
 221
 222    st->nb_adapt=0;
 223    st->consec_noise=0;
 224    st->nb_preprocess=0;
 225    return st;
 226 }
 227
 228 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
 229 {
 230    speex_free(st->frame);
 231    speex_free(st->ps);
 232    speex_free(st->gain2);
 233    speex_free(st->window);
 234    speex_free(st->noise);
 235    speex_free(st->old_ps);
 236    speex_free(st->gain);
 237    speex_free(st->prior);
 238    speex_free(st->post);
 239    speex_free(st->loudness_weight);
 240    speex_free(st->echo_noise);
 241
 242    speex_free(st->S);
 243    speex_free(st->Smin);
 244    speex_free(st->Stmp);
 245    speex_free(st->update_prob);
 246
 247    speex_free(st->noise_bands);
 248    speex_free(st->noise_bands2);
 249    speex_free(st->speech_bands);
 250    speex_free(st->speech_bands2);
 251
 252    speex_free(st->inbuf);
 253    speex_free(st->outbuf);
 254
 255    drft_clear(st->fft_lookup);
 256    speex_free(st->fft_lookup);
 257
 258    speex_free(st);
 259 }
 260
 261 static void update_noise(SpeexPreprocessState *st, float *ps, float *echo)
 262 {
 263    int i;
 264    float beta;
 265    st->nb_adapt++;
 266    beta=1.0/st->nb_adapt;
 267    if (beta < .05)
 268       beta=.05;
 269
 270    if (!echo)
 271    {
 272       for (i=0;i<st->ps_size;i++)
 273          st->noise[i] = (1-beta)*st->noise[i] + beta*ps[i];
 274    } else {
 275       for (i=0;i<st->ps_size;i++)
 276          st->noise[i] = (1-beta)*st->noise[i] + beta*max(0,ps[i]-echo[i]);
 277 #if 0
 278       for (i=0;i<st->ps_size;i++)
 279          st->noise[i] = 0;
 280 #endif
 281    }
 282 }
 283
 284 static int speex_compute_vad(SpeexPreprocessState *st, float *ps, float mean_prior, float mean_post)
 285 {
 286    int i, is_speech=0;
 287    int N = st->ps_size;
 288    float scale=.5/N;
 289
 290    /* FIXME: Clean this up a bit */
 291    {
 292       float bands[NB_BANDS];
 293       int j;
 294       float p0, p1;
 295       float tot_loudness=0;
 296       float x = sqrt(mean_post);
 297
 298       for (i=5;i<N-10;i++)
 299       {
 300          tot_loudness += scale*st->ps[i] * st->loudness_weight[i];
 301       }
 302
 303       for (i=0;i<NB_BANDS;i++)
 304       {
 305          bands[i]=1e4;
 306          for (j=i*N/NB_BANDS;j<(i+1)*N/NB_BANDS;j++)
 307          {
 308             bands[i] += ps[j];
 309          }
 310          bands[i]=log(bands[i]);
 311       }
 312
 313       /*p1 = .0005+.6*exp(-.5*(x-.4)*(x-.4)*11)+.1*exp(-1.2*x);
 314       if (x<1.5)
 315          p0=.1*exp(2*(x-1.5));
 316       else
 317          p0=.02+.1*exp(-.2*(x-1.5));
 318       */
 319
 320       p0=1/(1+exp(3*(1.5-x)));
 321       p1=1-p0;
 322
 323       /*fprintf (stderr, "%f %f ", p0, p1);*/
 324       /*p0 *= .99*st->speech_prob + .01*(1-st->speech_prob);
 325       p1 *= .01*st->speech_prob + .99*(1-st->speech_prob);
 326
 327       st->speech_prob = p0/(p1+p0);
 328       */
 329
 330       if (st->noise_bandsN < 50 || st->speech_bandsN < 50)
 331       {
 332          if (mean_post > 5)
 333          {
 334             float adapt = 1./st->speech_bandsN++;
 335             if (adapt<.005)
 336                adapt = .005;
 337             for (i=0;i<NB_BANDS;i++)
 338             {
 339                st->speech_bands[i] = (1-adapt)*st->speech_bands[i] + adapt*bands[i];
 340                /*st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*bands[i]*bands[i];*/
 341                st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*(bands[i]-st->speech_bands[i])*(bands[i]-st->speech_bands[i]);
 342             }
 343          } else {
 344             float adapt = 1./st->noise_bandsN++;
 345             if (adapt<.005)
 346                adapt = .005;
 347             for (i=0;i<NB_BANDS;i++)
 348             {
 349                st->noise_bands[i] = (1-adapt)*st->noise_bands[i] + adapt*bands[i];
 350                /*st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*bands[i]*bands[i];*/
 351                st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*(bands[i]-st->noise_bands[i])*(bands[i]-st->noise_bands[i]);
 352             }
 353          }
 354       }
 355       p0=p1=1;
 356       for (i=0;i<NB_BANDS;i++)
 357       {
 358          float noise_var, speech_var;
 359          float noise_mean, speech_mean;
 360          float tmp1, tmp2, pr;
 361
 362          /*noise_var = 1.01*st->noise_bands2[i] - st->noise_bands[i]*st->noise_bands[i];
 363            speech_var = 1.01*st->speech_bands2[i] - st->speech_bands[i]*st->speech_bands[i];*/
 364          noise_var = st->noise_bands2[i];
 365          speech_var = st->speech_bands2[i];
 366          if (noise_var < .1)
 367             noise_var = .1;
 368          if (speech_var < .1)
 369             speech_var = .1;
 370
 371          /*speech_var = sqrt(speech_var*noise_var);
 372            noise_var = speech_var;*/
 373          if (speech_var < .05*speech_var)
 374             noise_var = .05*speech_var;
 375          if (speech_var < .05*noise_var)
 376             speech_var = .05*noise_var;
 377
 378          if (bands[i] < st->noise_bands[i])
 379             speech_var = noise_var;
 380          if (bands[i] > st->speech_bands[i])
 381             noise_var = speech_var;
 382
 383          speech_mean = st->speech_bands[i];
 384          noise_mean = st->noise_bands[i];
 385          if (noise_mean < speech_mean - 5)
 386             noise_mean = speech_mean - 5;
 387
 388          tmp1 = exp(-.5*(bands[i]-speech_mean)*(bands[i]-speech_mean)/speech_var)/sqrt(2*M_PI*speech_var);
 389          tmp2 = exp(-.5*(bands[i]-noise_mean)*(bands[i]-noise_mean)/noise_var)/sqrt(2*M_PI*noise_var);
 390          /*fprintf (stderr, "%f ", (float)(p0/(.01+p0+p1)));*/
 391          /*fprintf (stderr, "%f ", (float)(bands[i]));*/
 392          pr = tmp1/(1e-25+tmp1+tmp2);
 393          /*if (bands[i] < st->noise_bands[i])
 394             pr=.01;
 395          if (bands[i] > st->speech_bands[i] && pr < .995)
 396          pr=.995;*/
 397          if (pr>.999)
 398             pr=.999;
 399          if (pr<.001)
 400             pr=.001;
 401          /*fprintf (stderr, "%f ", pr);*/
 402          p0 *= pr;
 403          p1 *= (1-pr);
 404       }
 405
 406       p0 = pow(p0,.2);
 407       p1 = pow(p1,.2);
 408
 409 #if 1
 410       p0 *= 2;
 411       p0=p0/(p1+p0);
 412       if (st->last_speech>20)
 413       {
 414          float tmp = sqrt(tot_loudness)/st->loudness2;
 415          tmp = 1-exp(-10*tmp);
 416          if (p0>tmp)
 417             p0=tmp;
 418       }
 419       p1=1-p0;
 420 #else
 421       if (sqrt(tot_loudness) < .6*st->loudness2 && p0>15*p1)
 422          p0=15*p1;
 423       if (sqrt(tot_loudness) < .45*st->loudness2 && p0>7*p1)
 424          p0=7*p1;
 425       if (sqrt(tot_loudness) < .3*st->loudness2 && p0>3*p1)
 426          p0=3*p1;
 427       if (sqrt(tot_loudness) < .15*st->loudness2 && p0>p1)
 428          p0=p1;
 429       /*fprintf (stderr, "%f %f ", (float)(sqrt(tot_loudness) /( .25*st->loudness2)), p0/(p1+p0));*/
 430 #endif
 431
 432       p0 *= .99*st->speech_prob + .01*(1-st->speech_prob);
 433       p1 *= .01*st->speech_prob + .99*(1-st->speech_prob);
 434
 435       st->speech_prob = p0/(1e-25+p1+p0);
 436       /*fprintf (stderr, "%f %f %f ", tot_loudness, st->loudness2, st->speech_prob);*/
 437
 438       if (st->speech_prob>.35 || (st->last_speech < 20 && st->speech_prob>.1))
 439       {
 440          is_speech = 1;
 441          st->last_speech = 0;
 442       } else {
 443          st->last_speech++;
 444          if (st->last_speech<20)
 445            is_speech = 1;
 446       }
 447
 448       if (st->noise_bandsN > 50 && st->speech_bandsN > 50)
 449       {
 450          if (mean_post > 5)
 451          {
 452             float adapt = 1./st->speech_bandsN++;
 453             if (adapt<.005)
 454                adapt = .005;
 455             for (i=0;i<NB_BANDS;i++)
 456             {
 457                st->speech_bands[i] = (1-adapt)*st->speech_bands[i] + adapt*bands[i];
 458                /*st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*bands[i]*bands[i];*/
 459                st->speech_bands2[i] = (1-adapt)*st->speech_bands2[i] + adapt*(bands[i]-st->speech_bands[i])*(bands[i]-st->speech_bands[i]);
 460             }
 461          } else {
 462             float adapt = 1./st->noise_bandsN++;
 463             if (adapt<.005)
 464                adapt = .005;
 465             for (i=0;i<NB_BANDS;i++)
 466             {
 467                st->noise_bands[i] = (1-adapt)*st->noise_bands[i] + adapt*bands[i];
 468                /*st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*bands[i]*bands[i];*/
 469                st->noise_bands2[i] = (1-adapt)*st->noise_bands2[i] + adapt*(bands[i]-st->noise_bands[i])*(bands[i]-st->noise_bands[i]);
 470             }
 471          }
 472       }
 473
 474
 475    }
 476
 477    return is_speech;
 478 }
 479
 480 static void speex_compute_agc(SpeexPreprocessState *st, float mean_prior)
 481 {
 482    int i;
 483    int N = st->ps_size;
 484    float scale=.5/N;
 485    float agc_gain;
 486    int freq_start, freq_end;
 487    float active_bands = 0;
 488
 489    freq_start = (int)(300.0*2*N/st->sampling_rate);
 490    freq_end   = (int)(2000.0*2*N/st->sampling_rate);
 491    for (i=freq_start;i<freq_end;i++)
 492    {
 493       if (st->S[i] > 20*st->Smin[i]+1000)
 494          active_bands+=1;
 495    }
 496    active_bands /= (freq_end-freq_start+1);
 497
 498    if (active_bands > .2)
 499    {
 500       float loudness=0;
 501       float rate, rate2=.2;
 502       st->nb_loudness_adapt++;
 503       rate=2.0/(1+st->nb_loudness_adapt);
 504       if (rate < .05)
 505          rate = .05;
 506       if (rate < .1 && pow(loudness, LOUDNESS_EXP) > st->loudness)
 507          rate = .1;
 508       if (rate < .2 && pow(loudness, LOUDNESS_EXP) > 3*st->loudness)
 509          rate = .2;
 510       if (rate < .4 && pow(loudness, LOUDNESS_EXP) > 10*st->loudness)
 511          rate = .4;
 512
 513       for (i=2;i<N;i++)
 514       {
 515          loudness += scale*st->ps[i] * st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
 516       }
 517       loudness=sqrt(loudness);
 518       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
 519         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
 520       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
 521
 522       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0/LOUDNESS_EXP);
 523
 524       loudness = pow(st->loudness, 1.0/LOUDNESS_EXP);
 525
 526       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
 527    }
 528
 529    agc_gain = st->agc_level/st->loudness2;
 530    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
 531    if (agc_gain>200)
 532       agc_gain = 200;
 533
 534    for (i=0;i<N;i++)
 535       st->gain2[i] *= agc_gain;
 536
 537 }
 538
 539 static void preprocess_analysis(SpeexPreprocessState *st, float *x)
 540 {
 541    int i;
 542    int N = st->ps_size;
 543    int N3 = 2*N - st->frame_size;
 544    int N4 = st->frame_size - N3;
 545    float *ps=st->ps;
 546
 547    /* 'Build' input frame */
 548    for (i=0;i<N3;i++)
 549       st->frame[i]=st->inbuf[i];
 550    for (i=0;i<st->frame_size;i++)
 551       st->frame[N3+i]=x[i];
 552
 553    /* Update inbuf */
 554    for (i=0;i<N3;i++)
 555       st->inbuf[i]=x[N4+i];
 556
 557    /* Windowing */
 558    for (i=0;i<2*N;i++)
 559       st->frame[i] *= st->window[i];
 560
 561    /* Perform FFT */
 562    drft_forward(st->fft_lookup, st->frame);
 563
 564    /* Power spectrum */
 565    ps[0]=1;
 566    for (i=1;i<N;i++)
 567       ps[i]=1+st->frame[2*i-1]*st->frame[2*i-1] + st->frame[2*i]*st->frame[2*i];
 568
 569 }
 570
 571 static void update_noise_prob(SpeexPreprocessState *st)
 572 {
 573    int i;
 574    int N = st->ps_size;
 575
 576    for (i=1;i<N-1;i++)
 577       st->S[i] = 100+ .8*st->S[i] + .05*st->ps[i-1]+.1*st->ps[i]+.05*st->ps[i+1];
 578
 579    if (st->nb_preprocess<1)
 580    {
 581       for (i=1;i<N-1;i++)
 582          st->Smin[i] = st->Stmp[i] = st->S[i]+100;
 583    }
 584
 585    if (st->nb_preprocess%80==0)
 586    {
 587       for (i=1;i<N-1;i++)
 588       {
 589          st->Smin[i] = min(st->Stmp[i], st->S[i]);
 590          st->Stmp[i] = st->S[i];
 591       }
 592    } else {
 593       for (i=1;i<N-1;i++)
 594       {
 595          st->Smin[i] = min(st->Smin[i], st->S[i]);
 596          st->Stmp[i] = min(st->Stmp[i], st->S[i]);
 597       }
 598    }
 599    for (i=1;i<N-1;i++)
 600    {
 601       st->update_prob[i] *= .2;
 602       if (st->S[i] > 5*st->Smin[i])
 603          st->update_prob[i] += .8;
 604       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
 605       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
 606    }
 607
 608 }
 609
 610 int speex_preprocess(SpeexPreprocessState *st, float *x, float *echo)
 611 {
 612    int i;
 613    int is_speech=1;
 614    float mean_post=0;
 615    float mean_prior=0;
 616    int N = st->ps_size;
 617    int N3 = 2*N - st->frame_size;
 618    int N4 = st->frame_size - N3;
 619    float scale=.5/N;
 620    float *ps=st->ps;
 621    float Zframe=0, Pframe;
 622
 623    preprocess_analysis(st, x);
 624
 625    update_noise_prob(st);
 626
 627    st->nb_preprocess++;
 628
 629    /* Noise estimation always updated for the 20 first times */
 630    if (st->nb_adapt<10)
 631    {
 632       update_noise(st, ps, echo);
 633    }
 634
 635    /* Deal with residual echo if provided */
 636    if (echo)
 637       for (i=1;i<N;i++)
 638          st->echo_noise[i] = (.7*st->echo_noise[i] + .3* echo[i]);
 639
 640    /* Compute a posteriori SNR */
 641    for (i=1;i<N;i++)
 642    {
 643       st->post[i] = ps[i]/(1+st->noise[i]+st->echo_noise[i]) - 1;
 644       if (st->post[i]>100)
 645          st->post[i]=100;
 646       /*if (st->post[i]<0)
 647         st->post[i]=0;*/
 648       mean_post+=st->post[i];
 649    }
 650    mean_post /= N;
 651    if (mean_post<0)
 652       mean_post=0;
 653
 654    /* Special case for first frame */
 655    if (st->nb_adapt==1)
 656       for (i=1;i<N;i++)
 657          st->old_ps[i] = ps[i];
 658
 659    /* Compute a priori SNR */
 660    {
 661       /* A priori update rate */
 662       float gamma;
 663       float min_gamma=0.12;
 664       gamma = 1.0/st->nb_preprocess;
 665
 666       /*Make update rate smaller when there's no speech*/
 667 #if 0
 668       if (mean_post<3.5 && mean_prior < 1)
 669          min_gamma *= (mean_post+.5);
 670       else
 671          min_gamma *= 4.;
 672 #else
 673       min_gamma = .2*fabs(mean_prior - mean_post)*fabs(mean_prior - mean_post);
 674       if (min_gamma>.6)
 675          min_gamma = .6;
 676       if (min_gamma<.01)
 677          min_gamma = .01;
 678 #endif
 679
 680       if (gamma<min_gamma)
 681          gamma=min_gamma;
 682
 683       for (i=1;i<N;i++)
 684       {
 685
 686          /* A priori SNR update */
 687          st->prior[i] = gamma*max(0.0,st->post[i]) +
 688          (1-gamma)*st->gain[i]*st->gain[i]*st->old_ps[i]/(1+st->noise[i]+st->echo_noise[i]);
 689
 690          if (st->prior[i]>100)
 691             st->prior[i]=100;
 692
 693          mean_prior+=st->prior[i];
 694       }
 695    }
 696    mean_prior /= N;
 697
 698 #if 0
 699    for (i=0;i<N;i++)
 700    {
 701       fprintf (stderr, "%f ", st->prior[i]);
 702    }
 703    fprintf (stderr, "\n");
 704 #endif
 705    /*fprintf (stderr, "%f %f\n", mean_prior,mean_post);*/
 706
 707    if (st->nb_preprocess>=20)
 708    {
 709       int do_update = 0;
 710       float noise_ener=0, sig_ener=0;
 711       /* If SNR is low (both a priori and a posteriori), update the noise estimate*/
 712       /*if (mean_prior<.23 && mean_post < .5)*/
 713       if (mean_prior<.23 && mean_post < .5)
 714          do_update = 1;
 715       for (i=1;i<N;i++)
 716       {
 717          noise_ener += st->noise[i];
 718          sig_ener += ps[i];
 719       }
 720       if (noise_ener > 3*sig_ener)
 721          do_update = 1;
 722       /*do_update = 0;*/
 723       if (do_update)
 724       {
 725          st->consec_noise++;
 726       } else {
 727          st->consec_noise=0;
 728       }
 729    }
 730
 731    if (st->vad_enabled)
 732       is_speech = speex_compute_vad(st, ps, mean_prior, mean_post);
 733
 734
 735    if (st->consec_noise>=3)
 736    {
 737       update_noise(st, st->old_ps, echo);
 738    } else {
 739       for (i=1;i<N-1;i++)
 740       {
 741          if (st->update_prob[i]<.5)
 742             st->noise[i] = .90*st->noise[i] + .1*st->ps[i];
 743       }
 744    }
 745
 746    for (i=1;i<N;i++)
 747    {
 748       st->zeta[i] = .7*st->zeta[i] + .3*st->prior[i];
 749    }
 750
 751    {
 752       int freq_start = (int)(300.0*2*N/st->sampling_rate);
 753       int freq_end   = (int)(2000.0*2*N/st->sampling_rate);
 754       for (i=freq_start;i<freq_end;i++)
 755       {
 756          Zframe += st->zeta[i];
 757       }
 758    }
 759
 760    Zframe /= N;
 761    if (Zframe<ZMIN)
 762    {
 763       Pframe = 0;
 764    } else {
 765       if (Zframe > 1.5*st->Zlast)
 766       {
 767          Pframe = 1;
 768          st->Zpeak = Zframe;
 769          if (st->Zpeak > 10)
 770             st->Zpeak = 10;
 771          if (st->Zpeak < 1)
 772             st->Zpeak = 1;
 773       } else {
 774          if (Zframe < st->Zpeak*ZMIN)
 775          {
 776             Pframe = 0;
 777          } else if (Zframe > st->Zpeak*ZMAX)
 778          {
 779             Pframe = 1;
 780          } else {
 781             Pframe = log(Zframe/(st->Zpeak*ZMIN)) / log(ZMAX/ZMIN);
 782          }
 783       }
 784    }
 785    st->Zlast = Zframe;
 786
 787    /*fprintf (stderr, "%f\n", Pframe);*/
 788    /* Compute gain according to the Ephraim-Malah algorithm */
 789    for (i=1;i<N;i++)
 790    {
 791       float MM;
 792       float theta;
 793       float prior_ratio;
 794       float p, q;
 795       float zeta1;
 796       float P1;
 797
 798       prior_ratio = st->prior[i]/(1.0001+st->prior[i]);
 799       theta = (1+st->post[i])*prior_ratio;
 800
 801       if (i==1 || i==N-1)
 802          zeta1 = st->zeta[i];
 803       else
 804          zeta1 = .25*st->zeta[i-1] + .5*st->zeta[i] + .25*st->zeta[i+1];
 805       if (zeta1<ZMIN)
 806          P1 = 0;
 807       else if (zeta1>ZMAX)
 808          P1 = 1;
 809       else
 810          P1 = LOG_MIN_MAX_1 * log(ZMIN_1*zeta1);
 811
 812       /*P1 = log(zeta1/ZMIN)/log(ZMAX/ZMIN);*/
 813
 814       /* FIXME: add global prop (P2) */
 815       q = 1-Pframe*P1;
 816       if (q>.95)
 817          q=.95;
 818       p=1/(1 + (q/(1-q))*(1+st->prior[i])*exp(-theta));
 819
 820
 821 #if 0
 822       /* log-spectral magnitude estimator */
 823       if (theta<6)
 824          MM = 0.74082*pow(theta+1,.61)/sqrt(.0001+theta);
 825       else
 826          MM=1;
 827 #else
 828       /* Optimal estimator for loudness domain */
 829       MM = hypergeom_gain(theta);
 830 #endif
 831
 832       st->gain[i] = prior_ratio * MM;
 833       /*Put some (very arbitraty) limit on the gain*/
 834       if (st->gain[i]>2)
 835       {
 836          st->gain[i]=2;
 837       }
 838
 839       if (st->denoise_enabled)
 840       {
 841          st->gain2[i]=p*p*st->gain[i];
 842       } else {
 843          st->gain2[i]=1;
 844       }
 845    }
 846    st->gain2[0]=st->gain[0]=0;
 847    st->gain2[N-1]=st->gain[N-1]=0;
 848
 849    if (st->agc_enabled)
 850       speex_compute_agc(st, mean_prior);
 851
 852 #if 0
 853    if (!is_speech)
 854    {
 855       for (i=0;i<N;i++)
 856          st->gain2[i] = 0;
 857    }
 858 #if 0
 859  else {
 860       for (i=0;i<N;i++)
 861          st->gain2[i] = 1;
 862    }
 863 #endif
 864 #endif
 865
 866    /* Apply computed gain */
 867    for (i=1;i<N;i++)
 868    {
 869       st->frame[2*i-1] *= st->gain2[i];
 870       st->frame[2*i] *= st->gain2[i];
 871    }
 872
 873    /* Get rid of the DC and very low frequencies */
 874    st->frame[0]=0;
 875    st->frame[1]=0;
 876    st->frame[2]=0;
 877    /* Nyquist frequency is mostly useless too */
 878    st->frame[2*N-1]=0;
 879
 880    /* Inverse FFT with 1/N scaling */
 881    drft_backward(st->fft_lookup, st->frame);
 882
 883    for (i=0;i<2*N;i++)
 884       st->frame[i] *= scale;
 885
 886    {
 887       float max_sample=0;
 888       for (i=0;i<2*N;i++)
 889          if (fabs(st->frame[i])>max_sample)
 890             max_sample = fabs(st->frame[i]);
 891       if (max_sample>28000)
 892       {
 893          float damp = 28000./max_sample;
 894          for (i=0;i<2*N;i++)
 895             st->frame[i] *= damp;
 896       }
 897    }
 898
 899    for (i=0;i<2*N;i++)
 900       st->frame[i] *= st->window[i];
 901
 902    /* Perform overlap and add */
 903    for (i=0;i<N3;i++)
 904       x[i] = st->outbuf[i] + st->frame[i];
 905    for (i=0;i<N4;i++)
 906       x[N3+i] = st->frame[N3+i];
 907
 908    /* Update outbuf */
 909    for (i=0;i<N3;i++)
 910       st->outbuf[i] = st->frame[st->frame_size+i];
 911
 912    /* Save old power spectrum */
 913    for (i=1;i<N;i++)
 914       st->old_ps[i] = ps[i];
 915
 916    return is_speech;
 917 }
 918
 919 void speex_preprocess_estimate_update(SpeexPreprocessState *st, float *x, float *noise)
 920 {
 921    int i;
 922    int N = st->ps_size;
 923    int N3 = 2*N - st->frame_size;
 924
 925    float *ps=st->ps;
 926
 927    preprocess_analysis(st, x);
 928
 929    update_noise_prob(st);
 930
 931    st->nb_preprocess++;
 932
 933    for (i=1;i<N-1;i++)
 934    {
 935       if (st->update_prob[i]<.5)
 936          st->noise[i] = .90*st->noise[i] + .1*ps[i];
 937    }
 938
 939    for (i=0;i<N3;i++)
 940       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
 941
 942    /* Save old power spectrum */
 943    for (i=1;i<N;i++)
 944       st->old_ps[i] = ps[i];
 945
 946 }
 947
 948
 949 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
 950 {
 951    SpeexPreprocessState *st;
 952    st=(SpeexPreprocessState*)state;
 953    switch(request)
 954    {
 955    case SPEEX_PREPROCESS_SET_DENOISE:
 956       st->denoise_enabled = (*(int*)ptr);
 957       break;
 958    case SPEEX_PREPROCESS_GET_DENOISE:
 959       (*(int*)ptr) = st->denoise_enabled;
 960       break;
 961
 962    case SPEEX_PREPROCESS_SET_AGC:
 963       st->agc_enabled = (*(int*)ptr);
 964       break;
 965    case SPEEX_PREPROCESS_GET_AGC:
 966       (*(int*)ptr) = st->agc_enabled;
 967       break;
 968
 969    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
 970       st->agc_level = (*(float*)ptr);
 971       if (st->agc_level<1)
 972          st->agc_level=1;
 973       if (st->agc_level>32768)
 974          st->agc_level=32768;
 975       break;
 976    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
 977       (*(float*)ptr) = st->agc_level;
 978       break;
 979
 980    case SPEEX_PREPROCESS_SET_VAD:
 981       st->vad_enabled = (*(int*)ptr);
 982       break;
 983    case SPEEX_PREPROCESS_GET_VAD:
 984       (*(int*)ptr) = st->vad_enabled;
 985       break;
 986    default:
 987       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
 988       return -1;
 989    }
 990    return 0;
 991 }