%======================================================%
% Function: Verify three point interpolation methods for the time delay estimation
% Author: Yang Jinhe
% Time: 2020-08-28
%======================================================%
samplingTime = 0.08;
time = 0:samplingTime:2; % 时间
delayTime = 0.5; % 时间延迟量
fs = 1/samplingTime; % 采样频率
w = pi; % 角速度
freq0 = w/(2*pi); % 原始信号频率
fc = 0.5*freq0; % 滤波器截断频率
SNR = 20; % 信噪比 (-5dB, 25dB)
x = sin(w*time); % 原始信号
inputSignal = awgn(x, SNR, 'measured', 'db'); % 输入信号
% plot(time, x, 'r', time, inputSignal, 'b'); % 原始信号和加入噪声后信号对比
stem(fft(x));
hold on;
stem(fft(inputSignal));
% 二阶低通Butterworth滤波器
n = 2;
[para1, para2] = butter(n, fc/(fs/2), 'low');
filteredSignal = filter(para1, para2, inputSignal);
refSignal = filteredSignal; % 参考信号
dftSignal0 = fft(filteredSignal);
dftSignal1 = fft(filteredSignal)*exp(-1i*w*delayTime);
delayedSignal = ifft(fft(filteredSignal)*exp(-1i*w*delayTime)); % 延迟信号
% delayedSignal = delayseq(signal, abs(delayTime), fs);
figure;
subplot 211, plot(time, refSignal, 'r', time, delayedSignal, 'b'); % 原始信号和加入噪声后信号对比
subplot 212, stem (dftSignal0); hold on;
stem(dftSignal1);
% % refSignal = sin(w*time); % 原始信号
% % delayedSignal = sin(w*(time+delayTime)); % 延迟信号
% subplot 211;
% plot(time, refSignal, 'r-', time, delayedSignal, 'b-');
% xlabel('时间 (s)');
% ylabel('电压 (V)');
% legend('原始信号', '延迟信号');
% title('信号时域波形图');
%
% [c, lags]=xcorr(refSignal, delayedSignal); %互相关
% subplot 212;
% stem(lags*samplingTime, c);
% %axis tight;
% grid;
% title('相关函数图');
%
% method = 'improvedGaussianMethod'; %gaussianMethod//parabolaMethod//improvedGaussianMethod//cosineMethod
%
% [maxValue, pos]=max(c);
% % parabolaMethod
% corrTime = lags(pos)*samplingTime;
% if strcmp(method, 'parabolaMethod')
% estimateTime = (c(pos-1)-c(pos+1))/(2.0*(c(pos-1)+c(pos+1)-2*maxValue));
% elseif strcmp(method, 'gaussianMethod')
% estimateTime = (log(c(pos-1))-log(c(pos+1)))/(2.0*(log(c(pos-1))+log(c(pos+1))-2*log(maxValue)));
% elseif strcmp(method, 'improvedGaussianMethod')
% beta = 1.0;
% estimateTime = (log(c(pos-1)+beta)-log(c(pos+1)+beta))/(2.0*(log(c(pos-1)+beta)+log(c(pos+1)+beta)-2*log(maxValue+beta)));
% elseif strcmp(method, 'cosineMethod')
% Omega = acos((c(pos-1)+c(pos+1))/(2*maxValue));
% phi = atan((c(pos-1)-c(pos+1))/(2*maxValue*sin(Omega)));
% estimateTime = (-1)*phi/Omega;
% end