%载波跟踪算法仿真
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));