tone_est_ok.py 文件源码

def tone_est_near_index(sdata,index,range,sr):
    samples = len(sdata)
    fft_size = 2**int(floor(log(samples)/log(2.0)))
    freq = fft(sdata[0:fft_size])
    pdata = numpy.zeros(fft_size)
    for i in xrange(fft_size): pdata[i] = abs(freq[i])
    peak = 0
    peak_index = 0
    for i in xrange(2*range):
        if (pdata[index+i-range] > peak):
            peak = pdata[index+i-range]
            peak_index = index+i-range

    print "peak is at ",peak_index," and is ",peak        

    R = peak*peak;
    p = (freq[peak_index+1].real * freq[peak_index].real + freq[peak_index+1].imag * freq[peak_index].imag)/R

    g = -p/(1.0-p)
    q = (freq[peak_index-1].real * freq[peak_index].real + freq[peak_index-1].imag * freq[peak_index].imag)/R
    e = q/(1.0-q)

    if ((p>0) and (q>0)):
        d = p
    else:
        d = q

    u = peak_index + d

    sum_phase = freq[peak_index-1]*c(-1,d) +    freq[peak_index]*c(0,d) + freq[peak_index+1]*c(1,d)

    sum_c_sq = abs(c(-1,d))*abs(c(-1,d)) +    abs(c(0,d))*abs(c(0,d)) + abs(c(1,d))*abs(c(1,d))

    amp = (abs(sum_phase)/sum_c_sq)/fft_size

    phase_r = cmath.phase(sum_phase)
    freq_est = 0.5*u*sr/fft_size

    return (amp,freq_est,phase_r)