3-tap pitch predictor seems to work
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Lont-Term Prediction functions
4 */
5
6
7 #define abs(x) ((x)<0 ? -(x) : (x))
8 /* Computes the open-loop pitch prediction. Returns pitch period and pitch gain */
9 int open_loop_ltp(float *x, int len, int start, int end, float *gain)
10    /*  x:     time-domain signal (note, x[-end] must be valid)
11        len:   length of the x signal
12        start: smallest pitch period possible
13        end:   largest pitch period possible
14        gain:  return value for the pitch predictor gain
15     */
16 {
17    int i, period, best_period=0;
18    float score, best_score=-1, corr, energy;
19    
20    for (period = start; period <= end; period++)
21    {
22       corr=energy=0;
23       for (i=0;i<len;i++)
24       {
25          corr += x[i]*x[i-period];
26          energy += x[i-period]*x[i-period];
27       }
28       score = corr*corr/(energy+1);
29       if (score > best_score)
30       {
31          best_score=score;
32          best_period=period;
33          *gain = corr/(energy+1);
34       }
35       
36    }
37    return best_period;
38 }
39
40 /* Computes a three-tap pitch predictor */
41 int three_tap_ltp(float *x, int len, int start, int end, float gain[3])
42
43    /*  x:     time-domain signal (note, x[-end] must be valid)
44        len:   length of the x signal
45        start: smallest pitch period possible
46        end:   largest pitch period possible
47        gain:  return value for the pitch predictor gain
48     */
49 {
50    int i, period, best_period=0;
51    float total, score[3]={0,0,0}, best_score=-1, corr[3]={0,0,0}, energy[3]={0,0,0};
52    float best_energy[3], best_corr[3], best_gain=0;
53    float A[3][3];
54    for (period = start; period <= end; period++)
55    {
56       corr[0]=energy[0]=0;
57       for (i=0;i<len;i++)
58       {
59          corr[0] += x[i]*x[i-period];
60          energy[0] += x[i-period]*x[i-period];
61       }
62       score[0] = corr[0]*corr[0]/(energy[0]+1);
63       total = score[0]+2*score[1]+score[2];
64       if (total > best_score && period >= start+3 && period <= end-3)
65       {
66          best_score=total;
67          best_period=period;
68          for (i=0;i<3;i++)
69          {
70             best_corr[i]=corr[i];
71             best_energy[i]=energy[i];
72          }
73          best_gain = corr[1]/(energy[1]+1);
74       }
75       score[2]=score[1];
76       score[1]=score[0];
77       corr[2]=corr[1];
78       corr[1]=corr[0];
79       energy[2]=energy[1];
80       energy[1]=energy[0];
81    }
82    A[0][0]=best_energy[0]+1;
83    A[1][1]=best_energy[1]+1;
84    A[2][2]=best_energy[2]+1;
85    A[0][1]=0;
86    A[0][2]=0;
87    for (i=0;i<len;i++)
88    {
89       A[0][1] += x[i-best_period+1]*x[i-best_period];
90       A[0][2] += x[i-best_period+2]*x[i-best_period];
91    }
92    A[1][0]=A[0][1];
93    A[2][0]=A[0][2];
94    A[1][2]=A[0][1] - x[-best_period+1]*x[-best_period] 
95                    + x[len-best_period]*x[len-best_period+1];
96    A[2][1]=A[1][2];
97    /*for (i=0;i<3;i++)
98       printf ("%f %f %f\n", A[i][0], A[i][1], A[i][2]);
99       printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
100    {
101       float tmp=A[1][0]/A[0][0];
102       for (i=0;i<3;i++)
103          A[1][i] -= tmp*A[0][i];
104       best_corr[1] -= tmp*best_corr[0];
105
106       tmp=A[2][0]/A[0][0];
107       for (i=0;i<3;i++)
108          A[2][i] -= tmp*A[0][i];
109       best_corr[2] -= tmp*best_corr[0];
110       
111       tmp=A[2][1]/A[1][1];
112       A[2][2] -= tmp*A[1][2];
113       best_corr[2] -= tmp*best_corr[1];
114
115       best_corr[2] /= A[2][2];
116       best_corr[1] = (best_corr[1] - A[1][2]*best_corr[2])/A[1][1];
117       best_corr[0] = (best_corr[0] - A[0][2]*best_corr[2] - A[0][1]*best_corr[1])/A[0][0];
118       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
119
120    }
121    gain[0]=best_corr[2];gain[1]=best_corr[1];gain[2]=best_corr[0];
122    
123    if (!((abs(gain[0]) + abs(gain[1]) + abs(gain[2])<2) 
124          && abs(gain[0]+gain[1]+gain[2]) < 1.2 
125          && abs(gain[0]+gain[1]+gain[2]) > -.2 ))
126    {
127       gain[0]=0;gain[1]=best_gain;gain[2]=0;
128       if (best_gain > 1.2)
129          gain[1]=1.2;
130       else if (best_gain<-.2)
131          gain[1]=-.2;
132       else 
133          gain[1]=best_gain;
134    }
135    return best_period-2;
136 }
137
138
139 void predictor_three_tap(float *x, int len, int period, float *gain)
140 {
141    int i;
142    for (i=0;i<len;i++)
143    {
144       x[i] -= gain[0]*x[i-period] + gain[1]*x[i-period-1] + gain[2]*x[i-period-2];
145    }
146 }