Merge pull request '[25.08.17] Applying DOA logic : CBF' (#4) from feature/applying_DOA_CBF into develop

Reviewed-on: #4

Functions/01. Direction finding/df_cbf.m

@@ -1,4 +1,4 @@
-function [esti_ang_deg, P_CBF] = df_cbf(snapshot, test_ang_elev_deg, test_ang_azi_deg, lambda_c, array_struct, Ntarget)
+function [esti_ang_deg, P_CBF] = df_cbf(snapshot, test_ang_elv_deg, test_ang_azi_deg, lambda_c, array_struct, Ntarget)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 % Conventioanl Beamforming algorithm for df 
@@ -9,7 +9,7 @@ function [esti_ang_deg, P_CBF] = df_cbf(snapshot, test_ang_elev_deg, test_ang_az
 % Description
 %   - Input
 %       - 1) snapshot [matrix], [-] : raw data(snapshot) for df 
-%       - 2) test_ang_elev_deg [vector], [deg] : elevation angle grid for beamforming
+%       - 2) test_ang_elv_deg [vector], [deg] : elvation angle grid for beamforming
 %       - 3) test_ang_azi_deg [vector], [deg] : Azimuth angle grid for beamforming
 %       - 4) lambda_c [scalar], [m] : wavelength of center frequency 
 %       - 4) array_struct [struct], [-] : Structure containing virtual array information
