tracking.txt

该代码为gps软件接收机的跟踪环路的matlab实现程序

function [navi_filter] = tracking(channel_nr,data)
% Performs code- and carrier tracking

global gold
global settings;
global channel;

% ***** Find the parameters for the channel *****
fc=channel(channel_nr).AcqFreq;//捕获得到的载波频率
code_phase=channel(channel_nr).AcqCAPhase;//捕获的得到的初始相位
svnum=channel(channel_nr).SVN;//卫星号

% ***** Initialize the variables *****
real_freq=fc;
fs=11.999e6; % *** sampling freq
ts=1/fs; % *** samp ling time
n=fs/1000; % *** data pt in l ms
nn=[0:n-1]; % *** total no. of pts
nf=floor(size(data,1)/n); % *** number of frames to be processed
wfc=2*pi*fc;
code_offset=6;
code_disc_max=0.5;

code_change=4;
phi_prv=0;
temp=0;
pull_in_frames=0;
carrier_phase=0;
phase=0;

% ***** Compute the PLL coefficients for the Pull-in phase *****
[c1,c2] = calculatePllCoef(0.900, 150);

% ***** Generate the three local code replicas *****
gold_code(1,=[gold(svnum,(code_phase-code_offset):n) gold(svnum,1code_phase-code_offset-1))];
gold_code(2,=[gold(svnum,code_phase:n) gold(svnum,1:code_phase-1)];
gold_code(3,=[gold(svnum,(code_phase+code_offset):n) gold(svnum,1code_phase+code_offset-1))];

for frame=1:nf
% ***** Generate local sine and cosine replica *****
expcol=exp(j*(wfc*ts*nn+phase));
phase=angle(exp(j*(wfc*ts*n+phase)));
sine=imag(expcol);
cosine=real(expcol);

% ***** Take 1 ms of data from the input data *****
x = data((1:n)+((frame-1)*n),';

% ***** Convert the signal down to baseband *****
x_sine=x.*sine;
x_cosine=x.*cosine;

% ***** Correlate the baseband signal with the three local replicas *****
for i=1:3
I(i) = sum(x_sine.*gold_code(i,);
Q(i) = sum(x_cosine.*gold_code(i,);
out(frame,i) = I(i)^2;
out2(frame,i) = Q(i)^2;
end

% ***** Code and Phase discriminators *****
code_discri(frame) = ((I(1)^2+Q(1)^2)-(I(3)^2+Q(3)^2))/((I(1)^2+Q(1)^2)+(I(3)^2+Q(3)^2));
phase_discri(frame) = atan(Q(2)/I(2));

% ***** Save the navigation message *****
navi(frame)=I(2);

% ***** Used in debugging mode *****
wfc_frame(1,frame)=wfc;
wfc_frame(2,frame)=real_freq*2*pi;
if frame==1
code_disc(frame)=code_discri(frame);
phase_disc(frame)=phase_discri(frame)/2;
else
if code_disc(frame-1)==0
code_disc(frame-1)=code_discri(frame);
end
code_disc(frame)=code_disc(frame-1)*0.95+code_discri(frame)*0.05;
phase_disc(frame)=phase_disc(frame-1)*0.9+phase_discri(frame)*0.1;
end

% ***** Calculate the exact code phase *****

exact_code_phase(frame)=code_phase+(-1.3451*code_disc(frame)^3-2.7247*code_disc(frame));

% ***** Change to tracking mode if been in pull-in mode for 30 ms *****
pull_in_frames=pull_in_frames+1;
if pull_in_frames==30 [c1,c2] = calculatePllCoef(0.700, 15); end

% ***** Phase lock loop (PLL) *****
dfrq=c1*phase_discri(frame)+c2*phase_discri(frame)+temp;
temp=temp+c2*phase_discri(frame);
wfc=2*pi*fc+dfrq;
dfrq_frame(frame)=dfrq;

% ***** Change the local code replicas, if the discriminator is to large *****
if code_disc(frame) < -code_disc_max
code_phase=code_phase+code_change;
code_disc(frame)=0;
gold_code(1,=[gold(svnum,mod(code_phase-code_offset,n):n) ...
gold(svnum,1:mod(code_phase-code_offset-1,n))];
gold_code(2,=[gold(svnum,mod(code_phase,n):n) gold(svnum,1:mod(code_phase-1,n))];
gold_code(3,=[gold(svnum,mod(code_phase+code_offset,n):n) ...
gold(svnum,1:mod(code_phase+code_offset-1,n))];
elseif code_disc(frame) > code_disc_max
code_phase=code_phase-code_change;
code_disc(frame)=0;
gold_code(1,=[gold(svnum,mod(code_phase-code_offset,n):n) ...
gold(svnum,1:mod(code_phase-code_offset-1,n))];
gold_code(2,=[gold(svnum,mod(code_phase,n):n) gold(svnum,1:mod(code_phase-1,n))];
gold_code(3,=[gold(svnum,mod(code_phase+code_offset,n):n) ...
gold(svnum,1:mod(code_phase+code_offset-1,n))];
end

% ***** Used in debugging mode *****
code_phase_frame(frame)=code_phase;
end

% ***** Check if the tracking loop lost lock *****
lock_check_first=sum(out(1:10,2))/sum(out(1:10,1));
lock_check_last=sum(out(nf-10:nf,2))/sum(out(nf-10:nf,1));
phase_lock_check=sum(out(1:nf,2))/sum(out2(1:nf,2));
if (lock_check_first<1.2)
fprintf('No code lock from the beginning of the frame !\n');
end
if (lock_check_last<1.2)
fprintf('Possible lose of code lock !\n');
end
if (phase_lock_check<2)
fprintf('Possible phase lock error !\n');
end