clear all;close all;clc;
tic;
%%%%%%%%%%%%%%%%%%%initialize*********************
load data.mat data
svnum=1;fl=1575.42e6;c=299792458.0;
ph=50;%捕获得到的码相位
data=data(ph:5000*2800+ph-1);
a=pow2(32)/5e6;g=pow2(32);b=length(data);%载波NCO和码NCO均采用32位的
% NCOFCWinit=a*1.023e6*(1+4800/fl);%码NCO初始频率控制字
NCOFCWinit=a*1.023e6*(1+0/fl)
NCOacc=zeros(1,5001);codeclk=zeros(1,5001);%码钟
Qcomp_lo=zeros(1,5000); Icomp_lo=zeros(1,5000);
M=a*1.25e6;
% CNCOFCWinit=a*4800;%载波NCO初始频率控制字
CNCOFCWinit=a*0;
CNCOacc=zeros(1,5001);
NCOFCW=NCOFCWinit;CNCOFCW=CNCOFCWinit;
y(1)=0;x(1)=0;j=0;code=zeros(1,5001);prompt=zeros(1,5000);early=zeros(1,5000);
late=zeros(1,5000);ca=codegen1(svnum);inda=1;
% pll filter cofficients载波环环路滤波器系数
[p1,p2]=calculatePLLCoef(50,0.707,(2*pi)/a);
%code loop filter coefficients码环环路滤波器系数
[c1,c2]=calculatePLLCoef(1,0.707,1/(2*a));
% [c1,c2]=calculatePLLCoef(1,0.707,50);
for m=1:2800%code tracking
%产生1ms的本地码和本地载波
for i=1:5000
% i=(m-1)*5000+n;
NCOacc(i+1)=NCOacc(i)+NCOFCW;
%%% if (NCOacc(i+1)>hex2dec('FFFFFFFF'))
if (NCOacc(i+1)>4294967295)
%%% NCOacc(i+1) = NCOacc(i+1) - hex2dec('100000000');
NCOacc(i+1) = NCOacc(i+1) -4294967296 ;
end
%%% if (NCOacc(i+1)>=hex2dec('80000000'))
if (NCOacc(i+1)>=2147483648)
codeclk(i+1) = 0;
else
codeclk(i+1) = 1;
end
if ((codeclk(i+1)-codeclk(i))>0) %码钟上升沿
j=j+1;
if j>1023
j=j-1023;
end
code(i+1)=ca(j);%本地码生成
else
code(i+1)=code(i);
end
%generate I/Q(pnco=phase offset)
Icomp_lo(i)=sin(2*pi*(CNCOacc(i)+i*M)/g);%产生载波
Qcomp_lo(i)=cos(2*pi*(CNCOacc(i)+i*M)/g);
CNCOacc(i+1)=CNCOacc(i)+CNCOFCW;
NCOacc(1)=NCOacc(5001);CNCOacc(1)=CNCOacc(5001);code(1)=code(5001);
codeclk(1)=codeclk(5001);
end
data1=data((m-1)*5000+1:m*5000);%read data
%*****generate C/A codes
prompt=code(2:5001);%即时码
early=[prompt(4999:5000) prompt(1:4998)];%chip_delay=2;超前码
late=[prompt(3:5000) prompt(1:2)];%滞后码
Icomp=data1.*Icomp_lo;
Qcomp=data1.*Qcomp_lo;
%In_phase
IE(m)=sum(early.*Icomp)/5000;% early
IP(m)=sum(prompt.*Icomp)/5000;%prompt
IL(m)=sum(late.*Icomp)/5000;%late
%Quadrature
QE(m)=sum(early.*Qcomp)/5000;%early
QP(m)=sum(prompt.*Qcomp)/5000;%prompt
QL(m)=sum(late.*Qcomp)/5000;%late
EE(m)=IE(m).^2+QE(m).^2;%early
PP(m)=IP(m).^2+QP(m).^2;%prompt
LL(m)=IL(m).^2+QL(m).^2;%late
%costas loop for carrier tracking
d2(m)=atan(QP(m)./IP(m));%atan discriminator
% if m==30
% [p1,p2]=calculatePLLCoef(10,0.707,2*pi/a);
% % [p1,p2]=calculatePLLCoef(20,0.707,400*pi);
% end
%loop filter
if m<2
y(m)=0;
else
%filtered discriminator output
y(m)=y(m-1)+p1*d2(m)-p2*d2(m-1);
end
CNCOFCW=CNCOFCWinit+y(m);
E_L(m)=(LL(m)-EE(m));%超前减去滞后包络
if m<2
x(m)=0;
else
x(m)=x(m-1)+c1*E_L(m)-c2*E_L(m-1);
end
if m>=2000
PR1(inda)=NCOacc(i+1)*c/(g*1.023e6);
L1_m(inda)=CNCOacc(i)*c/(fl*g);
inda=inda+1;
end
%调整码环NCO的频率
NCOFCW=NCOFCWinit+x(m);
end
pr1=PR1(240:550);L1=L1_m(240:550);
C1sm=pr1;wmin=0.0;wvar=0.005;w=1-wvar;C2sm=pr1;
%begin observation time loop
for b=2:length(pr1)
w1=1/b;
C1sm(b)=w1*pr1(b)+(1-w1)*(C1sm(b-1)+L1(b)-L1(b-1));
C2sm(b)=w*pr1(b)+(1-w)*(C2sm(b-1)+L1(b)-L1(b-1));
w=w-wvar;
if w<wmin
w=wmin;
end
end
t1=[1:311];
y3=(0.91428)*t1+6.6091;y4=(0.91311)*t1+6.5899;
y1=(0.91282+8.6066e-007)*t1+6.6159-2.824e-005;y2=(0.91569+4.2217e-006)*t1+2049.3+0.029809;
subplot(2,1,1)
plot(d2),grid on
title(['载波环鉴相器输出'])
xlabel('时间t(单位:ms)','FontSize',9.0)
ylabel('鉴相器输出','FontSize',9.0)
subplot(2,1,2)
plot(y),grid on
title(['载波NCO频率控制字的改变'])
xlabel('时间t(单位:ms)','FontSize',9.0)
ylabel('频率控制字的改变值','FontSize',9.0)
figure(2)
subplot(2,1,1)
plot(E_L),grid on
title(['码环鉴相器输出'])
xlabel('时间t(单位:ms)','FontSize',9.0)
ylabel('鉴相器输出','FontSize',9.0)
subplot(2,1,2)
plot(x),grid on
title(['码NCO频率控制字的改变'])
xlabel('时间t(单位:ms)','FontSize',9.0)
ylabel('频率控制字的改变值','FontSize',9.0)
figure(3)
subplot(2,1,1)
plot(PR1);hold on
title(['伪距'])
ylabel('PR(单位:m)','FontSize',9.0)
subplot(2,1,2)
plot(L1_m),hold on
ylabel('L1-m(单位:m)','FontSize',9.0)
title('载波相位测量值')
% figure
% subplot(2,1,1)
% plot(PR1(240:550)-y1)
% ylabel('(单位:m)','FontSize',9.0)
% title(['伪距残差'])
% text(240,0.4,'标准差:0.1019m')
% subplot(2,1,2)
% plot(L1_m(240:550)-y2)
% ylabel('(单位:m)','FontSize',9.0)
% title(['载波相位测量值残差'])
% text(240,-4e-4,'标准差:0.0001855m')
figure(4)
subplot(2,1,1)
plot(C1sm),
title(['平滑伪距,w=1/k'])
ylabel('PR(单位:m)','FontSize',9.0)
subplot(2,1,2)
plot(C1sm-y3)
title(['残差,w=1/k'])
ylabel('(单位:m)','FontSize',9.0)
text(240,-0.08,'标准差:0.02818m')
% figure
% subplot(2,1,1)
% plot(C2sm)
% title(['平滑伪距,wvar=0.005'])
% ylabel('PR(单位:m)','FontSize',9.0)
% subplot(2,1,2)
% plot(C2sm-y4)
% title(['残差,wvar=0.005'])
% ylabel('(单位:m)','FontSize',9.0)
% text(240,-0.1,'标准差:0.09724m')
figure(5)
subplot(2,1,1)
plot(PR1(240:550)-C1sm)
title(['实时平滑前后的伪距差值,w=1/k'])
ylabel('平滑前后差值(单位:m)','FontSize',9.0)
subplot(2,1,2)
plot(PR1(240:550)-C2sm)
title(['实时平滑前后的伪距差值,wvar=0.005'])
ylabel('平滑前后差值(单位:m)','FontSize',9.0)
toc;
t=toc;