freq_from_HPS.py

def freq_from_HPS(sig, fs):
    N = len(sig)
    windowed = sig * blackmanharris(N)
    X = np.abs(rfft(windowed))
    
    hps = X
    n_harmonic_partials = 6
    for h in range(2, n_harmonic_partials):
        # downsample the spectra
        dec = scipy.signal.decimate(X, h)
        hps[:len(dec)] *= dec
    
    # Find the peak and interpolate to get a more accurate peak
    i_peak = np.argmax(hps[:len(dec)])
    i_interp = interpolate(hps, i_peak)[0]
    
    return fs * i_interp / N # Hz