easy7.m

GPS导航电文相关的计算程序

function  offset = easy7(ofile, navfile);
% EASY7 Estimation of receiver clock offset and position
%	    through batch processing. Data are read from
%	    the RINEX ofile.
%	    The processing is iterated three times due to
%	    non-linearity in the position determination

% Typical call: recclock('pta.96o', 'pta.nav')

%Kai Borre 27-07-2002
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 2002/07/27  $

if nargin == 0
    ofile = 'site247j.01o';
    navfile = 'site247j.nav';
end

v_light = 299792458;    % vacuum speed of light m/s
fid = fopen(ofile,'rt');
[Obs_types, ant_delta, ifound_types, eof1] = anheader(ofile);
if ((ifound_types == 0) | (eof1 == 1))
    error('Basic information is missing in RINEX file'), end;
NoObs_types = size(Obs_types,2)/2;

% Downloading of ephemeris data
Eph = get_eph(navfile);
j = fobs_typ(Obs_types,'P2');

fid = fopen(ofile,'rt');
[tr_RAW, dt, sv, eof2] = fepoch_0(fid);
NoSv = size(sv,1);
obs = grabdata(fid, NoSv, NoObs_types);
pr = obs(:,j);

% CALL OF BAYES FILTER FOR FIRST POSITION
pos = b_point(pr,sv,tr_RAW,Eph); % navfile
fprintf(['\nPreliminary position:\n X = %10.2f Y = %10.2f', ...
        ' Z = %10.2f\n\n'], pos(1),pos(2),pos(3))

for iteration = 1:3
    fid = fopen(ofile,'rt');
    reduced_normals = zeros(3,3);
    reduced_absolute = zeros(3,1);
    eTe = [];
    eTb = [];
    eTA = [];
    no_epochs = 0;
    
    for tt = 1:20
        [tr_RAW, dt, sv, eof2] = fepoch_0(fid);
        if (eof2 == 1), break, end
        NoSv = size(sv,1);
        for t = 1:NoSv
            col_Eph(t) = find_eph(Eph,sv(t),tr_RAW);
        end
        obs = grabdata(fid, NoSv, NoObs_types);
        pr = obs(:,j);
        % Formation of Observation Equations
        A = zeros(NoSv,3);
        omc = zeros(NoSv,1);
        for jsat = 1:NoSv
            k = col_Eph(jsat);
            tx_RAW = tr_RAW - pr(jsat)/v_light;
            Toc = Eph(21,k);
            dt = check_t(tx_RAW - Toc);
            a0 = Eph(19,k);
            a1 = Eph(20,k);
            a2 = Eph(2,k);
            tcorr = a0 + (a1 + a2*dt)*dt;
            tx_GPS = tx_RAW - tcorr;
            X = satpos(tx_GPS, Eph(:,k));
            traveltime = 70.e-3;	  % 70 ms first guess
            for iter = 1:2
                Rot_X = e_r_corr(traveltime, X);
                rho = norm(Rot_X - pos(1:3,1));
                traveltime = rho/v_light;
            end; % iter-loop
            [phi,lambda,h] = togeod(6378137, 298.257223563, ...
                pos(1,1), pos(2,1), pos(3,1));
            [az,el,dist] = topocent(Rot_X, Rot_X-pos(1:3,1));
            corrected_pseudorange = pr(jsat) - ...
                tropo(sin(el),h/1000,1013.0,293.0,50.0,0.0,0.0,0.0);
            dx = Rot_X(1) - pos(1,1);
            dy = Rot_X(2) - pos(2,1);
            dz = Rot_X(3) - pos(3,1);
            distance = norm([dx dy dz]);
            calculated_pseudorange = distance - v_light*tcorr;
            omc(jsat,1) = corrected_pseudorange - calculated_pseudorange;
            A(jsat,1) = -dx/distance;
            A(jsat,2) = -dy/distance;
            A(jsat,3) = -dz/distance;
        end; % jsat-loop
        % fprintf('\n omc  %12.3f', omc(:,1))
        
        % Formation of Normal Equations
        % We have	    NoSv     number of sv.s
        %		    b = omc  right side,    dimension NoSv by 1;
        %		    A			    dimension NoSv by 3;
        %		    sum(A)		    dimension 1 by NoSv;
        eTe = [eTe NoSv];
        eTb = [eTb sum(omc)];
        eTA = [eTA sum(A)'];
        reduced_normals = reduced_normals + A'*A-sum(A)'*sum(A)/NoSv;
        reduced_absolute = reduced_absolute + ...
            A'*omc - sum(A)'*sum(omc)/NoSv;
        no_epochs = no_epochs +1;
    end % while loop
    x = inv(reduced_normals)*reduced_absolute
    pos(1:3,1) = pos(1:3,1) + x;
end % iteration

fprintf(['\nFinal position:\n X = %10.2f Y = %10.2f', ...
        ' Z = %10.2f\n\n'], pos(1),pos(2),pos(3))

for epoch = 1:no_epochs
    rec_clk_offset(epoch) =  (eTb(epoch)-eTA(:,epoch)'*x)/ ...
        (eTe(epoch)*v_light);  % offset in seconds
end
offset = rec_clk_offset*1.e9;
%fprintf('\nOffsets in nanoseconds:\n')
%fprintf('%12.3f %12.3f %12.3f %12.3f %12.3f\n',offset)

plot(offset,'linewidth', 2)  % in nanoseconds
title('Receiver Clock Offset as Determined by Batch Processing',...
    'Fontsize',16)
xlabel('Epochs [1 s interval]','Fontsize',16)
ylabel('Clock offset [ns]','Fontsize',16)
set(gca,'YTick',378000:2000:382000)
set(gca,'YTickLabel',{'378000','380000','382000'})
set(gca,'Fontsize',16);
print -deps easy7 

fclose('all');
%%%%%%%%% end easy7.m %%%%%%%%%