Ksyer/FAR_CS
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add Example

Browse Source
This commit is contained in:
Ksyer
2024-11-11 16:32:53 +08:00
parent a2cb6d1825
commit 29ae9585f9
56 changed files with 4029 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
0 - example/wzk - RDA/angle_estimation.m
+60
View File
@@ -0,0 +1,60 @@
function [ target_list_RDA ] = angle_estimation(target_list_RDA_temp, echo_mtx, A, d, lambda, acc)
% This function estimates all targets angle
%
% Usage:
% [target_list_RDA] = angle_estimation(target_list_RDA_temp, echo_mtx, A, d, lambda, acc)
%
% Inputs:
% target_list_RDA_temp: Temporary list of targets before angle estimation
% echo_mtx: Original echo data matrix
% A: Chirp measurement matrix
% d: Antenna spacing
% lambda: Wave length
% acc: Accuracy for angle estimation
%
% Outputs:
% target_list_RDA: Target list include range, doppler and angle information
% parameters
if nargin < 6
acc = 181;
end
% calculate all targets angle
target_list_RDA = zeros(size(target_list_RDA_temp));
for i = 1: size(target_list_RDA_temp, 1)
t_rda = target_list_RDA_temp(i, :);
target_list_RDA(i, 1:2) = t_rda(1:2);
target_list_RDA(i, 3) = cal_angle(t_rda(1), t_rda(2), echo_mtx, A, d, lambda, acc);
end
end
%% sub-functions
% calculate target actual angle with range and doppler index
function [ angle ] = cal_angle(r_idx, d_idx, echo_mtx, A, d, lambda, acc)
% range doppler processing
echo_r = zeros([size(echo_mtx, 1), 1, size(echo_mtx, 3)]);
for i = 1 : size(echo_mtx, 3)
echo_r(:, 1, i) = sum(bsxfun(@times, echo_mtx(:, :, i), A(:, r_idx)'), 2);
end
echo_r = squeeze(echo_r);
echo_r_ex = [echo_r, zeros(size(echo_r))];
echo_rd = zeros(size(echo_r_ex, 1), 1);
for i = 1 : size(echo_r_ex, 1)
temp_v = fftshift(fft(echo_r_ex(i,:)));
echo_rd(i) = temp_v(d_idx);
end
% CBF
THETA = linspace(-90, 90, round(acc));
fai = exp((0: size(echo_r_ex, 1) - 1)' *...
-1j * 2 * pi / lambda * d * sin(THETA / 180 * pi));
angle_mf = abs(fai'* echo_rd);
[v, idx] = max(angle_mf);
angle = THETA(idx);
end
0 - example/wzk - RDA/doppler_process_CS.m
+200
View File
@@ -0,0 +1,200 @@
function [ echo_rd_mtx, stat_RD, sigma_n_o ] = doppler_process_CS(echo_r_mtx, sigma_n, lambda, gamma, delta)
% Doppler processing for echo data using compressed sensing
%
% Usage:
% [echo_rd_mtx, stat_RD, sigma_n_o] = doppler_process_CS(echo_r_mtx, sigma_n, lambda, gamma, delta)
%
% Inputs:
% echo_r_mtx: Range processing echo data
% sigma_n: Input noise standard deviation
% lambda: LASSO weight
% gamma: Compressed ratio(0.5)
% delta: Convergence normalized difference(1e-6)
%
% Outputs:
% echo_rd_mtx: Range-doppler processing echo data
% stat_RD: Range-doppler statistics
% sigma_n_o: Output noise standard deviation
% parameters
if nargin < 5
delta = 1e-6;
end
if nargin < 4
gamma = 0.5;
end
[lenA, lenR, M] = size(echo_r_mtx);
N = round(M / gamma);
iter_max_VAMP = 1000;
lambda_v = zeros(N, 1) + lambda;
% generate mtx
F_ori = dftmtx(N);
F = F_ori(1:M,:);
F_inv = conj(F) / N;
% normalization
A = (sqrt(N) * eye(M)) * F_inv;
echo_r_mtx = sqrt(N) .* echo_r_mtx;
sigma_n = sqrt(N) * sigma_n;
% doppler processing
echo_rd_mtx = zeros(lenA, lenR, N);
stat_RD = zeros(lenA, lenR, N);
sigma_n_o_cnt = zeros(lenA, lenR);
for numA = 1: lenA
parfor numR = 1: lenR
sample = squeeze(echo_r_mtx(numA, numR, :));
y = sample;
x_LASSO = cVAMPro(y, A, lambda_v, delta, iter_max_VAMP);
[x_d_CROD, sigma_CROD] = CROD(y, A, x_LASSO, lambda, sigma_n);
sigma_n_o_cnt(numA, numR) = abs(sigma_CROD);
stat_RD(numA, numR, :) = abs(fftshift(x_d_CROD) / sigma_CROD).^2;
echo_rd_mtx(numA, numR, :) = fftshift(x_d_CROD);
end
end
% sigma_n_o = mean(sigma_n_o_cnt, 2);
sigma_n_o = mean(mean(sigma_n_o_cnt));
end
%% sub-functions
% algorithm for LASSO
% y: measurements
% A: measurement matrix
% lambda: LASSO weight
% tau: convergence normalized difference
% Kit: maximum number of iterations
% LASSO estimator
function x_hat_wl = cVAMPro(y, A, lambda, tau, Kit)
% Initialization
[M, N] = size(A);
gamma = M / N;
k = 0;
p = ctranspose(A) * y;
h_1 = p;
Q_1 = gamma;
tau_d = 1;
% Iteration
while ((k < Kit) && (tau_d > tau))
% Factorized Part
x_1 = ST(h_1, lambda, Q_1);
chi_1 = F1(x_1, lambda, Q_1);
% Message Passing
h_2 = x_1 / chi_1 - h_1;
Q_2 = 1 / chi_1 - Q_1;
% Gaussian Part
t1 = (p + h_2) / Q_2;
t2 = ctranspose(A) * (A * (p + h_2)) / ((Q_2 + 1) * Q_2);
x_2 = t1 - t2;
chi_2 = gamma / (Q_2 + 1) + (1 - gamma) / Q_2;
% Message Passing
h_1_next = x_2 ./ chi_2 - h_2;
Q_1_next = 1 / chi_2 - Q_2;
tau_d = norm(h_1_next - h_1, Inf) / norm(h_1_next, Inf);
k = k + 1;
% output
x_hat_wl = x_1;
% next
h_1 = h_1_next;
Q_1 = Q_1_next;
end
end
% soft threshold function
% x: processing object
% thd: threshold
% y: result
function x = ST(h_1, lambda, Q_1)
[N, M] = size(h_1);
x = zeros(N, M);
for i = 1:N
sign = h_1(i) ./ abs(h_1(i));
diff = abs(h_1(i)) - lambda(i);
x(i) = sign .* (diff ./ Q_1) .* SF(diff);
end
end
% Heaviside's step function
function v = SF(a)
if a > 0
v = 1;
elseif a == 0
v = 0; % at zero points
else
v = 0;
end
end
% Calculation of chi_1
function chi_1 = F1(x_1, lambda, Q_1)
[N, M] = size(x_1);
count = 0;
for i = 1:N
temp = Q_1 * abs(x_1(i)) + lambda(i);
count = count + (2 - lambda(i) / temp) * SF(abs(x_1(i)));
% count = count + (2-lambda(i)/temp) * (abs(x_1(i)) > 1e-4);
end
chi_1 = count / (2 * N * Q_1);
end
% calculate debiased LASSO estimator
% y: measurements
% A: measurement matrix
% x_LASSO: LASSO estimator
% lambda: LASSO weight
% sigma_n: input noise standard deviation
% x_d_CROD: debiased LASSO estimator
% sigma_CROD: equivalent noise standard deviation estimator
function [ x_d_CROD, sigma_CROD ] = CROD(y, A, x_LASSO, lambda, sigma_n)
[m, n] = size(A);
gamma = m / n;
rho_active = sum(abs(x_LASSO) > 1e-3)/n;
Q_hat = (gamma - rho_active)/(1 - rho_active);
Rho = sum((abs(x_LASSO) > 1e-3).* (2 - lambda./(Q_hat*abs(x_LASSO) + lambda))) / 2 / n;
diff = 1;
while(diff > 1e-4)
Rho_pre = Rho;
Rho = sum((abs(x_LASSO) > 1e-3).* (2 - lambda./((gamma-Rho)/(1-Rho)*abs(x_LASSO) + lambda))) / 2 / n;
diff = abs(Rho - Rho_pre);
end
Q_hat = (gamma-Rho)/(1-Rho);
x_d_CROD = x_LASSO + A'*(y - A*x_LASSO)/Q_hat;
RSS = sum(abs(y - A * x_LASSO).^2)/m;
chi = Rho*(1 - Rho)/(gamma - Rho);
if chi ~= 0
chi_temp = sqrt((chi+1)*(chi+1)-4*gamma*chi);
z = -(1 - chi + chi_temp) / (2*chi);
z_prime = -(1 - 2*gamma*chi + chi + chi_temp) / (2*chi*chi*chi_temp);
G_prime = (z + 1/chi);
G_wprime = (z_prime + 1/chi/chi);
chi_hat = gamma/2*G_wprime*RSS/(G_prime - chi*G_wprime)...
+ (G_prime*G_prime/2 - gamma/2*G_wprime)*sigma_n*sigma_n/(G_prime - chi*G_wprime);
else
G_prime = gamma;
G_wprime = gamma*(1-gamma);
chi_hat = gamma/2*G_wprime*RSS/(G_prime - chi*G_wprime)...
+ (G_prime*G_prime/2 - gamma/2*G_wprime)*sigma_n*sigma_n/(G_prime - chi*G_wprime);
end
sigma_CROD = sqrt(2*chi_hat) / Q_hat;
end
0 - example/wzk - RDA/doppler_process_MF.m
+45
View File
@@ -0,0 +1,45 @@
function [ echo_rd_mtx, stat_RD, sigma_n_o ] = doppler_process_MF(echo_r_mtx, sigma_n, gamma)
% Doppler processing for echo data using matching filter
%
% Usage:
% [echo_rd_mtx, stat_RD, sigma_n_o] = doppler_process_MF(echo_r_mtx, sigma_n, gamma)
%
% Inputs:
% echo_r_mtx: Range processing echo data
% sigma_n: Input noise standard deviation
% gamma: Compressed ratio(0.5)
%
% Outputs:
% echo_rd_mtx: Range-doppler processing echo data
% stat_RD: Range-doppler statistics
% sigma_n_o: Output noise standard deviation
% parameters
if nargin < 3
gamma = 0.5;
end
[lenA, lenR, M] = size(echo_r_mtx);
N = round(M / gamma);
% generate mtx
F_ori = dftmtx(N);
F = F_ori(1:M,:);
multiple_d = F(:,1)' * F(:,1);
% doppler matched filtering
echo_rd_mtx = zeros(lenA, lenR, N);
for numA = 1: lenA
for numR = 1: lenR
sample = squeeze(echo_r_mtx(numA, numR, :));
dpl_temp = transpose(F) * sample;
dpl_norm = fftshift(dpl_temp ./ multiple_d);
echo_rd_mtx(numA, numR, :) = dpl_norm;
end
end
% calculate output noise
sigma_n_o = sqrt(sigma_n^2 / multiple_d);
stat_RD = abs(echo_rd_mtx ./ sigma_n_o).^2;
end
0 - example/wzk - RDA/echo_processing_CS.m
+56
View File
@@ -0,0 +1,56 @@
function [ echo_rd_mtx, target_list_RDA, sigma_n_rd ] = ...
echo_processing_CS( echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d )
% This function processes echo data using compressed sensing method.
%
% Usage:
% [echo_rd_mtx, target_list_RDA, sigma_n_rd] = echo_processing_MF(echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d)
%
% Inputs:
% echo_mtx: Matrix containing the echo data
% PRF: Pulse Repetition Frequency
% fs: Sampling frequency
% fc: Carrier frequency
% B: Bandwidth
% D: Duty ratio
% d: Antenna spacing
% sigma_n: Input noise level of the echo data
% P_fa: False alarm probability threshold
% lambda_r: LASSO weight for range processing
% lambda_d: LASSO weight for doppler processing
%
% Outputs:
% echo_rd_mtx: Echo matrix after range-Doppler processing
% target_list_RDA: List of detected targets
% sigma_n_rd: Estimated noise level after range-Doppler processing
% parameters
c = 3e8;
lambda = c / fc;
[num_antenna, N, num_pluse] = size(echo_mtx);
% data processing
[distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, num_pluse);
[echo_sFFT_mtx, angle_v] = spatial_FFT(echo_mtx, d, lambda);
fprintf(' Spatial_FFT done.\n');
[A, signal_t] = generate_chirp_mtx(PRF, B, fs, D);
fprintf(' Generate_chirp_mtx done.\n');
[echo_r_mtx, sigma_n_o] = range_process_CS(echo_sFFT_mtx, A, sigma_n, lambda_r);
fprintf(' Range_processing done.\n');
[echo_rd_mtx, stat_RD, sigma_n_rd] = doppler_process_CS(echo_r_mtx, sigma_n_o, lambda_d);
fprintf(' Doppler_processing done.\n');
[target_list_RDA_temp, target_map] = rda_detection(stat_RD, P_fa);
fprintf(' Target_detection done.\n');
[target_list_RDA] = angle_estimation(target_list_RDA_temp, echo_mtx, A, d, lambda);
fprintf(' Angle_estimation done.\n');
target_list_RDA(:, 1) = distance_v(target_list_RDA(:, 1));
target_list_RDA(:, 2) = speed_v(target_list_RDA(:, 2));
end
0 - example/wzk - RDA/echo_processing_CS_v0.m
+28
View File
@@ -0,0 +1,28 @@
function [ echo_rd_mtx, target_list_RDA, sigma_n_rd ] = ...
echo_processing_CS_v0( echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d )
% parameters
c = 3e8;
lambda = c / fc;
[num_antenna, N, num_pluse] = size(echo_mtx);
% data processing
[distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, num_pluse);
[echo_sFFT_mtx, angle_v] = spatial_FFT(echo_mtx, d, lambda);
[A, signal_t] = generate_chirp_mtx(PRF, B, fs, D);
[echo_r_mtx, sigma_n_o] = range_process_CS(echo_sFFT_mtx, A, sigma_n, lambda_r);
[echo_rd_mtx, stat_RD, sigma_n_rd] = doppler_process_CS(echo_r_mtx, sigma_n_o, lambda_d); %差个方差
[target_list_RDA_temp, target_map] = rda_detection(stat_RD, P_fa);
target_list_RDA = target_list_RDA_temp;
target_list_RDA(:, 1) = distance_v(target_list_RDA(:, 1));
target_list_RDA(:, 2) = speed_v(target_list_RDA(:, 2));
target_list_RDA(:, 3) = angle_v(target_list_RDA(:, 3));
end
0 - example/wzk - RDA/echo_processing_MF.m
+54
View File
@@ -0,0 +1,54 @@
function [ echo_rd_mtx, target_list_RDA, sigma_n_rd ] =...
echo_processing_MF( echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa )
% This function processes echo data using matching filter method.
%
% Usage:
% [echo_rd_mtx, target_list_RDA, sigma_n_rd] = echo_processing_MF(echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa)
% Inputs:
% echo_mtx: Matrix containing the echo data
% PRF: Pulse Repetition Frequency
% fs: Sampling frequency
% fc: Carrier frequency
% B: Bandwidth
% D: Duty ratio
% d: Antenna spacing
% sigma_n: Input noise level of the echo data
% P_fa: False alarm probability threshold
%
% Outputs:
% echo_rd_mtx: Echo matrix after range-Doppler processing
% target_list_RDA: List of detected targets
% sigma_n_rd: Estimated noise level after range-Doppler processing
% parameters
c = 3e8;
lambda = c / fc;
[num_antenna, N, num_pluse] = size(echo_mtx);
% data processing
[distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, num_pluse);
[echo_sFFT_mtx, angle_v] = spatial_FFT(echo_mtx, d, lambda);
fprintf(' Spatial_FFT done.\n');
[A, signal_t] = generate_chirp_mtx(PRF, B, fs, D);
fprintf(' Generate_chirp_mtx done.\n');
[echo_r_mtx, sigma_n_o] = range_process_MF(echo_sFFT_mtx, A, sigma_n);
fprintf(' Range_processing done.\n');
[echo_rd_mtx, stat_RD, sigma_n_rd] = doppler_process_MF(echo_r_mtx, sigma_n_o);
fprintf(' Doppler_processing done.\n');
[target_list_RDA_temp, target_map] = rda_detection(stat_RD, P_fa);
fprintf(' Target_detection done.\n');
[target_list_RDA] = angle_estimation(target_list_RDA_temp, echo_mtx, A, d, lambda);
fprintf(' Angle_estimation done.\n');
target_list_RDA(:, 1) = distance_v(target_list_RDA(:, 1));
target_list_RDA(:, 2) = speed_v(target_list_RDA(:, 2));
end
0 - example/wzk - RDA/echo_processing_MF_v0.m
+28
View File
@@ -0,0 +1,28 @@
function [ echo_rd_mtx, target_list_RDA, sigma_n_rd ] =...
echo_processing_MF_v0( echo_mtx, PRF, fs, fc, B, D, d, sigma_n, P_fa )
% parameters
c = 3e8;
lambda = c / fc;
[num_antenna, N, num_pluse] = size(echo_mtx);
% data processing
[distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, num_pluse);
[echo_sFFT_mtx, angle_v] = spatial_FFT(echo_mtx, d, lambda);
[A, signal_t] = generate_chirp_mtx(PRF, B, fs, D);
[echo_r_mtx, sigma_n_o] = range_process_MF(echo_sFFT_mtx, A, sigma_n);
[echo_rd_mtx, stat_RD, sigma_n_rd] = doppler_process_MF(echo_r_mtx, sigma_n_o); %差个方差
[target_list_RDA_temp, target_map] = rda_detection(stat_RD, P_fa);
target_list_RDA = target_list_RDA_temp;
target_list_RDA(:, 1) = distance_v(target_list_RDA(:, 1));
target_list_RDA(:, 2) = speed_v(target_list_RDA(:, 2));
target_list_RDA(:, 3) = angle_v(target_list_RDA(:, 3));
end
0 - example/wzk - RDA/generate_chirp_mtx.m
+81
View File
@@ -0,0 +1,81 @@
function [ A, signal_t ] = generate_chirp_mtx(PRF, B, fs, D, gamma, sign_mid)
% Generates a chirp measurement matrix
%
% Usage:
% [A, signal_t] = generate_chirp_mtx(PRF, B, fs, D, gamma, sign_mid)
%
% Inputs:
% PRF: Pulse Repetition Frequency
% B: Bandwidth
% fs: Sampling frequency
% D: Duty ratio
% gamma: Compression ratio
% sign_mid: if sign_mid is 0 means the initial frequency is 0,
% if sign_mid is 1 means the initial frequency is -B/2,
%
% Outputs:
% A: Chirp measurement matrix
% signal_t: Transmitting signal
% parameters
if nargin < 5
gamma = 0.5;
end
if nargin < 6
sign_mid = 0;
end
Tr = 1 / PRF;
Tp = Tr * D;
K = B / Tp;
% generate_signal
N = Tr * fs;
N_high = Tp * fs;
N_mtx = round(N / gamma);
signal_t = zeros(1, N);
for i = 1: N_high
tp = i * (1 / fs) - sign_mid * N_high / fs / 2;
signal_t(1, i) = exp(1j * 2 * pi * 0.5 * K * tp .^ 2);
end
signal_t_2fs = zeros(1, N_mtx);
for i = 1: round(N_high / gamma)
tp = (i+1) * (1 / fs / 2) - sign_mid * N_high / fs / 2;
signal_t_2fs(1, i) = exp(1j * pi * K * tp .^ 2);
end
% generate chirp matrix
A = generate_matrix_by_signal2fs(transpose(signal_t_2fs));
end
%% sub-functions
% generate chirp matrix when gamma=0.5
function [ mtx ] = generate_matrix_by_signal2fs(signal)
mtx = [];
l = round(length(signal) / 2);
temp1 = signal(1:2:end);
temp2 = circshift(signal(2:2:end), 1);
if length(temp2) ~= length(temp1)
temp2 = [temp2; 0];
end
for i = 1:l
t1 = circshift(temp1, i-1);
t2 = circshift(temp2, i-1);
if i - 1 > 0
t1(1:i - 1,1) = 0;
end
if i - 1 > 0
t2(1:i - 1,1) = 0;
end
mtx = [mtx,t1,t2];
end
end
0 - example/wzk - RDA/get_range_speed_val.m
+35
View File
@@ -0,0 +1,35 @@
function [distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, numP, gamma_r, gamma_d)
% Calculates range and speed values for node index
%
% Usage:
% [distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, numP, gamma_r, gamma_d)
%
% Inputs:
% PRF: Pulse Repetition frequency
% fs: Sampling frequency
% fc: Carrier frequency
% numP: Number of pulses
% gamma_r: Range compression ratio(0.5)
% gamma_d: Doppler compression ratio(0.5)
%
% Outputs:
% distance_v: Real distance value
% speed_v: Real speed value
% parameters
if nargin < 6
gamma_d = 0.5;
end
if nargin < 5
gamma_r = 0.5;
end
c = 3e8;
Tr = 1 / PRF;
lambda = c / fc;
% calculate
distance_v = 0: (c / fs / 2 * gamma_r) : (Tr * c / 2 - c / fs / 2 * gamma_r);
speed_v = -(-PRF / 2: PRF / numP * gamma_d: PRF / 2 - PRF / numP * gamma_d) * lambda / 2 ;
end
0 - example/wzk - RDA/main_CS.m
+81
View File
@@ -0,0 +1,81 @@
clc
clear
close all
%% load echo data
filename = 'Raw_Echo_5dB';
load(['./data/', filename, '.mat']);
%% parameters
% ladar
PRF = 5000;
B = 5e6;
D = 0.1;
Tp = 2e-5;
fs = 5e6;
fc = 1.25e9;
% antenna
num_antenna = 18;
d = 0.12;
% echo
num_pulse = 64;
sigma_n = 0.1;
% detect
P_fa = 1e-6;
% algorithm
gamma_r = 0.5;
gamma_d = 0.5;
lambda_r = 0.005;
lambda_d = 0.15;
% accurate angle
sign_AA = 1;
%% data processing
fprintf('Using CS\n');
fprintf(['File: ', filename, '\n']);
tic;
if sign_AA == 0
[ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
echo_processing_CS_v0( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d );
else
[ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
echo_processing_CS( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d );
end
toc;
%% plot
Fontsize = 18;
plot_width = 800;
plot_height = 600;
Linewidth = 2;
Markersize = 8;
figure(1)
scatter3(target_list_RDA(:,1), target_list_RDA(:,2),...
target_list_RDA(:,3), 'filled', 'o')
xlabel('Range(m)');
ylabel('Speed(m/s)');
zlabel('Angle(°)');
zlim([-90, 90]);
ylim([80, 150]);
xlim([17000, 19000]);
set(gca, 'FontSize', Fontsize);
title('node')
set(gcf, 'position', [100, 200, plot_width+100, plot_height+50]);
set(gca,'fontsize',18,'fontname','Times');
%% save
save(['./output/', filename, '_CS_Pfa', num2str(P_fa), '.mat'],...
'target_list_RDA', 'echo_rd_mtx', 'sigma_n_out', 'P_fa');
fprintf(['[', filename, '_CS_Pfa', num2str(P_fa), '.mat]', ' saved.\n\n']);
0 - example/wzk - RDA/main_MF.m
+79
View File
@@ -0,0 +1,79 @@
clc
clear
close all
%% load echo data
filename = 'Raw_Echo_5dB';
load(['./data/', filename, '.mat']);
%% parameters
% ladar
PRF = 5000;
B = 5e6;
D = 0.1;
Tp = 2e-5;
fs = 5e6;
fc = 1.25e9;
% antenna
num_antenna = 18;
d = 0.12;
% echo
num_pulse = 64;
sigma_n = 0.1;
% detect
P_fa = 1e-6;
% algorithm
gamma_r = 0.5;
gamma_d = 0.5;
% accurate angle
sign_AA = 1;
%% data processing
fprintf('Using MF\n');
fprintf(['File: ', filename, '\n']);
tic;
if sign_AA == 0
[ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
echo_processing_MF_v0( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa );
else
[ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
echo_processing_MF( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa );
end
toc;
%% plot
Fontsize = 18;
plot_width = 800;
plot_height = 600;
Linewidth = 2;
Markersize = 8;
figure(1)
scatter3(target_list_RDA(:,1), target_list_RDA(:,2),...
target_list_RDA(:,3), 'filled', 'o')
xlabel('Range(m)');
ylabel('Speed(m/s)');
zlabel('Angle(°)');
zlim([-90, 90]);
ylim([80, 150]);
xlim([17000, 19000]);
set(gca, 'FontSize', Fontsize);
title('node')
set(gcf, 'position', [100, 200, plot_width+100, plot_height+50]);
set(gca,'fontsize',18,'fontname','Times');
%% save
save(['./output/', filename, '_MF_Pfa', num2str(P_fa), '.mat'],...
'target_list_RDA', 'echo_rd_mtx', 'sigma_n_out', 'P_fa');
fprintf(['[', filename, '_MF_Pfa', num2str(P_fa), '.mat]', ' saved.\n\n']);
0 - example/wzk - RDA/plot_result.m
+38
View File
@@ -0,0 +1,38 @@
clc
clear
close all
Fontsize = 18;
plot_width = 800;
plot_height = 600;
Linewidth = 2;
Markersize = 8;
Pfa_set = '1e-06';
method = 'CS';
SNR = 5;
filename = ['Raw_Echo_', num2str(SNR), 'dB_', method, '_Pfa', Pfa_set];
load(['./output/', filename, '.mat']);
%%
figure(1)
scatter3(target_list_RDA(:,1), target_list_RDA(:,2),...
target_list_RDA(:,3), 'filled', 'o')
xlabel('Range(m)');
ylabel('Speed(m/s)');
zlabel('Angle(°)');
zlim([-90, 90]);
% ylim([80, 150]);
% xlim([17000, 19000]);
set(gca, 'FontSize', Fontsize);
title('node')
set(gcf, 'position', [100, 200, plot_width+100, plot_height+50]);
set(gca,'fontsize',18,'fontname','Times');
0 - example/wzk - RDA/range_process_CS.m
+155
View File
@@ -0,0 +1,155 @@
function [ echo_r_mtx, sigma_n_o ] = range_process_CS(echo_mtx, A, sigma_n, lambda, delta)
% Range processing for echo data using compressed sensing
%
% Usage:
% [echo_r_mtx, sigma_n_o] = range_process_CS(echo_mtx, A, sigma_n, lambda, delta)
%
% Inputs:
% echo_mtx: Original echo data
% A: Chirp measurement matrix
% sigma_n: Input noise standard deviation
% lambda: LASSO weight
% delta: Convergence normalized difference(2e-7)
%
% Outputs:
% echo_r_mtx: Range processing echo data
% sigma_n_o: Output noise standard deviation
% parameters
if nargin < 5
delta = 2e-7;
end
[M, N] = size(A);
[lenA, M, lenP] = size(echo_mtx);
% normalization
J1 = A*A';
lambda_J=eig(J1);
A = A / sqrt(lambda_J(end));
echo_mtx = echo_mtx ./ sqrt(lambda_J(end));
sigma_n = sigma_n / sqrt(lambda_J(end));
% compressed sensing
echo_r_mtx = zeros(lenA, N, lenP);
sigma_n_o_cnt = zeros(lenA, lenP);
for numA = 1: lenA
parfor numP = 1: lenP
sr = echo_mtx(numA, :, numP);
y = transpose(sr);
% LASSO
x_FISTA = FISTA(y, A, lambda, delta);
% debiased LASSO
[x_d, sigma_w] = cal_debiased_LASSO(x_FISTA, A, y, lambda, sigma_n);
sigma_n_o_cnt(numA, numP) = abs(sigma_w);
echo_r_mtx(numA, :, numP) = x_d;
end
end
% calculate output noise
sigma_n_o = mean(mean(sigma_n_o_cnt));
end
%% sub-functions
% algorithm for LASSO
% y: measurements
% A: measurement matrix
% lambda: LASSO weight
% delta: convergence normalized difference
% z: LASSO estimator
function [z] = FISTA(y, A, lambda, delta)
x_pre = A'*y;
t = 1;
z = x_pre;
z_pre = z;
t_pre = t;
N = size(A, 2);
diff = 1;
E = eig(A'*A);
L = E(end);
temp1 = A'*y/L;
temp2 = eye(N) - A'*A/L;
k = 0;
while((diff > delta) && (k < 1000))
temp = temp1 + temp2 * z_pre;
x = sft_thd(temp, lambda/L);
t = 0.5*(1 + sqrt(1+4*t_pre*t_pre));
z = x + (x - x_pre) * (t_pre-1) / t;
diff = mean(abs(z_pre - z));
x_pre = x;
z_pre = z;
t_pre = t;
k = k + 1;
end
end
% soft threshold function
% x: processing object
% thd: threshold
% y: result
function y = sft_thd(x, thd)
if isequal(size(x), size(thd))
tmp = abs(x);
y = x;
y(tmp <= thd) = 0;
y(tmp > thd) = (tmp(tmp > thd) - thd(tmp > thd)) .* x(tmp > thd) ./ tmp(tmp > thd);
else
tmp = abs(x);
y = x;
y(tmp <= thd) = 0;
y(tmp > thd) = (tmp(tmp > thd) - thd) .* x(tmp > thd) ./ tmp(tmp > thd);
end
end
% calculate debiased LASSO estimator
% x: LASSO estimator
% A: measurement matrix
% y: measurements
% lambda: LASSO weight
% sigma_n: input noise standard deviation
% x_d: debiased LASSO estimator
% sigma_d: equivalent noise standard deviation
function [x_d, sigma_d] = cal_debiased_LASSO(x, A, y, lambda, sigma)
[M, N] = size(A);
gamma = M/N;
hat_Q1 = gamma;
[~, D] = eig(A'*A);
d = diag(D);
diff = 1;
T = 1000;
t = 0;
while (t < T) && (diff > 1e-6)
Q1_pre = hat_Q1;
rho = mean((2 - lambda./(hat_Q1*abs(x) + lambda)).*(abs(x) > 1e-4))/2;
hat_Q1 = rho/mean(1./(d + (1-rho)*hat_Q1/rho));
diff = abs(Q1_pre - hat_Q1);
t = t+1;
end
x_d = x + 1/hat_Q1*A'*(y - A*x);
chi = rho/hat_Q1;
hat_Q2 = 1/chi - hat_Q1;
t = -hat_Q2;
t_prime = -1/mean((1./(d+hat_Q2)).^2);
G_prime = t + 1/chi;
G_wprime = t_prime + 1/chi/chi;
RSS = sum(abs(y - A*x).^2)/M;
hat_chi = gamma*G_wprime/(2*G_prime-2*chi*G_wprime)*RSS +...
(-G_wprime*gamma+G_prime*G_prime)/(2*G_prime-2*chi*G_wprime)*sigma^2;
sigma_d = sqrt(2*hat_chi)/hat_Q1;
end
0 - example/wzk - RDA/range_process_MF.m
+36
View File
@@ -0,0 +1,36 @@
function [ echo_r_mtx, sigma_n_o ] = range_process_MF(echo_mtx, A, sigma_n)
% Range processing for echo data using matching filter
%
% Usage:
% [echo_r_mtx, sigma_n_o] = range_process_MF(echo_mtx, A, sigma_n)
%
% Inputs:
% echo_mtx: Original echo data
% A: Chirp measurement matrix
% sigma_n: Input noise standard deviation
%
% Outputs:
% echo_r_mtx: Range processing echo data
% sigma_n_o: Output noise standard deviation
% parameters
multiple_r = A(:, 1)' * A(:, 1);
[M, N] = size(A);
[lenA, M, lenP] = size(echo_mtx);
% matched filtering
echo_r_mtx = zeros(lenA, N, lenP);
for numA = 1: lenA
for numP = 1: lenP
sr = echo_mtx(numA, :, numP);
mf_res = A' * transpose(sr);
mf_norm = mf_res ./ multiple_r;
echo_r_mtx(numA, :, numP) = mf_norm;
end
end
% calculate output noise
sigma_n_o = sqrt(sigma_n^2 / multiple_r);
end
0 - example/wzk - RDA/rda_detection.m
+47
View File
@@ -0,0 +1,47 @@
function [ target_list_RDA, target_map ] = rda_detection(stat_RD, P_fa)
% Target detection, estimating distance, velocity, and angle intervals
%
% Usage:
% [target_list_RDA, target_map] = rda_detection(stat_RD, P_fa)
%
% Inputs:
% stat_RD: Range-Doppler statistics
% P_fa: Probability of false alarm
%
% Outputs:
% target_list_RDA: Target list include range, doppler and angle information
% target_map: Show targets in RD map
% parameters
[lenA, lenR, lenP] = size(stat_RD);
target_map = zeros(lenR, lenP);
% detection threshold
d_thd = chi2inv(1 - P_fa, 2) / 2;
% find target
target_list_RD = [];
for i = 1: lenA
RD_map = squeeze(stat_RD(i, :, :));
detect_map = zeros(size(RD_map));
detect_map(RD_map > d_thd) = 1;
% target_map(i, :, :) = detect_map;
[r, c] = find(detect_map);
temp_node = [r, c];
target_list_RD = [target_list_RD; temp_node];
end
target_list_RD_new = unique(target_list_RD, 'rows');
% estimate target angle interval
target_list_angle = [];
for i = 1: size(target_list_RD_new, 1)
tgt = target_list_RD_new(i, :);
stat_vct = stat_RD(:, tgt(1), tgt(2));
[v, idx] = max(stat_vct);
target_map(tgt(1), tgt(2)) = idx;
target_list_angle = [target_list_angle; idx];
end
target_list_RDA = [target_list_RD_new, target_list_angle];
end
0 - example/wzk - RDA/spatial_FFT.m
+31
View File
@@ -0,0 +1,31 @@
function [ echo_sFFT_mtx, angle_index ] = spatial_FFT(echo_mtx, d, lambda)
% FFT for echo space domain
%
% Usage:
% [echo_sFFT_mtx, angle_index] = spatial_FFT(echo_mtx, d, lambda)
%
% Inputs:
% echo_mtx: Matrix containing the echo data
% d: Antenna spacing
% lambda: Wave length
%
% Outputs:
% echo_sFFT_mtx: Processed echo matrix
% angle_index: True value of the angle
% parameters
[lenA, lenR, lenP] = size(echo_mtx);
% calculate angle
angle_index = asin(-1 * (-1/2: 1/lenA: 1/2 - 1/lenA) * lambda / d) / pi * 180;
% spatial FFT
echo_mtx = reshape(echo_mtx, [lenA, lenR * lenP]);
echo_sFFT_mtx = zeros(size(echo_mtx));
for i = 1: lenR * lenP
echo_sFFT_mtx(:, i) = fftshift(fft(echo_mtx(:, i))) ./ sqrt(lenA);
end
echo_sFFT_mtx = reshape(echo_sFFT_mtx, [lenA, lenR, lenP]);
end