@@ -29,11 +29,11 @@ function [esti_ang_deg, P_CBF] = df_cbf(snapshot, test_ang_elev_deg, test_ang_az
 
 
 % Spectrum
-P_CBF = zeros(length(test_ang_elev_deg), length(test_ang_azi_deg));
-for ang_elev_idx = 1 : length(test_ang_elev_deg)
+P_CBF = zeros(length(test_ang_elv_deg), length(test_ang_azi_deg));
+for ang_elv_idx = 1 : length(test_ang_elv_deg)
     for ang_azi_idx = 1 : length(test_ang_azi_deg)
-        sv = exp(-1i * 2 * pi / lambda_c * ( array_struct.azi_eff_ch_loc.' * array_struct.lambda_c * cosd(test_ang_elev_deg(ang_elev_idx)) * sind(test_ang_azi_deg(ang_azi_idx)) + array_struct.elev_eff_ch_loc.' * array_struct.lambda_c * sind(test_ang_elev_deg(ang_elev_idx))));         
-        P_CBF(ang_elev_idx, ang_azi_idx) = abs((sum(sv'*snapshot))).^2;       
+        sv = exp(-1i * 2 * pi / lambda_c * ( array_struct.azi_eff_ch_loc.' * array_struct.u_azi * cosd(test_ang_elv_deg(ang_elv_idx)) * sind(test_ang_azi_deg(ang_azi_idx)) + array_struct.elv_eff_ch_loc.' * array_struct.u_elv * sind(test_ang_elv_deg(ang_elv_idx))));         
+        P_CBF(ang_elv_idx, ang_azi_idx) = abs((sum(sv'*snapshot))).^2;       
     end
 end
 
@@ -46,7 +46,7 @@ if size(P_CBF, 1) == 1
         esti_ang_deg = pks_ang(order(1:Ntarget));
     end
 elseif size(P_CBF, 2) == 1
-    [pks, pks_ang, ~, ~] = findpeaks(pow2db(abs(P_CBF)), test_ang_elev_deg);
+    [pks, pks_ang, ~, ~] = findpeaks(pow2db(abs(P_CBF)), test_ang_elv_deg);
     [~, order] = sort(pks, 'descend');
     if isempty(pks)
         esti_ang_deg = nan;
@@ -59,7 +59,7 @@ else
     if isempty(pks)
         esti_ang_deg = nan;
     else
-        esti_ang_deg = [test_ang_elev_deg(locs_x(order(1:Ntarget))).' test_ang_azi_deg(locs_y(order(1:Ntarget))).'];
+        esti_ang_deg = [test_ang_elv_deg(locs_x(order(1:Ntarget))).' test_ang_azi_deg(locs_y(order(1:Ntarget))).'];
     end
 end
 

System_Analysis_Main.m

@@ -7,8 +7,13 @@ addpath(genpath(fullfile(pwd, 'Antenna_Pattern')));
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
-% 좌표계 : ego car 기준 앞뒤 : x 축, 좌우 : y 축, 위아래 : z 축
-%
+% 좌표계 : 
+% 1) 중심(0,0,0) : 레이더의 중심
+% 2) 레이더 boresight(레이더 안테나면의 수직방향) : +x axis
+% 3) 레이더 boresight 기준 왼쪽 : +y axis, 오른쪽 : -y axis
+% 4) X-Y 평면에서 위로 수직 : +z axis, 아래로 수직 : -z axis
+% 5) Elevation : x-y 평면 0 deg 기준. +z 방향으로 (+), -z 방향으로 (-)
+% 6) Azimuth : +x axis 를 0 deg 기준. x-y 평면상에서 반시계 : (+), 시계 : (-)
 %
 %
 %
@@ -26,7 +31,7 @@ opt_window = 1;
 
 flag_plot_physical_array = 0;
 flag_plot_virtual_array = 0;
-flag_plot_RVM = 1;
+flag_plot_RVM = 0;
 flag_plot_noise_esti = 0;
 flag_plot_birdview = 1;
 
@@ -90,12 +95,16 @@ if flag_plot_physical_array == 1
 end
 
 if flag_plot_virtual_array == 1
-    figure
-    scatter(mimoarray(2,:) / lambda_c, mimoarray(3,:) / lambda_c, 'o')
+    figure    
+    scatter(mimoarray(2,1:Nr) / lambda_c, mimoarray(3,1:Nr) / lambda_c, 'bo')
+    hold on
+    scatter(mimoarray(2,Nr+1:2*Nr) / lambda_c, mimoarray(3,Nr+1:2*Nr) / lambda_c, 'ro')
+    scatter(mimoarray(2,2*Nr+1:3*Nr) / lambda_c, mimoarray(3,2*Nr+1:3*Nr) / lambda_c, 'go')
     grid on
     xlabel('y-axis [\lambda]');
     ylabel('z-axis [\lambda]');
     title('Virtual Array Positions');
+    legend('Tx1','Tx2','Tx3');
 end
 
 %% Environments
@@ -105,15 +114,15 @@ velR = [0 0 0].'; % radar velocity
 accR = [0 0 0].'; % radar acceleration
 
 % 01. Target
-tgt_rng = [10];
-tgt_azi = [0];
+tgt_rng = [100];
+tgt_azi = [10];
 tgt_elv = [0];
 tgt_pos = [tgt_rng.*cosd(tgt_azi).*cosd(tgt_elv) tgt_rng.*sind(tgt_azi).*cosd(tgt_elv) tgt_rng.*sind(tgt_elv)].';
 
 %tgt_pos = [150 0 0].';
-tgt_vel = [5 0 0].';
+tgt_vel = [0 0 0].';
 tgt_acc = [0 0 0].';
-tgt_rcs = [20];
+tgt_rcs = [10];
 tgt_class = 'TCR';
 tgt = gen_tgt(tgt_pos, tgt_vel, tgt_acc, tgt_rcs, 'TCR');
 tgt.num = size(tgt.rcs, 1);
@@ -121,11 +130,11 @@ tgt.num = size(tgt.rcs, 1);
 for tgt_idx = 1 : tgt.num
     [tgt_azi_rad(tgt_idx), tgt_elv_rad(tgt_idx), ~] = cart2sph(tgt_pos(1, tgt_idx), tgt_pos(2, tgt_idx), tgt_pos(3, tgt_idx));
     tgt_azi_deg(tgt_idx, 1) = rad2deg(tgt_azi_rad(tgt_idx));
-    tgt_elv_deg(tgt_idx, 1) = rad2deg(tgt_elv_rad(tgt_idx));
-    tgt_radi_vel(tgt_idx, 1) = norm((tgt_vel(:, tgt_idx) - velR) .* (tgt_pos(:, tgt_idx) - posR)/norm((tgt_pos(:, tgt_idx) - posR)));
+    tgt_elv_deg(tgt_idx, 1) = rad2deg(tgt_elv_rad(tgt_idx));    
+    tgt_radi_vel(tgt_idx, 1) = sum((tgt_vel(:, tgt_idx) - velR) .* (tgt_pos(:, tgt_idx) - posR)/norm((tgt_pos(:, tgt_idx) - posR)));
 end
 
-tgt_table_truth = table([tgt_rng tgt_radi_vel tgt_azi_deg tgt_elv_deg]', 'VariableNames', {'Tgt.'}, 'RowNames', {'Rng[m]', 'Vel,[m/s]', 'Azi.[deg]','Elv.[deg]'});
+tgt_table_truth = table([tgt_rng tgt_radi_vel tgt_azi_deg tgt_elv_deg]', 'VariableNames', {'Tgt.'}, 'RowNames', {'Rng[m]', 'radi Vel.[m/s]', 'Azi.[deg]','Elv.[deg]'});
 disp(tgt_table_truth);
 
 % 02. Clutter
@@ -147,26 +156,18 @@ timing_struct.T_jumpback = 0.5e-6;
 timing_struct.T_reset = 1e-6;
 timing_struct.T_acq = 51.2e-6;
 tx_phase_initial = 0;
-timing_struct.T_idle = 9.5e-6; %[1e-6 9.5e-6 18e-6];
-
-% timing_struct.T_dwell = 0e-6;
-% timing_struct.T_settle = 0e-6;
-% timing_struct.T_jumpback = 0e-6;
-% timing_struct.T_reset = 0e-6;
-% timing_struct.T_acq = 51.2e-6;
-% tx_phase_initial = 0;
-% timing_struct.T_idle = 0e-6; %[1e-6 9.5e-6 18e-6];
+timing_struct.T_idle = 1e-6; %[1e-6 9.5e-6 18e-6];
 timing_struct.PRI = timing_struct.T_dwell + timing_struct.T_settle + timing_struct.T_jumpback + timing_struct.T_reset + timing_struct.T_acq + timing_struct.T_idle; 
 %[56.7e-6 65.2e-6 73.7e-6];
 
 
 % [24.08.22] Lowpass filter 구현 때문에 2배 oversampling 함.
-for m = 1 : NumChirps
-    %Tx_wave = gen_fastramp_fmcw2(timing_struct, 2*fs, fc-TxBw/2, TxBw, m, NumChirps, 0, tx_phase_initial, opt_waveform);
+temp_Ref_wave = zeros(2*NumSamples, NumChirps);
+for m = 1 : NumChirps    
     Tx_wave = gen_fastramp_fmcw(timing_struct, 2*fs, fc-TxBw/2, TxBw, NumChirps, 0, tx_phase_initial, opt_waveform);
-    Ref_wave(:,m) = Tx_wave.waveform;
+    temp_Ref_wave(:,m) = Tx_wave.waveform;
 end
-Ref_wave = repmat(Ref_wave, 1, 1, Nr);
+Ref_wave = repmat(temp_Ref_wave, 1, 1, Nr);
 
 % 02. DDMA
 DDMA_freq = [0 1/4 2/4] * 1/Tx_wave.T_chirp;
@@ -243,10 +244,9 @@ for tgtidx = 1 : tgt.num
                 powVar_dB(txidx, rxidx) = powVar_dB(txidx, rxidx) + rx_ant_gain_db + pow2db(lambda_c^2/(4*pi));
     
                 target_prop_time = target_prop_time_dod + target_prop_time_doa;
-                % [24.08.22] Lowpass filter 구현 때문에 2배 oversampling 함.
-                %delayed_wave = gen_fastramp_fmcw2(timing_struct, 2*fs, fc-TxBw/2, TxBw, m, NumChirps, target_prop_time, tx_phase_initial, opt_waveform);                     
+                % [24.08.22] Lowpass filter 구현 때문에 2배 oversampling 함.                
                 delayed_wave = gen_fastramp_fmcw(timing_struct, 2*fs, fc-TxBw/2, TxBw, NumChirps, target_prop_time, tx_phase_initial, opt_waveform);                     
-                delayed_Tx_sig(:,m,rxidx) = delayed_Tx_sig(:,m,rxidx) + sqrt(db2pow(powVar_dB(txidx, rxidx))) * (delayed_wave.waveform).' .* exp(-1i * 2 * pi * DDMA_freq(txidx) * ((m-1) * delayed_wave.T_chirp));
+                delayed_Tx_sig(:,m,rxidx) = txidx * delayed_Tx_sig(:,m,rxidx) + sqrt(db2pow(powVar_dB(txidx, rxidx))) * (delayed_wave.waveform).' .* exp(-1i * 2 * pi * DDMA_freq(txidx) * ((m-1) * delayed_wave.T_chirp));
             end              
         end
     end
@@ -452,11 +452,14 @@ sig_rng_dop_fft2 = sig_rng_dop_fft2_wo_n + noise_rng_dop_fft2;
 
 %% Signal Processing
 rng_grid = beat2rng(gen_freqgrid(rngFFTlen, fs, 0), Tx_wave.f_slope, c0);
-spd_grid = 0.5*dop2spd(gen_freqgrid(velFFTlen, 1/(Tx_wave.T_chirp), 1), lambda_c);
-spd_grid_2 = fftshift(spd_grid);
+spd_grid_fftshift = 0.5*dop2spd(gen_freqgrid(velFFTlen, 1/(Tx_wave.T_chirp), 1), lambda_c);
+spd_grid_no_fftshift = fftshift(spd_grid_fftshift);
 
-PS_sig_single_ch = abs(fftshift(sig_rng_dop_fft2 ,2)).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
-PS_noise_single_ch = abs(fftshift(noise_rng_dop_fft2 ,2)).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
+% PS_sig_single_ch = abs(fftshift(sig_rng_dop_fft2 ,2)).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
+% PS_noise_single_ch = abs(fftshift(noise_rng_dop_fft2 ,2)).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
+
+PS_sig_single_ch = abs(sig_rng_dop_fft2).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
+PS_noise_single_ch = abs(noise_rng_dop_fft2).^2/NumChirps/NumSamples/rngFFTlen/velFFTlen;
 
 Winloss_dB = 0;
 RBW_noise = fs/velFFTlen/rngFFTlen;
@@ -479,13 +482,13 @@ if flag_plot_RVM == 1
     figure
     for ch_idx = 1 : Nr
         subplot(sqrt(Nr), sqrt(Nr), ch_idx);
-        mesh(spd_grid, rng_grid(1:maxrngFFTidx), pow2db(squeeze(PS_sig_single_ch(1:maxrngFFTidx, :, ch_idx))) + 30);
-        xlabel('Rng [m]');
-        ylabel('Vel [m/s]');
+        mesh(spd_grid_fftshift, rng_grid(1:maxrngFFTidx), pow2db((squeeze(PS_sig_single_ch(1:maxrngFFTidx, :, ch_idx)))) + 30);
+        xlabel('Vel [m/s]');
+        ylabel('Rng [m]');
         zlabel('Pow [dBm]');
         hold on
-        mesh(spd_grid, rng_grid(1:maxrngFFTidx), noise_floor_dBm*ones(maxrngFFTidx, velFFTlen), 'EdgeColor', 'r');
-        mesh(spd_grid, rng_grid(1:maxrngFFTidx), pow2db(np_esti_single_ch(:, ch_idx).*ones(1,velFFTlen)) + 30, 'EdgeColor', 'g');
+        mesh(spd_grid_fftshift, rng_grid(1:maxrngFFTidx), noise_floor_dBm*ones(maxrngFFTidx, velFFTlen), 'EdgeColor', 'r');
+        mesh(spd_grid_fftshift, rng_grid(1:maxrngFFTidx), pow2db(np_esti_single_ch(:, ch_idx).*ones(1,velFFTlen)) + 30, 'EdgeColor', 'g');
         legend('Received Data', 'Expected Noise Floor', 'Esti. Noise Floor', 'Location', 'best');
         hold off
         title(['Ch : ', num2str(ch_idx)]);
@@ -508,7 +511,7 @@ PS_noise_Rx_NCI = noise_Rx_NCI/NumChirps/NumSamples/rngFFTlen/velFFTlen;
 
 if flag_plot_RVM == 1
     figure
-    mesh(spd_grid, rng_grid(1:maxrngFFTidx), pow2db(fftshift(PS_sig_Rx_NCI(1:maxrngFFTidx,:), 2)) + 30);
+    mesh(spd_grid_fftshift, rng_grid(1:maxrngFFTidx), pow2db((PS_sig_Rx_NCI(1:maxrngFFTidx,:))) + 30);
     axis([-vel_max vel_max 0 rng_max]);
     xlabel('Speed [m/s]');
     ylabel('Range [m]');
@@ -574,7 +577,7 @@ end
 
 % 4. DOA estimation
 % 4-1. Generate Snapshot and Resolving Doppler ambiguity by DDMA
-adddopidx = [64 128 192 0];
+adddopidx = [192 0 64 128];
 snapshot_idx_set = [4 1 2 3; 3 4 1 2; 2 3 4 1; 1 2 3 4];
 temp_snapshot_data = zeros((Nt+1)*Nr,length(rngidx_set));
 for idx = 1 : length(rngidx_set)
@@ -597,7 +600,7 @@ snapshot_data = temp_snapshot_data(1:Nt*Nr, :);
 
 % Add resolved vel info to DET_set structure
 for det_idx = 1 : length(rngidx_set)
-    temp_DET_set{det_idx}.v_amb_mps = spd_grid_2(dopidx_set(det_idx));
+    temp_DET_set{det_idx}.v_amb_mps = spd_grid_no_fftshift(dopidx_set(det_idx));
 end
 
 % 4-2. DOA estimation
@@ -605,20 +608,22 @@ az_array_pos = mimoarray(2, :) / u_azi;
 
 elv_Unit = u_elv / lambda_c;
 
-test_ang = -90 : 0.1 : 90;
-svmat = exp(-1i * 2 * pi / lambda_c * az_array_pos.' * u_azi .* sind(test_ang));
-
+test_ang_azi_deg = -90 : 0.1 : 90;
+test_ang_elev_deg = 0;
+array_struct.azi_eff_ch_loc = az_array_pos;
+array_struct.elv_eff_ch_loc = 0;
+array_struct.u_azi = u_azi;
+array_struct.u_elv = 0;
 det_jdx = 1;
 for det_idx = 1 : size(snapshot_data, 2)
     % DOA estimation
         % Elv esti.
-        elv_phase_diff = conj(snapshot_data(4, det_idx)) * snapshot_data(9, det_idx);
+        elv_phase_diff = conj(snapshot_data(9, det_idx)) * snapshot_data(4, det_idx);
         temp_esti_elv_deg = asind(angle(elv_phase_diff)/2/pi/elv_Unit);
 
         % Azi esti.
-        azi_spectrum = abs(svmat' * snapshot_data(:, det_idx)).^2;
-        [pks, esti_azi_deg] = findpeaks(azi_spectrum/max(azi_spectrum), test_ang, 'MinPeakHeight', 0.5);
-
+        Ntarget = 1;
+        [esti_azi_deg, P_CBF(det_idx,:)] = df_cbf(snapshot_data(:,det_idx), test_ang_elev_deg, test_ang_azi_deg, lambda_c, array_struct, Ntarget);
         esti_elv_deg = temp_esti_elv_deg * ones(1, length(esti_azi_deg));
 
 
@@ -646,6 +651,7 @@ if flag_plot_birdview == 1
     el_deg_set = cellfun(@(x) x.el_deg, DET_set);
     snr_db_set = cellfun(@(x) x.snr_db, DET_set);
     plotdet = plot3(y_m_set, x_m_set, z_m_set, 'bo');
+    set(gca, 'XDir', 'reverse');
     fn_add_data_tip(plotdet, 'Rng [m]:', r_m_set, 1);
     fn_add_data_tip(plotdet, 'aVel [m/s]:', v_amb_mps_set, 2);
     fn_add_data_tip(plotdet, 'Azi [deg]:', az_deg_set, 3);