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

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

x = data((1:n)+((frame-1)*n));
%將數(shù)據(jù)轉(zhuǎn)換到基帶
x_sine = x.*sine;
x_cosine = x.*cosine;
 
Q = sum(x_sine);      %經(jīng)過濾波器
I = sum(x_cosine);
phase_discri(frame) = atan(Q/I);   %得到鎖相環(huán)的輸入
 
%鎖相環(huán)
dfrq = c1*phase_discri(frame)+temp; %經(jīng)過環(huán)路濾波器
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('鎖相環(huán)跟蹤','實際的載波頻率');
grid
mean_freq=mean(dfrq_frame/2/pi)
p=abs(real_fc-mean_freq)/real_fc;