Add frequency estimators by Jacobsen and Quinn

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)
@@ -38,4 +40,155 @@ function esprit(x::AbstractArray, M::Integer, p::Integer, Fs::Real=1.0)
     angle.(D)*Fs/2π
 end
 
+"""
+    jacobsen(x::Vector, Fs::Real = 1.0)
+
+Estimate the largest frequency (in Hz) in the signal `x` using Jacobsen's
+algorithm [^Jacobsen2007].
+
+[^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::Vector{<:Real}, Fs::Real = 1.0)
+    N = length(x)
+    X = rfft(x)
+    k = argmax(abs.(X))  # index of DFT peak
+    if (k+1 <= N) && (k-1 >= 1)
+        δ = -real((X[k+1] - X[k-1]) / (2X[k] - X[k-1] - X[k+1]) )
+    else
+        δ = 0.0
+    end
+
+    return (k - 1 + δ)*Fs/N
+end
+
+function jacobsen(x::Vector{<:Complex}, Fs::Real = 1.0)
+    N = length(x)
+    X = fftshift(fft(x))
+    k = argmax(abs.(X))  # index of DFT peak
+    f = fftshift(fftfreq(N, Fs))[k]  # frequency at index k
+    if (k+1 <= N) && (k-1 >= 1)
+        δ = -real((X[k+1] - X[k-1]) / (2X[k] - X[k-1] - X[k+1]) )
+    else
+        δ = 0.0
+    end
+
+    f + δ*Fs/N
+end
+
+"""
+    quinn(x::Vector, f0::Real = 0.0, 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 or if it is equal to zero, then a guess
+is calculated by `quinn` using Jacobsen's estimator. The sampling frequency `Fs`
+defaults to `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, Fs), 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
+    T = 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), T)
+    β = zero(eltype(x))
+    iter = 0
+    @inbounds while iter < maxiters
+        iter += 1
+        ξ[1] = x[1]
+        ξ[2] = x[2] + α*ξ[1]
+        for t in 3:T
+            ξ[t] = x[t] + α*ξ[t-1] - ξ[t-2]
+        end
+        β = ξ[2]/ξ[1]
+        for t = 3:T
+            β += (ξ[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
+    T = length(x)
+
+    ω̂ = π*f0/fₙ
+
+    # Remove any DC term in x
+    x .= x .- complex(mean(real.(x)), mean(imag.(x)))
+
+    # iteration
+    ξ = zeros(eltype(x), T)
+    iter = 0
+    @inbounds while iter < maxiters
+        iter += 1
+        # step 2
+        ξ[1] = x[1]
+        for t in 2:T
+            ξ[t] = x[t] + exp(complex(0,ω̂))*ξ[t-1]
+        end
+        # step 3
+        S = let s = 0.0
+                for t=2:T
+                    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,54 @@ using DSP, Test
     frequencies_estimated = sort(esprit(x, M, p, Fs))
     @test isapprox(frequencies', frequencies_estimated; atol = 1e-2)
 end
+
+@testset "jacobsen" 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 = jacobsen(sr, fs)
+    @test isapprox(f_est_real, fr, atol = 0.5)
+    @test jacobsen(zeros(10)) == 0.1
+    # complex input
+    fc = -40.3
+    sc = cis.(2π*fc*t .+ π/1.4)
+    f_est_complex = jacobsen(sc, fs)
+    @test isapprox(f_est_complex, fc, atol = 1e-2)
+    @test jacobsen(zeros(ComplexF64, 10)) == -0.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)
+    ### real input
+    (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) # assumes f0 = 0.0
+    @test maxiter == false
+    @test isapprox(f_est_real, fr, atol = 1e-3)
+    # use default fs
+    (f_est_real, maxiter) = quinn(sr) # assumes fs = 1.0, f0 = 0.0
+    @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) # assumes f0 = 0.0
+    @test maxiter == false
+    @test isapprox(f_est_real, fc, atol = 1e-3)
+    # use default fs
+    (f_est_real, maxiter) = quinn(sc) # assumes fs = 1.0, f0 = 0.0
+    @test maxiter == false
+    @test isapprox(f_est_real, fc/fs, atol = 1e-3)
+end