1 // f0 -- frequency estimation
2
3 #include <stdio.h>
4
5
6
7 // Estimate a local minimum (or maximum) using parabolic
8 // interpolation. The parabola is defined by the points
9 // (x1,y1),(x2,y2), and (x3,y3).
parabolic_interp(float x1,float x2,float x3,float y1,float y2,float y3,float * min)10 float parabolic_interp(float x1, float x2, float x3, float y1, float y2, float y3, float *min)
11 {
12 float a, b, c;
13 float pos;
14
15 // y1=a*x1^2+b*x1+c
16 // y2=a*x2^2+b*x2+c
17 // y3=a*x3^2+b*x3+c
18
19 // y1-y2=a*(x1^2-x2^2)+b*(x1-x2)
20 // y2-y3=a*(x2^2-x3^2)+b*(x2-x3)
21
22 // (y1-y2)/(x1-x2)=a*(x1+x2)+b
23 // (y2-y3)/(x2-x3)=a*(x2+x3)+b
24
25 a= ((y1-y2)/(x1-x2)-(y2-y3)/(x2-x3))/(x1-x3);
26 b= (y1-y2)/(x1-x2) - a*(x1+x2);
27 c= y1-a*x1*x1-b*x1;
28
29 *min= c;
30
31 // dy/dx = 2a*x + b = 0
32
33 pos= -b/2.0F/a;
34
35 return pos;
36
37 }
38
39
40
f0_estimate(float * samples,int n,int m,float threshold,float * results,float * min)41 float f0_estimate(float *samples, int n, int m, float threshold, float *results, float *min)
42 // samples is a buffer of samples
43 // n is the number of samples, equals twice longest period, must be even
44 // m is the shortest period in samples
45 // results is an array of size n/2 - m + 1, the number of different lags
46 {
47 // work from the middle of the buffer:
48 int middle = n / 2;
49 int i, j; // loop counters
50 // how many different lags do we compute?
51 float left_energy = 0;
52 float right_energy = 0;
53 // for each window, we keep the energy so we can compute the next one
54 // incrementally. First, we need to compute the energies for lag m-1:
55 for (i = 0; i < m - 1; i++) {
56 float left = samples[middle - 1 - i];
57 left_energy += left * left;
58 float right = samples[middle + i];
59 right_energy += right * right;
60 }
61 for (i = m; i <= middle; i++) {
62 // i is the lag and the length of the window
63 // compute the energy for left and right
64 float left = samples[middle - i];
65 left_energy += left * left;
66 float right = samples[middle - 1 + i];
67
68 right_energy += right * right;
69 // compute the autocorrelation
70 float auto_corr = 0;
71 for (j = 0; j < i; j++) {
72 auto_corr += samples[middle - i + j] * samples[middle + j];
73 }
74 float non_periodic = (left_energy + right_energy - 2 * auto_corr);// / i;
75 results[i - m] = non_periodic;
76
77 }
78
79
80 // normalize by the cumulative sum
81 float cum_sum=0.0;
82 for (i = m; i <= middle; i++) {
83 cum_sum+=results[i-m];
84 results[i-m]=results[i-m]/(cum_sum/(i-m+1));
85
86 }
87
88 int min_i=m; // value of initial estimate
89 for (i = m; i <= middle; i++) {
90 if (results[i - m] < threshold) {
91 min_i=i;
92 break;
93 } else if (results[i-m]<results[min_i-m])
94 min_i=i;
95
96 }
97
98
99
100 // use parabolic interpolation to improve estimate
101 float freq;
102 if (i>m && i<middle) {
103 freq=parabolic_interp((float)(min_i-1),(float)(min_i),(float)(min_i+1),
104 results[min_i-1-m],results[min_i-m],results[min_i+1-m], min);
105 //freq=(float)min_i;
106 printf("%d %f\n",min_i,freq);
107 } else {
108 freq=(float)min_i;
109 *min=results[min_i-m];
110 }
111 return freq;
112 }
113
114
115
best_f0(float * samples,int n,int m,float threshold,int Tmax)116 float best_f0(float *samples, int n, int m, float threshold, int Tmax)
117 // samples is a buffer of samples
118 // n is the number of samples, equals twice longest period plus Tmax, must be even
119 // m is the shortest period in samples
120 // threshold is the
121 // results is an array of size n/2 - m + 1, the number of different lags
122 // Tmax is the length of the search
123 {
124 float* results=new float[n/2-m+1];
125 float min=10000000.0;
126 float temp;
127 float best_f0;
128 float f0;
129
130 for (int i=0; i<Tmax; i++) {
131 f0=f0_estimate(&samples[i], n, m, threshold, results, &temp);
132 if (temp<min) {
133 min=temp;
134 best_f0=f0;
135 }
136 }
137 delete[](results);
138 return best_f0;
139 }
140