Add frequency estimators by Jacobsen and Quinn (#503)

    * `jacobsen`, a very fast algorithm that performs a three-point
      curve fit around the DFT peak. Not super accurate.
    * `quinn`, a very accurate iterative algorithm based on ARMA(2,2).
      It requires an initial frequency estimate (which may be
      provided by `jacobsen`).

    Both algorithms work with real and complex signals.

---------

Co-authored-by: Miguel Bazdresch <[email protected]>
Co-authored-by: Martin Holters <[email protected]>
Co-authored-by: Viral B. Shah <[email protected]>

docs/src/estimation.md

@@ -2,4 +2,6 @@
 
 ```@docs
 esprit
+jacobsen
+quinn
 ```

src/estimation.jl

@@ -1,8 +1,10 @@
 module Estimation
 
 using LinearAlgebra: eigen, svd
+using FFTW
+using Statistics: mean
 
-export esprit
+export esprit, jacobsen, quinn
 
 """
     esprit(x::AbstractArray, M::Integer, p::Integer, Fs::Real=1.0)
@@ -41,4 +43,159 @@ function esprit(x::AbstractArray, M::Integer, p::Integer, Fs::Real=1.0)
     angle.(D)*Fs/2π
 end
 
+"""
+    jacobsen(x::AbstractVector, Fs::Real = 1.0)
+
+Estimate the largest frequency in the complex signal `x` using Jacobsen's
+algorithm [^Jacobsen2007]. Argument `Fs` is the sampling frequency.  All
+frequencies are expressed in Hz.
+
+If the signal `x` is real, the estimated frequency is guaranteed to be
+positive, but it may be highly inaccurate (especially for frequencies close to
+zero or to `Fs/2`).
+
+If the sampling frequency `Fs` is not provided, then it is assumed that `Fs =
+1.0`.
+
+[^Jacobsen2007]: E Jacobsen and P Kootsookos, "Fast, Accurate Frequency Estimators", Chapter
+10 in "Streamlining Digital Signal Processing", edited by R. Lyons, 2007, IEEE Press.
+"""
+function jacobsen(x::AbstractVector, Fs::Real = 1.0)
+    N = length(x)
+    X = fft(x)
+    k = argmax(abs.(X)) # index of DFT peak
+    fpeak = fftfreq(N, Fs)[k]  # peak frequency
+    if (k == N)     # k+1 is OOB -- X[k+1] = X[1]
+        Xkm1 = X[N-1]
+        Xkp1 = X[1]
+    elseif (k == 1) # k-1 is OOB -- X[k-1] = X[N]
+        Xkp1 = X[2]
+        Xkm1 = X[N]
+    else
+        Xkp1 = X[k+1]
+        Xkm1 = X[k-1]
+    end
+    δ = -real( (Xkp1 - Xkm1) / (2X[k] - Xkm1 - Xkp1) )
+    estimate = fpeak + δ*Fs/N
+    # if signal is real, return positive frequency
+    if eltype(x) <: Real
+        return abs(estimate)
+    end
+    return estimate
+end
+
+"""
+    quinn(x::Vector, f0::Real, Fs::Real = 1.0 ; tol = 1e-6, maxiters = 20)
+
+    quinn(x::Vector, Fs::Real = 1.0 ; kwargs...)
+
+    quinn(x::Vector ; kwargs...)
+
+Algorithms by Quinn and Quinn & Fernandes for frequency estimation. Given a
+signal `x` and an initial guess `f0`, estimate and return the frequency of the
+largest sinusoid in `x`. `Fs` is the sampling frequency. All frequencies are
+expressed in Hz.
+
+If the initial guess `f0` is not provided, then a guess is calculated using
+Jacobsen's estimator. If the sampling frequency `Fs` is not provided, then it
+is assumed that `Fs = 1.0`.
+
+The following keyword arguments control the algorithm's behavior:
+
+- `tol`: the algorithm stops when the absolut value of the difference between
+   two consecutive estimates is less than `tol`. Defaults to `1e-6`.
+- `maxiters`: the maximum number of iterations to run. Defaults to `20`.
+
+Returns a tuple `(estimate, reachedmaxiters)`, where `estimate` is the
+estimated frequency, and `reachedmaxiters` is `true` if the algorithm finished
+after running for `maxiters` iterations (this may indicate that the algorithm
+did not converge).
+
+If the signal `x` is real, then the algorithm used is [^Quinn1991]. If the signal is
+complex, the algorithm is [^Quinn2009].
+
+[^Quinn1991]: B Quinn and J Fernandes, "A fast efficient technique for the
+estimation of frequency", Biometrika, Vol. 78 (1991).
+
+[^Quinn2009]: B Quinn, "Recent advances in rapid frequency estimation", Digital
+Signal Processing, Vol. 19 (2009), Elsevier.
+
+"""
+quinn(x ; kwargs...) = quinn(x, jacobsen(x, 1.0), 1.0 ; kwargs...)
+
+quinn(x, Fs ; kwargs...) = quinn(x, jacobsen(x, Fs), Fs ; kwargs...)
+
+function quinn(x::Vector{<:Real}, f0::Real, Fs::Real ; tol = 1e-6, maxiters = 20)
+    fₙ = Fs/2
+    N = length(x)
+
+    # Run a quick estimate of largest sinusoid in x
+    ω̂ = π*f0/fₙ
+
+    # remove DC
+    x .= x .- mean(x)
+
+    # Initialize algorithm
+    α = 2cos(ω̂)
+
+    # iteration
+    ξ = zeros(eltype(x), N)
+    β = zero(eltype(x))
+    iter = 0
+    @inbounds while iter < maxiters
+        iter += 1
+        ξ[1] = x[1]
+        ξ[2] = x[2] + α*ξ[1]
+        for t in 3:N
+            ξ[t] = x[t] + α*ξ[t-1] - ξ[t-2]
+        end
+        β = ξ[2]/ξ[1]
+        for t = 3:N
+            β += (ξ[t]+ξ[t-2])*ξ[t-1]
+        end
+        β = β/(ξ[1:end-1]'*ξ[1:end-1])
+        abs(α - β) < tol && break
+        α = 2β-α
+    end
+
+    fₙ*acos(0.5*β)/π, iter == maxiters
+end
+
+function quinn(x::Vector{<:Complex}, f0::Real, Fs::Real ; tol = 1e-6, maxiters = 20)
+    fₙ = Fs/2
+    N = length(x)
+
+    ω̂ = π*f0/fₙ
+
+    # Remove any DC term in x
+    x .= x .- mean(x)
+
+    # iteration
+    ξ = zeros(eltype(x), N)
+    iter = 0
+    @inbounds while iter < maxiters
+        iter += 1
+        # step 2
+        ξ[1] = x[1]
+        for t in 2:N
+            ξ[t] = x[t] + exp(complex(0,ω̂))*ξ[t-1]
+        end
+        # step 3
+        S = let s = 0.0
+                for t=2:N
+                    s += x[t]*conj(ξ[t-1])
+                end
+                s
+            end
+        num = imag(S*cis(-ω̂))
+        den = sum(abs2.(ξ[1:end-1]))
+        ω̂ += 2*num/den
+
+        # stop condition
+        (abs(2*num/den) < tol) && break
+    end
+
+    fₙ*ω̂/π, iter == maxiters
+end
+
 end # end module definition

test/estimation.jl

@@ -19,3 +19,80 @@ using DSP, Test
     frequencies_estimated = sort(esprit(x, M, p, Fs))
     @test isapprox(frequencies', frequencies_estimated; atol = 1e-2)
 end
+
+@testset "jacobsen" begin
+    fs = 100
+    t = range(0, 5, step = 1/fs)
+    # test at two arbitrary frequencies
+    fc = -40.3
+    sc = cis.(2π*fc*t .+ π/1.4)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    fc = 14.3
+    sc = cis.(2π*fc*t .+ π/3)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    # test near fs/2
+    fc = 49.90019
+    sc = cis.(2π*fc*t)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    # test near -fs/2
+    fc = -49.90019
+    sc = cis.(2π*fc*t)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    # test near +zero
+    fc = 0.04
+    sc = cis.(2π*fc*t)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    # test near -zero
+    fc = -0.1
+    sc = cis.(2π*fc*t)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-5)
+    # tests for real signals: test only around fs/4, where the
+    # expected error is small.
+    fr = 28.3
+    sr = cos.(2π*fr*t .+ π/4.2)
+    f_est_real = jacobsen(sr, fs)
+    @test isapprox(f_est_real, fr, atol = 1e-5)
+    fr = 23.45
+    sr = sin.(2π*fr*t .+ 3π/2.2)
+    f_est_real = jacobsen(sr, fs)
+    @test isapprox(f_est_real, fr, atol = 1e-5)
+end
+
+@testset "quinn" begin
+    ### real input
+    fs = 100
+    t = range(0, 5, step = 1/fs)
+    fr = 28.3
+    sr = cos.(2π*fr*t .+ π/4.2)
+    (f_est_real, maxiter) = quinn(sr, 50, fs)
+    @test maxiter == false
+    @test isapprox(f_est_real, fr, atol = 1e-3)
+    # use default initial guess
+    (f_est_real, maxiter) = quinn(sr, fs) # initial guess given by Jacobsen
+    @test maxiter == false
+    @test isapprox(f_est_real, fr, atol = 1e-3)
+    # use default fs
+    (f_est_real, maxiter) = quinn(sr) # fs = 1.0, initial guess given by Jacobsen
+    @test maxiter == false
+    @test isapprox(f_est_real, fr/fs, atol = 1e-3)
+    ### complex input
+    fc = -40.3
+    sc = cis.(2π*fc*t .+ π/1.4)
+    (f_est_real, maxiter) = quinn(sc, -20, fs)
+    @test maxiter == false
+    @test isapprox(f_est_real, fc, atol = 1e-3)
+    # use default initial guess
+    (f_est_real, maxiter) = quinn(sc, fs) # initial guess given by Jacobsen
+    @test maxiter == false
+    @test isapprox(f_est_real, fc, atol = 1e-3)
+    # use default fs
+    (f_est_real, maxiter) = quinn(sc) # fs = 1.0, initial guess by Jacobsen
+    @test maxiter == false
+    @test isapprox(f_est_real, fc/fs, atol = 1e-3)
+end