Merge pull request 'feature/fix_angle_error' (#3) from feature/fix_angle_error into develop

Reviewed-on: #3

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 평면상에서 반시계 : (+), 시계 : (-)
 %
 %
 %
@@ -91,11 +96,15 @@ end
 
 if flag_plot_virtual_array == 1
     figure    
-    scatter(mimoarray(2,:) / lambda_c, mimoarray(3,:) / lambda_c, 'o')
+    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,13 +114,13 @@ velR = [0 0 0].'; % radar velocity
 accR = [0 0 0].'; % radar acceleration
 
 % 01. Target
-tgt_rng = [10];
+tgt_rng = [100];
 tgt_azi = [0];
 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 = [10];
 tgt_class = 'TCR';
@@ -122,10 +131,10 @@ 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_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
@@ -237,7 +246,7 @@ for tgtidx = 1 : tgt.num
                 target_prop_time = target_prop_time_dod + target_prop_time_doa;
                 % [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
@@ -443,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;
@@ -470,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)]);
@@ -499,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]');
@@ -565,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)
@@ -588,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
@@ -603,13 +615,14 @@ 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);
-
+        [pks, esti_azi_deg] = findpeaks(azi_spectrum/max(azi_spectrum), test_ang, 'MinPeakHeight', 0.9);
+        figure
+        plot(test_ang, pow2db(azi_spectrum));
         esti_elv_deg = temp_esti_elv_deg * ones(1, length(esti_azi_deg));
 
 
@@ -637,6 +650,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);