Add SAMV to estimate DoA

src/PhasedArray.jl

@@ -17,6 +17,7 @@ module PhasedArray
         get_steer_vec,
         Pattern,
         Pattern3D,
+        ScatterPattern,
         calc_prewhitening_filter,
         calc_amplitude_filter,
         calc_eigen_beamformer,
@@ -25,7 +26,8 @@ module PhasedArray
         est_doa_by_signal_subspace,
         est_doa_by_noise_subspace,
         est_doa_by_music,
-        get_num_ants
+        get_num_ants,
+        samv2
 
     const cart2sph = SphericalFromCartesian()
     const sph2cart = CartesianFromSpherical()
@@ -39,4 +41,6 @@ module PhasedArray
     include("real_manifold.jl")
     include("pattern.jl")
     include("filter.jl")
+    include("points_on_sphere.jl")
+    include("samv.jl")
 end

src/pattern.jl

@@ -17,6 +17,33 @@ struct Pattern3D{T}
     max_el::Float64
 end
 
+struct ScatterPattern{
+    A<:AbstractVector,
+    V<:AbstractVector
+}
+azs::A
+els::A
+values::V
+max_el::Float64
+end
+
+function ScatterPattern(doas, values::AbstractVector{<:Real})
+    doas_sph = cart2sph.(doas)
+    azs = map(doa -> doa.θ, doas_sph)
+    els = map(doa -> (π / 2 - doa.ϕ) * 180 / π, doas_sph)
+    ScatterPattern(azs, els, values, maximum(els))
+end
+
+@recipe function f(pattern::ScatterPattern;)
+    seriestype := :scatter
+    seriescolor --> :Reds
+    colorbar_title --> "Magnitude"
+    projection --> :polar
+    ylims --> (0, pattern.max_el)
+    marker_z --> pattern.values
+    pattern.azs, pattern.els
+end
+
 function Pattern(manifold, reduce_ant_fun = norm; num_az = 360, num_el = 91, max_el = π / 2)
     azs = range(0, step = 2π/num_az, length = num_az + 1)
     els = range(0, stop = max_el, length = num_el)

src/points_on_sphere.jl

@@ -0,0 +1,10 @@
+# The golden spiral method
+# http://extremelearning.com.au/how-to-evenly-distribute-points-on-a-sphere-more-effectively-than-the-canonical-fibonacci-lattice/
+# min_phi defines the minimum elevation. 90° is the zenith, 0° horizon and -90° the nadir
+function create_points_on_sphere(num_points, min_phi = -20 * π / 180, ϵ = 0.36)
+    map(0:num_points - 1) do index
+        phi = acos(1 - (1 - sin(min_phi)) * (index + ϵ) / (num_points - 1 + 2 * ϵ))
+        theta = π * (1 + sqrt(5)) * index
+        SVector(cos(theta) * sin(phi), sin(theta) * sin(phi), cos(phi))
+    end
+end

src/samv.jl

@@ -0,0 +1,31 @@
+function calc_covariance(A, signal_powers, noise_power)
+    Hermitian(A * Diagonal(signal_powers) * A' + I * noise_power)
+end
+
+function samv2(
+    manifold::AbstractManifold,
+    covariance_matrix::Hermitian;
+    num_doas,
+    min_phi = -10 * π / 180,
+    max_iterations = 100,
+    break_threshold = 1e-3
+)
+    doas = create_points_on_sphere(num_doas, min_phi)
+    steer_vecs = map(doa -> get_steer_vec(manifold, doa), doas)
+    A = reduce(hcat, steer_vecs)
+    signal_powers = map(steer_vec -> abs(steer_vec' * covariance_matrix * steer_vec) / norm(steer_vec)^4, steer_vecs)
+    noise_power = first(eigvals(covariance_matrix))
+    for i = 1:max_iterations
+        estimated_covariance_matrix = calc_covariance(A, signal_powers, noise_power)
+        next_signal_powers = map(signal_powers, steer_vecs) do signal_power, steer_vec
+            abs(signal_power * steer_vec' * (estimated_covariance_matrix \ (covariance_matrix * (estimated_covariance_matrix \ steer_vec))) /
+                (steer_vec' * (estimated_covariance_matrix \ steer_vec)))
+        end
+        noise_power = abs(tr(estimated_covariance_matrix \ (estimated_covariance_matrix \ covariance_matrix)) /
+            tr(inv(estimated_covariance_matrix)^2))
+        normed_diff = norm(signal_powers - next_signal_powers) / norm(signal_powers)
+        signal_powers = next_signal_powers
+        normed_diff < break_threshold && break;
+    end
+    doas, signal_powers
+end

test/samv.jl

@@ -0,0 +1,40 @@
+@testset "SAMV" begin
+    f_0 = 1575420e3
+    c₀ = 299_792_458
+    λ = c₀ / f_0
+    ant_pos = SVector(λ / 4 * SVector(1, 1, 0), λ / 4 * SVector(-1, 1, 0), λ / 4 * SVector(1, -1, 0), λ / 4 * SVector(-1, -1 , 0))
+    num_ants = length(ant_pos)
+    manifold = IdealManifold(f_0, ant_pos)
+
+    signal1_doa_sph = Spherical(1.0, 0.1π, 0.8π / 2)
+    signal2_doa_sph = Spherical(1.0, 1.2π, 0.5π / 2)
+
+    signal1_doa = CartesianFromSpherical()(signal1_doa_sph)
+    signal2_doa = CartesianFromSpherical()(signal2_doa_sph)
+
+    num_samples = 1000
+    signal1_freq = 1.2e6
+    signal2_freq = -0.5e6
+    sampling_freq = 5e6
+    signal = transpose(get_steer_vec(manifold, signal1_doa_sph)) .* cis.(2π * (0:num_samples - 1) * signal1_freq / sampling_freq .+ rand() * 2π) * 15.0 +
+        transpose(get_steer_vec(manifold, signal2_doa_sph)) .* cis.(2π * (0:num_samples - 1) * signal2_freq / sampling_freq .+ rand() * 2π) * 20.0 +
+        randn(ComplexF64, num_samples, num_ants)
+
+    Rxx = Hermitian(transpose(signal) * conj(signal) / num_samples)
+
+    doas, signal_powers = PhasedArray.samv2(manifold, Rxx; num_doas = 1000,)
+
+    found_doas = doas[signal_powers .> 13^2]
+    found_signal_powers = signal_powers[signal_powers .> 13^2]
+    sort_perm = sortperm(found_signal_powers)
+
+    @test acosd(signal2_doa' * found_doas[sort_perm][2]) < 1 # Smaller than 1 degree
+    @test acosd(signal1_doa' * found_doas[sort_perm][1]) < 1.5 # Smaller than 1.5 degree
+
+#=
+    pattern = ScatterPattern(doas, sqrt.(abs.(signal_powers)))
+    plot(pattern)
+    scatter!([signal1_doa.θ], [(π / 2 - signal1_doa.ϕ) * 180 / π], marker = :cross, markerstrokewidth = 4)
+    scatter!([signal2_doa.θ], [(π / 2 - signal2_doa.ϕ) * 180 / π], marker = :cross, markerstrokewidth = 4)
+=#
+end