fs = 12e6; %采樣頻率
ts = 1/fs; 
num = 2.5e6;  %數據長度
SNR = -15;
real_fc = 3563000; %實信號頻率
data = sin(2*pi*real_fc*(0:num-1)*ts+pi/4)+sqrt(10^(SNR/10))*randn(1,num); %科斯塔斯環的輸入信號
fc = 3562800; %本地頻率
 
n = fs/1000; %累積時間為1ms
nn = [0:n-1];
nf = floor(length(data)/n);% 將輸入數據分成1ms的多個數據塊
wfc = 2*pi*fc;  %本地信號
phi_prv = 0;
temp = 0;
frame = 0;
carrier_phase = 0;
phase = 0;
 
%環路濾波器的參數
c1 = 153.7130;
c2 = 6.1498;

for frame=1:nf 
% 產生本地的sin和cos函數
expcol = exp(j*(wfc*ts*nn+phase));
sine = imag(expcol);   
cosine = real(expcol);

x = data((1:n)+((frame-1)*n));
%將數據轉換到基帶
x_sine = x.*sine;
x_cosine = x.*cosine;
 
Q = sum(x_sine);      %經過濾波器
I = sum(x_cosine);
phase_discri(frame) = atan(Q/I);   %得到鎖相環的輸入
 
%鎖相環
dfrq = c1*phase_discri(frame)+temp; %經過環路濾波器
temp = temp+c2*phase_discri(frame);
wfc = wfc-dfrq*2*pi;   %改變本地頻率
dfrq_frame(frame) = wfc; 
phase = wfc*ts*n+phase;   %得到不同塊的相位
end
plot(dfrq_frame/(2*pi));
hold on
plot([1:length(dfrq_frame)], real_fc,'r');
legend('鎖相環跟蹤','實際的載波頻率');
grid
mean_freq=mean(dfrq_frame/2/pi)
p=abs(real_fc-mean_freq)/real_fc;