48 lines
1.1 KiB
Matlab
48 lines
1.1 KiB
Matlab
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
|