Working readout for beacon frequency thanks to Dylan!

software/ros_packages/rover_science/src/freq_estimates.py

@@ -1,128 +0,0 @@
-from __future__ import division
-from numpy.fft import rfft
-from numpy import argmax, mean, diff, log
-from matplotlib.mlab import find
-from scipy.signal import blackmanharris, fftconvolve
-from numpy import polyfit, arange
-
-
-def freq_from_crossings(sig, fs):
-    """
-    Estimate frequency by counting zero crossings
-    """
-    # Find all indices right before a rising-edge zero crossing
-    indices = find((sig[1:] >= 0) & (sig[:-1] < 0))
-
-    # Naive (Measures 1000.185 Hz for 1000 Hz, for instance)
-    # crossings = indices
-
-    # More accurate, using linear interpolation to find intersample
-    # zero-crossings (Measures 1000.000129 Hz for 1000 Hz, for instance)
-    crossings = [i - sig[i] / (sig[i+1] - sig[i]) for i in indices]
-
-    # Some other interpolation based on neighboring points might be better.
-    # Spline, cubic, whatever
-
-    return fs / mean(diff(crossings))
-
-
-def freq_from_fft(sig, fs):
-    """
-    Estimate frequency from peak of FFT
-    """
-    # Compute Fourier transform of windowed signal
-    windowed = sig * blackmanharris(len(sig))
-    f = rfft(windowed)
-
-    # Find the peak and interpolate to get a more accurate peak
-    i = argmax(abs(f))  # Just use this for less-accurate, naive version
-    true_i = parabolic(log(abs(f)), i)[0]
-
-    # Convert to equivalent frequency
-    return fs * true_i / len(windowed)
-
-
-def freq_from_autocorr(sig, fs):
-    """
-    Estimate frequency using autocorrelation
-    """
-    # Calculate autocorrelation (same thing as convolution, but with
-    # one input reversed in time), and throw away the negative lags
-    corr = fftconvolve(sig, sig[::-1], mode='full')
-    corr = corr[len(corr)//2:]
-
-    # Find the first low point
-    d = diff(corr)
-    start = find(d > 0)[0]
-
-    # Find the next peak after the low point (other than 0 lag).  This bit is
-    # not reliable for long signals, due to the desired peak occurring between
-    # samples, and other peaks appearing higher.
-    # Should use a weighting function to de-emphasize the peaks at longer lags.
-    peak = argmax(corr[start:]) + start
-    px, py = parabolic(corr, peak)
-
-    return fs / px
-
-
-def freq_from_HPS(sig, fs):
-    """
-    Estimate frequency using harmonic product spectrum (HPS)
-    """
-    windowed = sig * blackmanharris(len(sig))
-
-    from pylab import subplot, plot, log, copy, show
-
-    # harmonic product spectrum:
-    c = abs(rfft(windowed))
-    maxharms = 8
-    subplot(maxharms, 1, 1)
-    plot(log(c))
-    for x in range(2, maxharms):
-        a = copy(c[::x])  # Should average or maximum instead of decimating
-        # max(c[::x],c[1::x],c[2::x],...)
-        c = c[:len(a)]
-        i = argmax(abs(c))
-        true_i = parabolic(abs(c), i)[0]
-        print 'Pass %d: %f Hz' % (x, fs * true_i / len(windowed))
-        c *= a
-        subplot(maxharms, 1, x)
-        plot(log(c))
-    show()
-
-
-def parabolic(f, x):
-    """Quadratic interpolation for estimating the true position of an
-    inter-sample maximum when nearby samples are known.
-
-    f is a vector and x is an index for that vector.
-
-    Returns (vx, vy), the coordinates of the vertex of a parabola that goes
-    through point x and its two neighbors.
-
-    Example:
-    Defining a vector f with a local maximum at index 3 (= 6), find local
-    maximum if points 2, 3, and 4 actually defined a parabola.
-
-    In [3]: f = [2, 3, 1, 6, 4, 2, 3, 1]
-
-    In [4]: parabolic(f, argmax(f))
-    Out[4]: (3.2142857142857144, 6.1607142857142856)
-
-    """
-    xv = 1 / 2. * (f[x - 1] - f[x + 1]) / (f[x - 1] - 2 * f[x] + f[x + 1]) + x
-    yv = f[x] - 1 / 4. * (f[x - 1] - f[x + 1]) * (xv - x)
-    return (xv, yv)
-
-
-def parabolic_polyfit(f, x, n):
-    """Use the built-in polyfit() function to find the peak of a parabola
-
-    f is a vector and x is an index for that vector.
-
-    n is the number of samples of the curve used to fit the parabola.
-    """
-    a, b, c = polyfit(arange(x - n // 2, x + n // 2 + 1), f[x - n // 2:x + n // 2 + 1], 2)
-    xv = -0.5 * b / a
-    yv = a * xv ** 2 + b * xv + c
-    return (xv, yv)