From 4d885deb8b0acd9e8cf005eaf8c5e6d93ee05dd9 Mon Sep 17 00:00:00 2001 From: Soeren Schoenbrod Date: Fri, 7 Oct 2022 15:20:48 +0200 Subject: [PATCH] Add SAMV to estimate DoA --- src/PhasedArray.jl | 6 +++++- src/pattern.jl | 27 +++++++++++++++++++++++++++ src/points_on_sphere.jl | 10 ++++++++++ src/samv.jl | 31 +++++++++++++++++++++++++++++++ test/samv.jl | 40 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 113 insertions(+), 1 deletion(-) create mode 100644 src/points_on_sphere.jl create mode 100644 src/samv.jl create mode 100644 test/samv.jl diff --git a/src/PhasedArray.jl b/src/PhasedArray.jl index 81a963b..4b960c3 100644 --- a/src/PhasedArray.jl +++ b/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 diff --git a/src/pattern.jl b/src/pattern.jl index f8c8cec..968c657 100644 --- a/src/pattern.jl +++ b/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) diff --git a/src/points_on_sphere.jl b/src/points_on_sphere.jl new file mode 100644 index 0000000..d63faea --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/samv.jl b/src/samv.jl new file mode 100644 index 0000000..96d2d0a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/test/samv.jl b/test/samv.jl new file mode 100644 index 0000000..cd103c4 --- /dev/null +++ b/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 \ No newline at end of file