GPS载波同步MATLAB仿真

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