function velocity = get_velocity(s_T, s_R, origin_velocity) global T_r delta_t c f_c max_t slow_len; freq_s_T = 1 / T_r; slow_freqs = ((0:slow_len - 1) * (1 / T_r)) / slow_len; range_t_slow = 1:slow_len; num_slow = max_t / T_r; s_R_slow = zeros(1, slow_len); k = floor(T_r / delta_t); idx = 1; while abs(s_R(idx)) == 0 idx = idx + 1; end for i = 0:num_slow - 1 while abs(s_R(idx + i * k)) == 0 idx = idx + 1; end s_R_slow(i + 1) = s_R(idx + i * k); end s_R_fft = fft(s_R_slow); [~, max_index_s_R] = max(s_R_fft); freq_s_R = slow_freqs(max_index_s_R); % figure(2); % subplot(2, 1, 1); % plot(range_t_slow, abs(s_R_slow)); % title(sprintf('s_R')); % subplot(2, 1, 2); % plot(slow_freqs, abs(s_R_fft)); % title(sprintf('s_R_fft, freq = %E', freq_s_R)); % xlabel('频率 (Hz)'); f_d = freq_s_T - freq_s_R; velocity = (c * f_d) / (2 * f_c); fprintf("predict velocity: %f\n", velocity); fprintf("origin velocity: %f\n", origin_velocity); fprintf("err = %f\n", velocity - origin_velocity); fprintf("\n"); end