目錄
1.軟體版本
2.核心代碼
3.操作步驟與仿真結論
4.參考文獻
5.完整源碼獲得方式
1.軟體版本
MATLAB2021a
2.核心代碼
%載波跟蹤算法仿真
clc
clear
clf
%輸入信号參數定義
fd=2.45e+3;
f_ref=4.092e+6;
fi=(4.092e+6)+fd;
fs=16.368e+6;
Ts=1/fs;
N=16368;
Repeat_Num=0.5000e+4;
Freq_Step=500;
Freq_Max=5.0e+3;
Step_Num=Freq_Max/Freq_Step;
T_IC=1.0e-3;
m0=4;
Freq_Acquisition=4.092e+6+m0*Freq_Step;
%載波NCO的初始參數,NCO字長和量化電平個數
NCO_bit=5;
NCO_L=2^NCO_bit;
A_Ref=2;
fai1=0;
NCO_Step=2*A_Ref/NCO_L;
factor=2^32;
scaled_factor=factor/fs;
%定義變量
Ips_Array=zeros(1,Repeat_Num+1);
Qps_Array=zeros(1,Repeat_Num+1);
Ips_Normalize_Array=zeros(1,Repeat_Num+1);
Qps_Normalize_Array=zeros(1,Repeat_Num+1);
Accumulator=zeros(1,2*N);
s_I=zeros(1,N);
s_Q=zeros(1,N);
sdown_I=zeros(1,N);
sdown_Q=zeros(1,N);
NCO_I=zeros(1,N);
NCO_Q=zeros(1,N);
sita_out=zeros(1,2*N);
n_sample=zeros(1,N);
Phase_Modify=zeros(1,Repeat_Num+1);
Real_Freq_Error=zeros(1,Repeat_Num+1);
Real_Phase_AfterAccumu=zeros(1,Repeat_Num+1);
%input signal PN code
I_CodePhase=[3,7];
Q_CodePhase=[2,6];
I_Code=codegen(I_CodePhase);
Q_Code=codegen(Q_CodePhase);
%I_Code=ones(1,1023);
%Q_Code=ones(1,1023);
%Delay_Time=500;
%I_InputCode=[I_Code(Delay_Time+1:1023),I_Code(1:Delay_Time)];
%Q_InputCode=[Q_Code(Delay_Time+1:1023),Q_Code(1:Delay_Time)];
%輸入同相僞碼和正交僞碼
fN=16; %fN:samples per chip
I_LocalCode=codesamp(I_Code,fN);
Q_LocalCode=codesamp(Q_Code,fN);
I_InputCode=codesamp(I_Code,fN);
Q_InputCode=codesamp(Q_Code,fN);
%産生調制資料
Datarate_Ratio=341;
Data_Code=datagen(Datarate_Ratio);
Data_Code_Samp=codesamp(Data_Code,fN);
%産生輸入延遲僞碼
Delay_Time=8000;
I_InputCode1=[I_InputCode(Delay_Time+1:N),I_InputCode(1:Delay_Time)];
Q_InputCode1=[Q_InputCode(Delay_Time+1:N),Q_InputCode(1:Delay_Time)];
%信号模型
Track_Threshold=0.99;
A=3;
fai=pi/8; %輸入信号初始相位
WorkMode=2; %1為無正交載波和噪聲；2為有噪聲，但無正交載波；3為有正交載波和噪聲
n0=1; %高斯白噪聲功率（方差）
Noise_Bw=2.0e+6;
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);
for sn_dB=-20:10:-20
A=sqrt(2)*Noise_Power*10^(sn_dB/20); %根據信噪比計算信号的幅度
%定義變量
Ips_Array=zeros(1,Repeat_Num+1);
Qps_Array=zeros(1,Repeat_Num+1);
Ips_Normalize_Array=zeros(1,Repeat_Num+1);
Qps_Normalize_Array=zeros(1,Repeat_Num+1);
Ips_Filtered=zeros(1,Repeat_Num+1);
Qps_Filtered=zeros(1,Repeat_Num+1);
Accumulator=zeros(1,2*N);
s_I=zeros(1,N);
s_Q=zeros(1,N);
sdown_I=zeros(1,N);
sdown_Q=zeros(1,N);
NCO_I=zeros(1,N);
NCO_Q=zeros(1,N);
sita_out=zeros(1,2*N);
n_sample=zeros(1,N);
Phase_Modify=zeros(1,Repeat_Num+1);
Real_Freq_Error=zeros(1,Repeat_Num+1);
Real_Phase_AfterAccumu=zeros(1,Repeat_Num+1);
n=[0:N-1];
%輸入信号同相和正交載波
s_I=A*cos(2*pi*n*Ts*fi+fai);
s_Q=A*sin(2*pi*n*Ts*fi+fai);
%高斯噪聲
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);
%輸入信号
if WorkMode==1
s_input=I_InputCode1.*s_I;
elseif WorkMode==2
s_input=I_InputCode1.*s_I+Guass_Noise;
elseif WorkMode==3
s_input=I_InputCode1.*s_I+Guass_Noise+(Data_Code_Samp.*Q_InputCode1).*s_Q;
end
s_input_Quanti=GPSChip_Out_Simula(s_input);
%向螢幕輸出
fprintf('calculating from acquisition doppler frequence=%d\n',m0*500);
%載波NCO輸出同相和正交參考信号
NCO_I=A_Ref*cos(2*pi*n*Ts*Freq_Acquisition+fai1);
NCO_Q=-A_Ref*sin(2*pi*n*Ts*Freq_Acquisition+fai1);
NCO_I_Quanti=round(NCO_I/NCO_Step)*NCO_Step;%載波NCO輸出5bit(32個電平)量化
NCO_Q_Quanti=round(NCO_Q/NCO_Step)*NCO_Step;
%NCO_I_Quanti=NCO_I;
%NCO_Q_Quanti=NCO_Q;
%下變頻
sdown_I=s_input_Quanti.*NCO_I_Quanti;
sdown_Q=s_input_Quanti.*NCO_Q_Quanti;
%本地再生僞碼
delayno=1001;%1001正好對齊,1002滞後1/2碼片,1000超前1/2碼片
I_LocalCode1=[I_LocalCode((delayno-1)*8+1:N),I_LocalCode(1:(delayno-1)*8)];
Q_LocalCode1=[Q_LocalCode((delayno-1)*8+1:N),Q_LocalCode(1:(delayno-1)*8)];
%解擴
%sdespread_I=sdown_I(1:N).*I_LocalCode1;%sdespread_Q=sdown_Q(1:N).*I_LocalCode1;
%積分-清除器輸出
%saccum_I=sum(sdespread_I);%saccum_Q=sum(sdespread_Q);
%解擴和積分合并計算（上兩步合并）
I_Ps=sdown_I*I_LocalCode1'/N;
Q_Ps=sdown_Q*I_LocalCode1'/N;
%用四相鑒頻器進行頻率牽引
%------------------------------------------------------------------------------------
% 四相鑒頻器頻率牽引
%------------------------------------------------------------------------------------
%四相鑒頻器的參數定義
Freq_Dev=zeros(1,Repeat_Num+1);
Phase_Error=0;
Freq_Error=500;
Real_Freq_Error=zeros(1,Repeat_Num+1);
Quadrant_Freq_Out=zeros(1,Repeat_Num+1);
Phase_Modify(1)=2*pi*Freq_Acquisition*N*Ts;
Track_FlagValue_Filtered=0;
Quadrant_Initial_Freq=2*pi*Ts*Freq_Acquisition;
beta=0;
f_beta=0;
f_beta_temp=0;
Freq_Temp=0;
Freq_Word=round(Freq_Acquisition/fs*factor);
%載波跟蹤環全程檔案記錄
if WorkMode==1
filename_Quadra=['CarrLoop_QuantiTh_Quadra_TrackErr_','NoQ_NoNoise.txt'];,
elseif WorkMode==2
filename_Quadra=['CarrLoop_QuantiTh_Quadra_TrackErr_',num2str(sn_dB),'dB','_NoQ.txt'];
elseif WorkMode==3
filename_Quadra=['CarrLoop_QuantiTh_Quadra_TrackErr_',num2str(sn_dB),'dB','.txt'];
end
fileid_Quadra=fopen(filename_Quadra,'w');
fprintf(fileid_Quadra,'時間序列\t牽引量\t殘餘頻率估計量\t真實頻率殘餘\t收斂判别量\t收斂判别量（濾波後）\t收斂标志\r\n');
Ips_Array(1)=I_Ps;%第一個1ms積厘清除器的同相輸出
Qps_Array(1)=Q_Ps;%第一個1ms積厘清除器的正交輸出
Ips_Filtered(1)=Ips_Array(1);
Qps_Filtered(1)=Qps_Array(1);
n_sample=[0:2*N-1];
sita_out=2*pi*n_sample*Ts*Freq_Acquisition+fai1;%初始載波NCO輸出
k=0;
nn=0;
while k<=Repeat_Num
%計算新的一毫秒的輸入信号資料
k=k+1;
n_sample=[k*N:(k+1)*N-1];
s_I=A*cos(2*pi*n_sample*Ts*fi+fai);%輸入信号同相載波
s_Q=A*sin(2*pi*n_sample*Ts*fi+fai);%輸入信号正交載波
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);%高斯噪聲
if WorkMode==1
s_input=I_InputCode1.*s_I;
elseif WorkMode==2
s_input=I_InputCode1.*s_I+Guass_Noise;
elseif WorkMode==3
s_input=I_InputCode1.*s_I+Guass_Noise+(Data_Code_Samp.*Q_InputCode1).*s_Q;
end
s_input_Quanti=GPSChip_Out_Simula(s_input);
%s_input_Quanti=s_input;
%參考信号
if mod(k,2)==0%？？
NCO_I=A_Ref*cos(sita_out(1:N));
NCO_Q=-A_Ref*sin(sita_out(1:N));
else
NCO_I=A_Ref*cos(sita_out(N+1:2*N));
NCO_Q=-A_Ref*sin(sita_out(N+1:2*N));
end
NCO_I_Quanti=round(NCO_I/NCO_Step)*NCO_Step; %載波NCO輸出5bit量化
NCO_Q_Quanti=round(NCO_Q/NCO_Step)*NCO_Step;
%NCO_I_Quanti=NCO_I;
%NCO_Q_Quanti=NCO_Q;
%下變頻
sdown_I=s_input_Quanti.*NCO_I_Quanti; %同相下變頻信号
sdown_Q=s_input_Quanti.*NCO_Q_Quanti; %正交下變頻信号
%積厘清除器的輸出
Ips_Array(k+1)=sdown_I*I_LocalCode1'/N; %同相積厘清除器的輸出
Qps_Array(k+1)=sdown_Q*I_LocalCode1'/N; %正交積厘清除器的輸出
Ips_Normalize_Array(k+1)=Ips_Array(k+1)/sqrt(Ips_Array(k+1)^2+Qps_Array(k+1)^2);
Qps_Normalize_Array(k+1)=Qps_Array(k+1)/sqrt(Ips_Array(k+1)^2+Qps_Array(k+1)^2);
%收斂判别标志
Track_FlagValue=(Ips_Array(k+1)*Ips_Array(k)+Qps_Array(k+1)*Qps_Array(k))/(Ips_Array(k+1)^2+Qps_Array(k+1)^2);
Track_FlagValue_Filtered=0.9*Track_FlagValue_Filtered+0.1*Track_FlagValue;
if Track_FlagValue_Filtered>Track_Threshold
Track_Flag=1;
else
Track_Flag=0;
end
%頻率牽引
if mod(k,2)==1
nn=nn+1;
%真實誤差
Real_Freq_Error(nn)=fi-Freq_Word/factor*fs;
%atan2鑒頻器并估計頻率做牽引用
%dot=Ips_Array(k)*Ips_Array(k+1)+Qps_Array(k)*Qps_Array(k+1);
%cross=Ips_Array(k)*Qps_Array(k+1)-Ips_Array(k+1)*Qps_Array(k);
%Phase_Error=atan2(Qps_Array(k+1),Ips_Array(k+1))-atan2(Qps_Array(k),Ips_Array(k));
%上式等效于Phase_Error=atan2(cross,dot);
%Freq_Dev(nn)=Phase_Error/(2*pi*T_IC);
%用頻率鑒别器判别并對頻率鑒别器輸出歸一化
%Discriminator_Sign=sign(Ips_Array(k)*Ips_Array(k+1)+Qps_Array(k)*Qps_Array(k+1));
Freq_Dev(nn)=(Ips_Array(k)*Qps_Array(k+1)-Qps_Array(k)*Ips_Array(k+1))/(2*pi*T_IC);
Freq_Dev(nn)=Freq_Dev(nn)/(Ips_Array(k+1)^2+Qps_Array(k+1)^2); %幅度歸一化
%目前誤差
Freq_Error=Freq_Dev(nn);
%螢幕輸出
fprintf('no.:%d real frequence error:%.3f , Freq Discriminator:%.3f ',k,Real_Freq_Error(nn),Freq_Dev(nn));
%中止條件
if abs(Freq_Error)<10 %abs(Freq_Dev(nn)-Freq_Dev(nn-1))<0.2 |
break;
end
%四相鑒頻器算法
if abs(Ips_Array(k+1))>abs(Qps_Array(k+1))
beta=sign(Ips_Array(k+1))*(Qps_Array(k+1)-Qps_Array(k))/sqrt(Ips_Array(k+1)^2+Qps_Array(k+1)^2);
else
beta=-sign(Qps_Array(k+1))*(Ips_Array(k+1)-Ips_Array(k))/sqrt(Ips_Array(k+1)^2+Qps_Array(k+1)^2);
end
Quadrant_Freq_Out(nn)=beta/(2*pi*T_IC);%頻率牽引量
beta=Quadrant_Freq_Out(nn);
fprintf('Quadrant freq out:%.3f\n',Quadrant_Freq_Out(nn));
%向檔案輸出
fprintf(fileid_Quadra,'%d\t%f\t%f\t%f\t%f\t%f\t%f\r\n',k,Quadrant_Freq_Out(nn),Freq_Dev(nn),Real_Freq_Error(nn),...
Track_FlagValue,Track_FlagValue_Filtered,Track_Flag);
%累加所有牽引量并更新載波NCO
f_beta=f_beta_temp+beta;
f_beta_temp=f_beta;
%調整頻率
Freq_Word=round((f_beta+Freq_Acquisition)*factor/fs);
%NCO_Phase_Modify=Accumulator(2*N)/factor*2*pi;
for mn=1:2*N
if mn==1
Accumulator(mn)=Accumulator(2*N)+Freq_Word;
if(Accumulator(mn)>=factor)
Accumulator(mn)=Accumulator(mn)-factor;
end
sita_out(mn)=Accumulator(mn)/factor*2*pi;
else
Accumulator(mn)=Accumulator(mn-1)+Freq_Word;
if(Accumulator(mn)>=factor)
Accumulator(mn)=Accumulator(mn)-factor;
end
sita_out(mn)=Accumulator(mn)/factor*2*pi;
end
end
w_beta=2*pi*f_beta*Ts;
%Phase_Modify(nn)=(Quadrant_Initial_Freq+w_beta)*N;%N=1/Ts*T_IC;
fprintf('%f\n',Phase_Modify(nn));
end
end
fclose(fileid_Quadra);
fprintf('\nafter Quadrant drag:frequency=%f\n',fi-Freq_Word/factor*fs);
Accumu=Accumulator;
FLL_Initial_Freq=Freq_Word/factor*fs;
Max_Num_Quadrant=k;
Max_Update_Num_Quadrant=nn;
FLL_Initial_DataNo=Max_Num_Quadrant-1;
nx=[1:Max_Update_Num_Quadrant]*2;
figure(1);
plot(nx,Real_Freq_Error(1:Max_Update_Num_Quadrant),'r-',nx,Freq_Dev(1:Max_Update_Num_Quadrant),'b-.',...
nx,Quadrant_Freq_Out(1:Max_Update_Num_Quadrant),'g--');
ylabel('頻率（Hz）');
xlabel('疊代次數（次）');
legend('牽引後真實殘餘多普勒頻率','鑒頻器估計的殘餘多普勒頻率','多普勒頻率牽引量');
%----------------------------------------------------------------------------------------
% FLL鎖頻跟蹤環路
%----------------------------------------------------------------------------------------
%用FLL鎖頻環進行頻率跟蹤
f_Quadrant=FLL_Initial_Freq-f_ref;
fprintf('\n\nFLL初始頻率為: %f\n\n',f_Quadrant);
fprintf('\n--------------FLL tacking begin--------------\n');
Freq_Discriminator=zeros(1,Repeat_Num+1);
Freq_Discriminator_Filtered=zeros(1,Repeat_Num+1);
FLL_Freq_Dev=zeros(1,Repeat_Num+1);
Phase=zeros(1,Repeat_Num+1);
Ips_Filtered=zeros(1,Repeat_Num+1);
Qps_Filtered=zeros(1,Repeat_Num+1);
f_derivate=0;
f_derivate_temp=0;
f=0;
f_temp=0;
%環路帶寬參數
B_LF_FLL=3.0;
w_nF_FLL=1.89*B_LF_FLL;%4*sqrt(2)/3*B_LF
%擷取四相鑒頻器的有關參數
factor=2^32;
scaled_factor=factor/fs;
%初始化有關參數
update_w=0;
update_f=zeros(1,Repeat_Num+1);
Freq_Err=500;
%初始載波NCO輸出
Freq_Word=round(FLL_Initial_Freq*scaled_factor);
update_f(1)=0;
for mn=1:2*N
if(Accumu(mn)>=factor)%？？Accumu
Accumu(mn)=Accumu(mn)-factor;
end
sita_out(mn)=Accumu(mn)/factor*2*pi;
end
%NCO_Phase_Modify=(Accumu(1)-Freq_Word)/factor*2*pi;
NCO_Phase_Modify=Accumu(1)/factor*2*pi;%？？
%輸入信号采樣
n_sample=[FLL_Initial_DataNo*N:(FLL_Initial_DataNo+1)*N-1];
s_I=A*cos(2*pi*n_sample*Ts*fi+fai);%輸入信号同相載波
s_Q=A*sin(2*pi*n_sample*Ts*fi+fai);%輸入信号正交載波
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);%高斯噪聲
if WorkMode==1
s_input=I_InputCode1.*s_I;
elseif WorkMode==2
s_input=I_InputCode1.*s_I+Guass_Noise;
elseif WorkMode==3
s_input=I_InputCode1.*s_I+Guass_Noise+(Data_Code_Samp.*Q_InputCode1).*s_Q;
end
s_input_Quanti=GPSChip_Out_Simula(s_input);
%s_input_Quanti=s_input;
%下變頻
NCO_I=A_Ref*cos(sita_out(1:N));
NCO_Q=-A_Ref*sin(sita_out(1:N));
NCO_I_Quanti=round(NCO_I/NCO_Step)*NCO_Step;
NCO_Q_Quanti=round(NCO_Q/NCO_Step)*NCO_Step;
%NCO_I_Quanti=NCO_I;
%NCO_Q_Quanti=NCO_Q;
%下邊頻
sdown_I=s_input_Quanti.*NCO_I_Quanti;
sdown_Q=s_input_Quanti.*NCO_Q_Quanti;
%計算前後1毫秒的積厘清除器的輸出
Ips_Array(1)=sdown_I*I_LocalCode1'/N;%第一個1ms積厘清除器的同相輸出
Qps_Array(1)=sdown_Q*I_LocalCode1'/N;%第一個1ms積厘清除器的正交輸出
Ips_Filtered(1)=Ips_Array(1);
Qps_Filtered(1)=Qps_Array(1);
if WorkMode==1
filename_FLL=['CarrLoop_FLL_QuantiTh_TrackErr_',num2str(B_LF_FLL),'Hz','_NoQ_NoNoise.txt'];,
elseif WorkMode==2
filename_FLL=['CarrLoop_FLL_QuantiTh_TrackErr_',num2str(sn_dB),'dB_',num2str(B_LF_FLL),'Hz','_NoQ.txt'];
elseif WorkMode==3
filename_FLL=['CarrLoop_FLL_QuantiTh_TrackErr_',num2str(sn_dB),'dB_',num2str(B_LF_FLL),'Hz','.txt'];
end
fileid_FLL_track_err=fopen(filename_FLL,'w');
fprintf(fileid_FLL_track_err,'跟蹤帶寬為%f,載波相位跟蹤環跟蹤誤差（接收僞碼和本地僞碼相差%f碼片條件下)',B_LF_FLL,(delayno-1)*8-Delay_Time);%,sn_dB);
fprintf(fileid_FLL_track_err,'多普勒頻率為%f，起始頻差%f\r\n',fd,m0*Freq_Step);
fprintf(fileid_FLL_track_err,'時間點\t鑒頻出來的頻差\t濾波器輸出\t實際頻率差\t收斂判别量\t收斂判别量（濾波後）\t收斂标志\r\n');
k1=0;
nn=0;
while k1<=Repeat_Num
k1=k1+1;
n_sample=[(FLL_Initial_DataNo+k1)*N:(FLL_Initial_DataNo+k1+1)*N-1];
s_I=A*cos(2*pi*n_sample*Ts*fi+fai);%輸入信号同相載波
s_Q=A*sin(2*pi*n_sample*Ts*fi+fai);%輸入信号正交載波
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);%高斯噪聲
if WorkMode==1
s_input=I_InputCode1.*s_I;
elseif WorkMode==2
s_input=I_InputCode1.*s_I+Guass_Noise;
elseif WorkMode==3
s_input=I_InputCode1.*s_I+Guass_Noise+(Data_Code_Samp.*Q_InputCode1).*s_Q;
end
s_input_Quanti=GPSChip_Out_Simula(s_input);
%s_input_Quanti=s_input;
%參考信号
if mod(k1,2)==0
NCO_I=A_Ref*cos(sita_out(1:N));
NCO_Q=-A_Ref*sin(sita_out(1:N));
else
NCO_I=A_Ref*cos(sita_out(N+1:2*N));
NCO_Q=-A_Ref*sin(sita_out(N+1:2*N));
end
NCO_I_Quanti=round(NCO_I/NCO_Step)*NCO_Step;
NCO_Q_Quanti=round(NCO_Q/NCO_Step)*NCO_Step;
%NCO_I_Quanti=NCO_I;
%NCO_Q_Quanti=NCO_Q;
%下變頻
sdown_I=s_input_Quanti.*NCO_I_Quanti; %同相下變頻信号
sdown_Q=s_input_Quanti.*NCO_Q_Quanti; %正交下變頻信号
%積厘清除器的輸出
Ips_Array(k1+1)=sdown_I*I_LocalCode1'/N; %同相積厘清除器的輸出
Qps_Array(k1+1)=sdown_Q*I_LocalCode1'/N; %正交積厘清除器的輸出
Ips_Normalize_Array(k1+1)=Ips_Array(k1+1)/(Ips_Array(k1+1)^2+Qps_Array(k1+1)^2);
Qps_Normalize_Array(k1+1)=Qps_Array(k1+1)/(Ips_Array(k1+1)^2+Qps_Array(k1+1)^2);
%收斂判别标志
Track_FlagValue=(Ips_Array(k1+1)*Ips_Array(k1)+Qps_Array(k1+1)*Qps_Array(k1))/(Ips_Array(k1+1)^2+Qps_Array(k1+1)^2);
Track_FlagValue_Filtered=0.9*Track_FlagValue_Filtered+0.1*Track_FlagValue;
if Track_FlagValue_Filtered>Track_Threshold
Track_Flag=1;
else
Track_Flag=0;
end
if mod(k1,2)==1
nn=nn+1;
%叉積自動頻率環鑒頻器
Discriminator_Sign=sign(Ips_Array(k1)*Ips_Array(k1+1)+Qps_Array(k1)*Qps_Array(k1+1));
Phase_Error=(Ips_Array(k1)*Qps_Array(k1+1)-Qps_Array(k1)*Ips_Array(k1+1))/(Ips_Array(k1+1)^2+Qps_Array(k1+1)^2);
FLL_Freq_Dev(nn)=Discriminator_Sign*(Phase_Error/(2*pi*T_IC));
%atan2頻率鑒别器
%Phase_Error=atan2(Qps_Array(k1+1),Ips_Array(k1+1))-atan2(Qps_Array(k1),Ips_Array(k1));
%FLL_Freq_Dev(nn)=Phase_Error/(2*pi*T_IC);
Freq_Err=FLL_Freq_Dev(nn);
%相位修正項,測試用
Phase_Modify(nn)=rem(2*pi*Freq_Word/scaled_factor*Ts*N*(FLL_Initial_DataNo+k1-1),2*pi);
%Phase(nn)=atan(((Qps_Array(k1+1)/Ips_Array(k1+1))-tan(Phase_Modify(nn)+NCO_Phase_Modify))/(1+(Qps_Array(k1+1)/Ips_Array(k1+1))*tan(Phase_Modify(nn)+NCO_Phase_Modify))/1);
%Phase(nn)=atan2(Qps_Array(k1+1),Ips_Array(k1+1));
Phase(nn)=atan2(tan(atan2(Qps_Array(k1+1),Ips_Array(k1+1))-Phase_Modify(nn)+NCO_Phase_Modify),1);
Real_Freq_Error(nn)=fi-Freq_Word/factor*fs;
Real_Phase_AfterAccumu(nn)=2*pi*(Real_Freq_Error(nn)*(FLL_Initial_DataNo+k1+1)*N*Ts-0.5*N*Ts...22
*Real_Freq_Error(nn))+fai-fai1;
Real_PhaseErr_AfterAccumu(nn)=rem(Real_Phase_AfterAccumu(nn)+Phase_Modify(nn),2*pi);
Real_Phase_AfterAccumu(nn)=rem(Real_Phase_AfterAccumu(nn),2*pi);
%FLL跳轉條件
if abs(FLL_Freq_Dev(nn))<1 & abs(Phase_Error)<0.176 %phase=0.176對應10度 %phase=0.276對應15度 %abs(Freq_Err)<1.0e-2%
break;
end
%環路濾波器1
f_derivate=f_derivate_temp+w_nF_FLL^2*(2*T_IC)*FLL_Freq_Dev(nn);
f=f_temp+(2*T_IC)*f_derivate+sqrt(2)*w_nF_FLL*(2*T_IC)*FLL_Freq_Dev(nn);
f_derivate_temp=f_derivate;
%環路濾波器2
%alpha=0.90;
%f=alpha*FLL_Freq_Dev(nn)+(1-alpha)*f_temp;
delta_f=f-f_temp;
f_temp=f;
%頻率更新
update_f(nn+1)=f;
%寫入檔案
fprintf(fileid_FLL_track_err,'%d\t%f\t%f\t%f\t%f\t%f\t%f\r\n',k1,FLL_Freq_Dev(nn),delta_f,Real_Freq_Error(nn),...
Track_FlagValue,Track_FlagValue_Filtered,Track_Flag);
%螢幕顯示
fprintf('no.:%d, Real Frequency:%f freq_discriminator:%f',k1,Real_Freq_Error(nn),FLL_Freq_Dev(nn));
Freq_Discriminator_Filtered(nn)=delta_f;
fprintf(' freq_discriminator(filtered):%f\n',Freq_Discriminator_Filtered(nn));
fprintf('no.:%d ,Phase_Error(Discriminator):%f, phase(Modified):%f , Real Phase:%f\n',k1,Phase_Error,Phase(nn),Real_Phase_AfterAccumu(nn));
%NCO更新
Freq_Word=round((FLL_Initial_Freq+update_f(nn))*scaled_factor);
%NCO_Phase_Modify=Accumu(2*N)/factor*2*pi;
for mn=1:2*N
if mn==1
Accumu(mn)=Accumu(2*N)+Freq_Word;
else
Accumu(mn)=Accumu(mn-1)+Freq_Word;
end
if(Accumu(mn)>=factor)
Accumu(mn)=Accumu(mn)-factor;
end
sita_out(mn)=Accumu(mn)/factor*2*pi;
end
NCO_Phase_Modify=Accumu(1)/factor*2*pi;
end
end
status=fclose(fileid_FLL_track_err);
PLL_Initial_nn=nn;
PLL_Initial_Freq=Freq_Word/factor*fs;
Max_Num_FLL=k1;
Max_Update_Num_FLL=nn;
PLL_Initial_DataNo=FLL_Initial_DataNo+Max_Num_FLL-1;
%-----------------------------------------------------------------------------------
% PLL鎖相跟蹤環路
%-----------------------------------------------------------------------------------
%用PLL鎖頻環進行頻率跟蹤
f_FLL=PLL_Initial_Freq-f_ref;
fprintf('\n\nPLL初始頻率為: %f\n\n',f_FLL);
%參數初始化
PLL_Discriminator=zeros(1,Repeat_Num+1);
Phase_Filtered=zeros(1,Repeat_Num+1);
phase_error=zeros(1,Repeat_Num+1);%雙線性映射2階濾波器
f_filtered=zeros(1,Repeat_Num+1);%雙線性映射2階濾波器
NCO_Freq_Out=zeros(1,Repeat_Num+1);
n_sample=zeros(1,N);
Ips_Filtered=zeros(1,Repeat_Num+1);
Qps_Filtered=zeros(1,Repeat_Num+1);
%Demodulate_Data=zeros(Repeat_Num*3);
%3階J-R濾波器所用疊代變量及其初始化
phase_second_derivate=0;
phase_derivate=0;
phase_second_derivate_temp=2*pi*f_derivate;
phase_derivate_temp=2*pi*f;
phase=0;
phase_temp=0;
%載波鎖相環的初始角頻率
fprintf('\n--------------PLL tacking begin--------------\n');
%環路帶寬參數
m_band=0;
for B_LF_PLL=10:2:10
fprintf('目前計算的跟蹤環路帶寬為%f\r\n',B_LF_PLL);
%3階J-R濾波器所用疊代變量及其初始化
phase_second_derivate=0;
phase_derivate=0;
phase_second_derivate_temp=2*pi*f_derivate;
phase_derivate_temp=2*pi*f;
phase=0;
phase_temp=0;
m_band=m_band+1;
if WorkMode==1
filename=['CarrLoop_QuantiTh_TrackErr_',num2str(B_LF_PLL),'Hz','(3階環路)_NoQ_NoNoise.txt'];%,
elseif WorkMode==2
filename=['CarrLoop_QuantiTh_TrackErr_',num2str(sn_dB),'dB_',num2str(B_LF_PLL),'Hz','(3階環路)_NoQ.txt'];
elseif WorkMode==3
filename=['CarrLoop_QuantiTh_TrackErr_',num2str(sn_dB),'dB_',num2str(B_LF_PLL),'Hz','(3階環路).txt'];
end
file_track_err=fopen(filename,'w');
fprintf(file_track_err,'跟蹤帶寬為%f,載波相位跟蹤環跟蹤誤差（接收僞碼和本地僞碼相差%f碼片條件下),信噪比為%ddB',B_LF_PLL,(delayno-1)*8-Delay_Time,sn_dB);
fprintf(file_track_err,'多普勒頻率為%f，起始頻差%f\r\n',fd,m0*Freq_Step);
fprintf(file_track_err,'時間點\t鑒相器輸出\t濾波器輸出\t實際相差\t實際頻率差\t收斂判别量\t收斂判别量（濾波後）\t收斂标志\r\n');
w_nF_PLL=1.2*B_LF_PLL;%
Kd=1;
Kv=2*pi*fs/factor;
PLL_Loop_Gain=Kd*Kv;
damp_factor=0.707; %阻尼因子
c1=(8*damp_factor*w_nF_PLL)/(PLL_Loop_Gain*(4+4*damp_factor*w_nF_PLL*T_IC+(w_nF_PLL*T_IC)^2));
c2=(4*(w_nF_PLL^2*T_IC))/(PLL_Loop_Gain*(4+4*damp_factor*w_nF_PLL*T_IC+(w_nF_PLL*T_IC)^2));
%NCO頻率字轉換系數
Update_Freq_Word=0;
%初始載波NCO輸出
Accumulator=Accumu;
Freq_Word=round(PLL_Initial_Freq*scaled_factor);
Phase_Filtered(1)=update_f(PLL_Initial_nn);
for mn=1:N %注意這裡切換到鎖相環後是從FLL跟蹤環NCO輸出上一個點開始
if(Accumulator(mn)>=factor)
Accumulator(mn)=Accumulator(mn)-factor;
end
sita_out(mn)=Accumulator(mn)/factor*2*pi;
end
NCO_Phase_Modify=Accumulator(1)/factor*2*pi;
k2=0;
while k2<=Repeat_Num
k2=k2+1;
n_sample=[(PLL_Initial_DataNo+k2-1)*N:(PLL_Initial_DataNo+k2)*N-1];%注意這裡切換到鎖相環後是從FLL跟蹤環上一個點開始
s_I=A*cos(2*pi*n_sample*Ts*fi+fai);%輸入信号同相載波
s_Q=A*sin(2*pi*n_sample*Ts*fi+fai);%輸入信号正交載波
[Guass_Noise,Noise_Power]=NB_GuassNoise(n0,N,Noise_Bw);%高斯噪聲
if WorkMode==1
s_input=I_InputCode1.*s_I;
elseif WorkMode==2
s_input=I_InputCode1.*s_I+Guass_Noise;
elseif WorkMode==3
s_input=I_InputCode1.*s_I+Guass_Noise+(Data_Code_Samp.*Q_InputCode1).*s_Q;
end
s_input_Quanti=GPSChip_Out_Simula(s_input);
%s_input_Quanti=s_input;
%NCO參考信号
NCO_I=A_Ref*cos(sita_out(1:N));
NCO_Q=-A_Ref*sin(sita_out(1:N));
NCO_I_Quanti=round(NCO_I/NCO_Step)*NCO_Step;
NCO_Q_Quanti=round(NCO_Q/NCO_Step)*NCO_Step;
%NCO_I_Quanti=NCO_I;
%NCO_Q_Quanti=NCO_Q;
%下變頻
sdown_I=s_input_Quanti.*NCO_I_Quanti; %同相下變頻信号
sdown_Q=s_input_Quanti.*NCO_Q_Quanti; %正交下變頻信号
%積厘清除器的輸出
Ips_Array(k2)=sdown_I*I_LocalCode1'/N; %同相積厘清除器的輸出
Qps_Array(k2)=sdown_Q*I_LocalCode1'/N; %正交積厘清除器的輸出
Ips_Normalize_Array(k2)=Ips_Array(k2)/sqrt(Ips_Array(k2)^2+Qps_Array(k2)^2);
Qps_Normalize_Array(k2)=Qps_Array(k2)/sqrt(Ips_Array(k2)^2+Qps_Array(k2)^2);
%收斂判别标志
if k2~=1
Track_FlagValue=(Ips_Array(k2)*Ips_Array(k2-1)+Qps_Array(k2)*Qps_Array(k2-1))/(Ips_Array(k2)^2+Qps_Array(k2)^2);
Track_FlagValue_Filtered=0.9*Track_FlagValue_Filtered+0.1*Track_FlagValue;
if Track_FlagValue_Filtered>Track_Threshold
Track_Flag=1;
else
Track_Flag=0;
end
end
%解調資料
% for j1=1:3
% Demodulate_Data((k2-1)*3+j1)=(-1)*sdown_Q((j1-1)*Datarate_Ratio*fN+1:j1*Datarate_Ratio*fN)*...
% Q_LocalCode1((j1-1)*Datarate_Ratio*fN+1:j1*Datarate_Ratio*fN)'/(Datarate_Ratio*fN);
% end
Phase_Modify(k2)=(2*pi*Freq_Word/scaled_factor)*N*Ts*(PLL_Initial_DataNo+k2-1);
Real_Freq_Error(k2)=fi-Freq_Word/scaled_factor;
Real_Phase_AfterAccumu(k2)=2*pi*(Real_Freq_Error(k2)*(PLL_Initial_DataNo+k2)*N*Ts-0.5*N*Ts...
*Real_Freq_Error(k2))+fai-fai1;
%求真正的相位誤差(1ms重采樣輸出之後)
Real_Phase_Error_AfterAccumu(k2)=atan(Qps_Normalize_Array(k2)/Ips_Normalize_Array(k2));
%fprintf(' %f\r\n',Real_Phase_Error_AfterAccumu(k2));
%乘積鑒相器
% if (k2==1)
% Discriminator_Sign=1;
% else
% Discriminator_Sign=sign(Ips_Array(k2)*Ips_Array(k2-1)+Qps_Array(k2)*Qps_Array(k2-1));
% end
PLL_Discriminator(k2)=Qps_Array(k2)/sqrt((Qps_Array(k2)^2+Ips_Array(k2)^2));
PLL_Discriminator1(k2)=Qps_Normalize_Array(k2)/sqrt((Qps_Normalize_Array(k2)^2+Ips_Normalize_Array(k2)^2));
%atan鑒相器
%PLL_Discriminator(k2)=atan2(Qps_Array(k2)/Ips_Array(k2));
%PLL_Discriminator(k2)=atan2(Qps_Array(k2),Ips_Array(k2));
%螢幕顯示計算過程
fprintf('%d ',k2);
if(mod(k2,30)==0)
fprintf('\r\n');
end
%fprintf('no.:%d , Real_Freq_Error=%f , Ips=%f , Qps=%f \n',k2,Real_Freq_Error(k2)...
% ,Ips_Array(k2)/sqrt(Ips_Array(k2)^2+Qps_Array(k2)^2),Qps_Array(k2)/sqrt(Ips_Array(k2)^2+Qps_Array(k2)^2));
%退出
if k2>=Repeat_Num % abs(PLL_Discriminator(k2))<1.0e-5
break;
end
%環路濾波器1（2階濾波器，數字矩形積分器）
PLL_Discriminator(k2)=PLL_Discriminator(k2)/PLL_Loop_Gain;
phase_second_derivate=phase_second_derivate_temp+w_nF_PLL^3*T_IC*PLL_Discriminator(k2);
phase_derivate=phase_derivate_temp+T_IC*phase_second_derivate+2*w_nF_PLL^2*T_IC*PLL_Discriminator(k2);
phase=phase_temp+T_IC*phase_derivate+2*w_nF_PLL*T_IC*PLL_Discriminator(k2);
delta_phase=(phase-phase_temp);
phase_second_derivate_temp=phase_second_derivate;
phase_derivate_temp=phase_derivate;
phase_temp=phase;
Phase_Filtered(k2+1)=delta_phase/(2*pi*T_IC);
%環路濾波器2（1階濾波器，雙線性映射）
%phase_error(k2+1)=PLL_Discriminator(k2);
%f_filtered(k2+1)=f_filtered(k2)+c2*phase_error(k2+1);
%Phase_Filtered(k2+1)=f_filtered(k2+1)+c1*phase_error(k2+1);
%Phase_Filtered(k2+1)=Phase_Filtered(k2+1)/(2*pi*T_IC);
%環路濾波器3（普通1階濾波器）
%alpha=0.90;
%phase=(1-alpha)*PLL_Discriminator(k2)+alpha*phase_temp;
%PLL_Discriminator_Filtered(k2)=delta_phase;%/(2*pi*T_IC);
%fprintf('no.:%d , real phase:%f , PLL_discriminator:%f , PLL_dicriminator(filtered):%f\n',...
% k2,Real_Phase_AfterAccumu(k2),PLL_Discriminator(k2),Phase_Filtered(k2));
%fprintf('Track_FlagValue=%f,Track_FlagValue_Filtered=%f,Track_Flag=%d\n',...
% Track_FlagValue,Track_FlagValue_Filtered,Track_Flag);
%寫入檔案
fprintf(file_track_err,'%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\r\n',k2,PLL_Discriminator(k2),Phase_Filtered(k2),...
Real_Phase_Error_AfterAccumu(k2),Real_Freq_Error(k2),Track_FlagValue,Track_FlagValue_Filtered,Track_Flag);
%頻率更新
Update_Freq_Word=Kv*Phase_Filtered(k2)*scaled_factor;
Freq_Word=round(PLL_Initial_Freq*scaled_factor+Update_Freq_Word);
for mn=1:N
if mn==1
Accumulator(mn)=Accumulator(N)+Freq_Word;
else
Accumulator(mn)=Accumulator(mn-1)+Freq_Word;
end
if(Accumulator(mn)>=factor)
Accumulator(mn)=Accumulator(mn)-factor;
end
sita_out(mn)=Accumulator(mn)/factor*2*pi;
end
NCO_Phase_Modify=Accumulator(1)/factor*2*pi;
NCO_Freq_Out(k2)=Freq_Word/scaled_factor-4.092e+6;
end %for while k2<Repeat_Num
%關閉檔案
status=fclose(file_track_err);
end %for B_LF_PLL
end %for sn_dB
PLL_Track_Freq=Freq_Word/scaled_factor;
Freq_PLL=PLL_Track_Freq;
fprintf('\nPLL get doppler frequency=%f\n',PLL_Track_Freq-4.092e+6);
figure(5);
n=[0:N-1];
plot(n,s_I(1:N)/A,'r-',n,cos(sita_out(1:N)),'b-.');
ylabel('幅度');
xlabel('時間');
figure(6);
Max_Num_PLL=k2;
nx=[1:Max_Num_PLL];
plot(nx,Real_Phase_Error_AfterAccumu(1:Max_Num_PLL),'r-',nx,PLL_Discriminator(1:Max_Num_PLL),'b-.');
xlabel('跟蹤時間（ms）');
ylabel('相位（弧度）');
legend('真實殘餘相位','鑒相器估計的殘餘相位');
title('鑒相器估計的殘餘相位');
figure(7);
plot(nx,Ips_Normalize_Array(1:Max_Num_PLL),'r-',nx,Qps_Normalize_Array(1:Max_Num_PLL),'b-.');
xlabel('跟蹤時間（ms）');
ylabel('幅度');
title('I路和Q路的積厘清除器的輸出');
legend('I路','Q路');
figure(8);
plot(nx,NCO_Freq_Out(1:Max_Num_PLL)); 3.操作步驟與仿真結論
