Converted filters with memory to direct form II transposed, this creates
[speexdsp.git] / libspeex / filters.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: filters.c
3    Various analysis/synthesis filters
4
5    This library is free software; you can redistribute it and/or
6    modify it under the terms of the GNU Lesser General Public
7    License as published by the Free Software Foundation; either
8    version 2.1 of the License, or (at your option) any later version.
9    
10    This library is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    Lesser General Public License for more details.
14    
15    You should have received a copy of the GNU Lesser General Public
16    License along with this library; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20 #include "filters.h"
21 #include "stack_alloc.h"
22 #include <stdio.h>
23 #include <math.h>
24
25 #define min(a,b) ((a) < (b) ? (a) : (b))
26
27 #define MAX_ORD 20
28
29 void print_vec(float *vec, int len, char *name)
30 {
31    int i;
32    printf ("%s ", name);
33    for (i=0;i<len;i++)
34       printf (" %f", vec[i]);
35    printf ("\n");
36 }
37
38 void bw_lpc(float gamma, float *lpc_in, float *lpc_out, int order)
39 {
40    int i;
41    float tmp=1;
42    for (i=0;i<order+1;i++)
43    {
44       lpc_out[i] = tmp * lpc_in[i];
45       tmp *= gamma;
46    }
47 }
48
49 void enh_lpc(float *ak, int order, float *num, float *den, float k1, float k2, float *stack)
50 {
51    int i;
52    float *n2, *d2;
53    float k3, r;
54    n2=PUSH(stack,order+1);
55    d2=PUSH(stack,order+1);
56    for (i=0;i<=order;i++)
57    {
58       /*FIXME: Big kludge here!!!*/
59       den[i]=0*ak[i];
60       num[i]=0;
61    }
62    den[0]=1;
63    for (i=order+1;i<=(order<<1);i++)
64       den[i]=num[i]=0;
65    r=.9;
66    k3=(1-(1-r*k1)/(1-r*k2))/r;
67    bw_lpc(k1, ak, d2, order);
68    num[0]=1;
69    bw_lpc(k2, ak, num, order);
70    bw_lpc(k3, ak, n2, order);
71    residue_zero(num,n2,num,1+(order<<1),order);
72    residue_zero(den,d2,den,1+(order<<1),order);
73    POP(stack);
74    POP(stack);
75 }
76
77 void syn_filt(float *x, float *a, float *y, int N, int ord)
78 {
79    int i,j;
80    for (i=0;i<N;i++)
81    {
82       y[i]=x[i];
83       for (j=1;j<=ord;j++)
84          y[i] -= a[j]*y[i-j];
85    }
86 }
87
88 void syn_filt_zero(float *x, float *a, float *y, int N, int ord)
89 {
90    int i,j;
91    for (i=0;i<N;i++)
92    {
93       y[i]=x[i];
94       for (j=1;j<=min(ord,i);j++)
95          y[i] -= a[j]*y[i-j];
96    }
97 }
98
99 void syn_filt_mem(float *x, float *a, float *y, int N, int ord, float *mem)
100 {
101    int i,j;
102    for (i=0;i<N;i++)
103    {
104       y[i]=x[i];
105       for (j=1;j<=min(ord,i);j++)
106          y[i] -= a[j]*y[i-j];
107       for (j=i+1;j<=ord;j++)
108          y[i] -= a[j]*mem[j-i-1];
109    }
110    for (i=0;i<ord;i++)
111       mem[i]=y[N-i-1];
112 }
113
114
115 void residue(float *x, float *a, float *y, int N, int ord)
116 {
117    int i,j;
118    for (i=N-1;i>=0;i--)
119    {
120       y[i]=x[i];
121       for (j=1;j<=ord;j++)
122          y[i] += a[j]*x[i-j];
123    }
124 }
125
126 void residue_zero(float *x, float *a, float *y, int N, int ord)
127 {
128    int i,j;
129    for (i=N-1;i>=0;i--)
130    {
131       y[i]=x[i];
132       for (j=1;j<=min(ord,i);j++)
133          y[i] += a[j]*x[i-j];
134    }
135 }
136
137 void residue_mem(float *x, float *a, float *y, int N, int ord, float *mem)
138 {
139    int i,j;
140    for (i=N-1;i>=0;i--)
141    {
142       y[i]=x[i];
143       for (j=1;j<=min(ord,i);j++)
144          y[i] += a[j]*x[i-j];
145       for (j=i+1;j<=ord;j++)
146          y[i] += a[j]*mem[j-i-1];
147    }
148    for (i=0;i<ord;i++)
149       mem[i]=x[N-i-1];
150 }
151
152
153 void filter_mem2(float *x, float *num, float *den, float *y, int N, int ord, float *mem)
154 {
155    int i,j;
156    float xi;
157    for (i=0;i<N;i++)
158    {
159       xi=x[i];
160       y[i] = num[0]*xi + mem[0];
161       for (j=0;j<ord-1;j++)
162       {
163          mem[j] = mem[j+1] + num[j+1]*xi - den[j+1]*y[i];
164       }
165       mem[ord-1] = num[ord]*xi - den[ord]*y[i];
166    }
167 }
168
169 void fir_mem2(float *x, float *num, float *y, int N, int ord, float *mem)
170 {
171    int i,j;
172    float xi;
173    for (i=0;i<N;i++)
174    {
175       xi=x[i];
176       y[i] = num[0]*xi + mem[0];
177       for (j=0;j<ord-1;j++)
178       {
179          mem[j] = mem[j+1] + num[j+1]*xi;
180       }
181       mem[ord-1] = num[ord]*xi;
182    }
183 }
184
185 void iir_mem2(float *x, float *den, float *y, int N, int ord, float *mem)
186 {
187    int i,j;
188    for (i=0;i<N;i++)
189    {
190       y[i] = x[i] + mem[0];
191       for (j=0;j<ord-1;j++)
192       {
193          mem[j] = mem[j+1] - den[j+1]*y[i];
194       }
195       mem[ord-1] = - den[ord]*y[i];
196    }
197 }
198
199
200 void pole_zero_mem(float *x, float *num, float *den, float *y, int N, int ord, float *mem, float *stack)
201 {
202    float *tmp=PUSH(stack, N);
203    syn_filt_mem(x, den, tmp, N, ord, mem);
204    residue_mem(tmp, num, y, N, ord, mem+ord);
205    POP(stack);
206 }
207
208
209 /*
210 void fir_mem(float *x, float *a, float *y, int N, int M, float *mem)
211 {
212    int i,j;
213    for (i=0;i<N;i++)
214    {
215       y[i]=0;
216       for (j=0;j<=min(M-1,i);j++)
217          y[i] += a[j]*x[i-j];
218       for (j=i+1;j<=M-1;j++)
219          y[i] += a[j]*mem[j-i-1];
220    }
221    for (i=0;i<M-1;i++)
222       mem[i]=x[N-i-1];
223 }
224 */
225
226 void syn_percep_zero(float *xx, float *ak, float *awk1, float *awk2, float *y, int N, int ord)
227 {
228    int i,j;
229    float long_filt[21];
230    float iir[20];
231    float fir[10];
232    float x[40];
233    int ord2=ord<<1;
234    for (i=0;i<=ord;i++)
235       long_filt[i]=ak[i];
236    for (i=ord+1;i<=ord2;i++)
237       long_filt[i]=0;
238    residue_zero(long_filt,awk2,long_filt,1+(ord<<1),ord);
239
240    for (i=0;i<N;i++)
241       x[i]=xx[i];
242    for (i=0;i<ord2;i++)
243       iir[i]=long_filt[ord2-i];
244    for (i=0;i<ord;i++)
245       fir[i]=awk1[ord-i];
246
247    for (i=0;i<ord2;i++)
248    {
249       y[i]=x[i];
250       for (j=1;j<=min(ord2,i);j++)
251          y[i] -= long_filt[j]*y[i-j];
252       for (j=1;j<=min(ord,i);j++)
253          y[i] += awk1[j]*x[i-j];
254    }
255 #if 0
256    for (i=ord2;i<N;i++)
257    {
258       float *yptr=y+i-ord2;
259       float *xptr=x+i-ord;
260       y[i]=x[i];
261       for (j=0;j<ord2;j++)
262          y[i]-= iir[j]*yptr[j];
263       for (j=0;j<ord;j++)
264          y[i]+= fir[j]*xptr[j];
265    }
266 #else
267    for (i=ord2;i<N;i++)
268    {
269       float *f, *ptr;
270       float sum1=x[i], sum2=0;
271       f=iir;
272       ptr=y+i-ord2;
273       for (j=0;j<ord2;j+=2)
274       {
275          sum1-= f[0]*ptr[0];
276          sum2-= f[1]*ptr[1];
277          f+=2;
278          ptr+=2;
279       }
280       f=fir;
281       ptr=x+i-ord;
282       for (j=0;j<ord;j+=2)
283       {
284          sum1+= f[0]*ptr[0];
285          sum2+= f[1]*ptr[1];
286          f+=2;
287          ptr+=2;
288       }
289       y[i]=sum1+sum2;
290    }
291 #endif
292 }
293
294 #if 1
295 #define MAX_FILTER 100
296 #define MAX_SIGNAL 1000
297 void fir_mem(float *xx, float *aa, float *y, int N, int M, float *mem)
298 {
299    int i,j;
300    float a[MAX_FILTER];
301    float x[MAX_SIGNAL];
302    for (i=0;i<M;i++)
303       a[M-i-1]=aa[i];
304    for (i=0;i<M-1;i++)
305       x[i]=mem[M-i-2];
306    for (i=0;i<N;i++)
307       x[i+M-1]=xx[i];
308    for (i=0;i<N;i++)
309    {
310       y[i]=0;
311       for (j=0;j<M;j++)
312          y[i]+=a[j]*x[i+j];
313    }
314    for (i=0;i<M-1;i++)
315      mem[i]=xx[N-i-1];
316 }
317 #else
318 void fir_mem(float *xx, float *aa, float *y, int N, int M, float *mem)
319 {
320    fir_mem2(xx, aa, y, N, M-1, mem);
321 }
322 #endif
323
324 void comb_filter(
325 float *exc,          /*decoded excitation*/
326 float *new_exc,      /*enhanced excitation*/
327 float *ak,           /*LPC filter coefs*/
328 int p,               /*LPC order*/
329 int nsf,             /*sub-frame size*/
330 int pitch,           /*pitch period*/
331 float *pitch_gain,   /*pitch gain (3-tap)*/
332 float  comb_gain     /*gain of comb filter*/
333 )
334 {
335    int i;
336    float exc_energy=0, new_exc_energy=0;
337    float gain;
338
339    /*Compute excitation energy prior to enhancement*/
340    for (i=0;i<nsf;i++)
341       exc_energy+=exc[i]*exc[i];
342
343    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/
344    for (i=0;i<nsf;i++)
345    {
346       new_exc[i] = exc[i] + comb_gain * (
347                                          pitch_gain[0]*exc[i-pitch+1] +
348                                          pitch_gain[1]*exc[i-pitch] +
349                                          pitch_gain[2]*exc[i-pitch-1]
350                                          );
351    }
352    
353    /*Gain after enhancement*/
354    for (i=0;i<nsf;i++)
355       new_exc_energy+=new_exc[i]*new_exc[i];
356
357    /*Compute scaling factor and normalize energy*/
358    gain = sqrt(exc_energy)/sqrt(.1+new_exc_energy);
359    for (i=0;i<nsf;i++)
360       new_exc[i] *= gain;
361 }