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