程序介绍：
本程序分为三个函数以及一个主程序
读取导航电文：
function [gps_week, rate, SA_system, h, sec, bias, i, shift, cic, i0,... sqrt_a, cis, idot, sv_accuracy, code_on_l2_channel, iodc, sv_health,...crc, iode, tgd, crs, l2_p_data_flag, time1, cuc, m0, time2, current_line,...mins, tline, cus, month, toe, day, omega, year, delta_n, omega0, e ,...omega_dot, prn] = ReadNavFile(NavFileName)
%this function read the navgation file and output all the parameters
%2021/6/25
global gps_week rate SA_system h sec bias i shift cic i0 sqrt_a cis idot sv_accuracy code_on_l2_channel iodc sv_health crc iode tgd crs l2_p_data_flag time1 cuc m0 time2 current_line mins tline cus month toe day omega year delta_n omega0 e omega_dot prn;
D = fopen(NavFileName, 'r');
if D == -1
disp('the file does not exist. ')
return;
end
flag_end_of_header=0;
while flag_end_of_header==0
current_line=fgetl(D);
if strfind(current_line, 'END OF HEADER')
flag_end_of_header=1;
end
end
tline = fgetl(D);
i = 1;
while tline ~= -1
SA_system(i) = tline(1);
prn(i) = str2num(tline(2:3));
year(i) = str2num(tline(5:8));
month(i) = str2num(tline(10:11));
day(i) = str2num(tline(13:14));
h(i) = str2num(tline(16:17));
mins(i) = str2num(tline(19:20));
sec(i) = str2num(tline(22:23));
bias(i) = str2num(tline(24:42));
shift(i) = str2num(tline(43:61));
rate(i) = str2num(tline(62:80));
tline = fgetl(D);% line 2
iode(i) = str2num(tline(5:23));
crs(i) = str2num(tline(24:42));
delta_n(i) = str2num(tline(43:61));
m0(i) = str2num(tline(62:80));
tline = fgetl(D);% line 3
cuc(i) = str2num(tline(5:23));
e(i) = str2num(tline(24:42));
cus(i) = str2num(tline(43:61));
sqrt_a(i) = str2num(tline(62:80));
tline = fgetl(D);% line 4
toe(i) = str2num(tline(5:23));
cic(i) = str2num(tline(24:42));
omega0(i) = str2num(tline(43:61));
cis(i) = str2num(tline(62:80));
tline = fgetl(D);% line 5
i0(i) = str2num(tline(5:23));
crc(i) = str2num(tline(24:42));
omega(i) = str2num(tline(43:61));
omega_dot(i) = str2num(tline(62:80));
tline = fgetl(D);% line 6
idot(i) = str2num(tline(5:23));
code_on_l2_channel(i) = str2num(tline(24:42));
gps_week(i) = str2num(tline(43:61));
l2_p_data_flag(i) = str2num(tline(62:80));
tline = fgetl(D);% line 7
sv_accuracy(i) = str2num(tline(5:23));
sv_health(i) = str2num(tline(24:42));
tgd(i) = str2num(tline(43:61));
iodc(i) = str2num(tline(62:80));
tline = fgetl(D);% line 8
time1(i) = str2num(tline(5:23));
time2(i) = str2num(tline(24:42));
i = i+1;
tline = fgetl(D);% line
end
读取观测文件：
function [p1, p2, elevation, mjd, sod, pr] = ReadObsFile(ObsFileName)
%this function read the data in observation file
%p1, p2 is the observation distance
%elevation is in degree unit
%mjd and sod is the receive time
P = fopen(ObsFileName, 'r');
if P == -1
disp('the file does not exist. ')
return;
end
tline = fgetl(P);
i = 1;
while tline ~= -1
mjd(i) = str2num(tline(1:5) );
sod(i) = str2num(tline(7:15));
pr(i) = str2num(tline(23:24));
p1(i) = str2num(tline(233:245));
p2(i) = str2num(tline(255:267));
elevation(i) = str2num(tline(71:78));
i = i+1;
tline = fgetl(P);% line
end
基于输入时间与卫星PRN号计算卫星位置、钟差以及设备时延
function [sa_x, sa_y, sa_z, sa_t, system_delay] = CalSaPos(mjd1, pr_num)
%this funtion input the time and prn num of satellite
%output the x, y, z coordinates satellite clock error t and system delay time
format long;
mu = 3.986004415e14;
omega_e_deri = 7.2921151467e-5;
global tgd rate SA_system h sec bias shift cic i0 sqrt_a cis idot crc crs time1 cuc m0 mins cus month toe day omega year delta_n omega0 e omega_dot prn;
% calculate the position of SAs%%%%%%%%%%%%%%%%%%%%
A = ((sqrt_a)).^2 ;
n0 = sqrt(mu./(A).^3);
n = n0 + delta_n ;
M0 = m0;
t = time1;
[one, epochs] = size(A);
%select nearst epoches based on the input time (LOCAL NEAREST)
%devide different SAs
class = 0;
for mmm = 1: epochs-1
if(prn(mmm) ~= prn(mmm+1))
class = class+1;
devide(class) = mmm;
end
end
class = class +1;
devide(class) = epochs;
MJD = mjd1;
week = floor((MJD - 44244.0)/7);
sow = round((MJD - week*7 - 44244.0)*86400);
if (sow == 604800)
sow = 0;
week = week+1;
end
h_r = h + mins/60 + sec/3600;
MJDS = 1721013.5 + 367.*year - floor(7/4.*(year + floor((month+9)/12))) + day +h_r/24 + floor(275.*month/9) - 2400000.5;
delta_mjd = roundn(abs(MJD - MJDS), -7);
index = 1;
pos = 1;
for kkk = 1: class
[value, target(index)] = min(delta_mjd(pos : devide(kkk)));
pos = devide(kkk)+1;
index = index+1;
end
index = index - 1;
target(2 : 32) = target(2 : 32) + devide(1: 31);
toe_ = toe(target);%toe记录了所有历元的toe，即437个，使用toe(target)便只取其中符合最近历元条件的toe
tk = sow - toe_;
n_ = n(target);
Mk = M0(target) + n_.*tk;
e_ =e(target);
% calculate the Ek using iterative method%%%%%%%%%%%%%
for hh = 1:index
Ek(hh) = Mk(hh);
E0(hh) = Ek(hh);
Ek(hh) = Mk(hh) + e_(hh)*sin(E0(hh));
while(abs(Ek(hh) - E0(hh))> 1e-12)
E0(hh) = Ek(hh);
Ek(hh) = Mk(hh) + e_(hh)*sin(E0(hh));
end
end
sin_vk =(sqrt(1-e(target).^2).*sin(Ek)./(1 - e(target).*cos(Ek)));
cos_vk = ((cos(Ek) - e(target))./(1 - e(target).*cos(Ek)));
vk = atan2(sin_vk, cos_vk);
for iii = 1: index
if vk(iii) < 0
vk(iii) = vk(iii) + 2*pi;
end
end
phik = vk + omega(target);
delta_uk = cus(target).*sin(2.*phik) + cuc(target).*cos(2.*phik);
delta_rk = crs(target).*sin(2.*phik) + crc(target).*cos(2.*phik);
delta_ik = cis(target).*sin(2.*phik) + cic(target).*cos(2.*phik);
uk = phik + delta_uk;
rk = A(target).*(1 - e(target).*cos(Ek)) + delta_rk;
ik = i0(target) + idot(target).*tk + delta_ik;
xk = rk.*cos(uk);
yk = rk.*sin(uk);
omegak = omega0(target) + (omega_dot(target) - omega_e_deri).*tk - omega_e_deri.*toe(target);
Xk = xk.*cos(omegak) - yk.*cos(ik).*sin(omegak);
Yk = xk.*sin(omegak) + yk.*cos(ik).*cos(omegak);
Zk = yk.*sin(ik);
%calculate the clock bias%%%%%%%%%%%%%%%%%%%%%%
a_0 = bias(target);
a_1 = shift(target);
a_2 = rate(target);
C = 2.99792458e+8;
F = -2*sqrt(mu)/(C^2);
delta_tsv = a_0 + a_1.*(sow - toe(target)) + a_2.*(sow - toe(target)).^2 + F.*e(target).*sqrt(A(target)).*sin(Ek);
SA_system_ = SA_system(target);
year_ = year(target);
month_ = month(target);
day_ = day(target);
h_ = h(target);
min_ = mins(target);
sec_ = sec(target);
gpst = toe(target);
TGD = tgd(target);
p = pr_num;
sa_x= Xk(p) ;
sa_y = Yk(p) ;
sa_z = Zk(p);
sa_t = delta_tsv(p);
system_delay = TGD(p);
end
主程序：主要思路是串联前三个函数得到的结果，迭代计算卫星位置， 并进行地球自转改正，以及平差。
其伪代码如下：
clear;
C = 2.99792458e+8;
w = 7.2921151467E-5 ;
%read navgation file and observation file
NavFileName = 'brdc0940.21n';
ObsFileName = 'omc_2021094_ABMF';
[p1, p2, elevation, mjd, sod, pr] = ReadObsFile(ObsFileName);
[gps_week, rate, SA_system, h, sec, bias, i, shift, cic, i0,...
sqrt_a, cis, idot, sv_accuracy, code_on_l2_channel, iodc, sv_health,...
crc, iode, tgd, crs, l2_p_data_flag, time1, cuc, m0, time2, current_line,...
mins, tline, cus, month, toe, day, omega, year, delta_n, omega0, e ,...
omega_dot, prn] = ReadNavFile(NavFileName);
%devide epochs
delta_rou = p1 - p2;
rou_real = 1.54573*delta_rou + p1;
p = mjd + sod/86400;
olo = 1;
devides_epoch(olo) = 1;
olo = olo + 1;
for jj = 2:length(p1)
if(p(jj) ~= p(jj-1))
devides_epoch(olo) = jj;
olo = olo+1;
end
end
%calculate the numbers of each epoch
for opp = 1:length(devides_epoch)-1
lengths_epoch(opp) = devides_epoch(opp+1) - devides_epoch(opp);
end
lengths_epoch(opp+1) = 9;
for tps = 1: 25529
if (elevation(tps) >= 30)
p(tps) = 1;
else
p(tps) = 1/2*(sin(elevation(tps)/180*pi));
end
end
%%
for ks = 1: 2880
index = [devides_epoch(ks):devides_epoch(ks)+lengths_epoch(ks)-1];
if(ks == 1)
Xr = 2919785.795;
Yr = -5383744.941;
Zr = 1774604.873;
Tr = 0;
% Xr =0; Yr = 0; Zr = 0;
end
dists = [];
xa_rot_corr = [];
ya_rot_corr = [];
za_rot_corr = [];
sa_t = [];
system_delay = [];
for tt =1:lengths_epoch(ks)
mjd_sa = mjd(index(tt));
sod_sa = sod(index(tt));
mjd_origin = mjd_sa + sod_sa/86400;
[sa_x(tt), sa_y(tt), sa_z(tt), sa_t(tt), system_delay(tt)] = CalSaPos(mjd_origin, pr(index(tt)));
dist1 = sqrt((sa_x(tt) - Xr).^2+ (sa_y(tt) - Yr).^2+ (sa_z(tt) - Zr).^2);
trans_t = dist1/C;
mjd1 = mjd_origin - trans_t/86400;
[sa_x(tt), sa_y(tt), sa_z(tt), sa_t(tt), system_delay(tt)] = CalSaPos(mjd1, pr(index(tt)));
dist2 = sqrt((sa_x(tt) - Xr).^2+ (sa_y(tt) - Yr).^2+ (sa_z(tt) - Zr).^2);
delta_tou = (dist1 - dist2)/C;
while(abs(delta_tou) >= 1e-8)
dist1 = dist2;
trans_t = dist1/C;
mjd1 = mjd_origin - trans_t/86400;
[sa_x(tt), sa_y(tt), sa_z(tt), sa_t(tt), system_delay(tt)] = CalSaPos(mjd1, pr(index(tt)));
dist2 = sqrt((sa_x(tt) - Xr).^2+ (sa_y(tt) - Yr).^2+ (sa_z(tt) - Zr).^2);
delta_tou = (dist1 - dist2)/C;
end
tou = dist2/C;
%earth rotation adjust
xa_rot_corr(tt) = sa_x(tt) + w*tou*sa_y(tt) ;
ya_rot_corr(tt) = sa_y(tt) - w*tou*sa_x(tt);
za_rot_corr(tt) = sa_z(tt);
dists(tt) = sqrt((xa_rot_corr(tt) - Xr).^2+ (ya_rot_corr(tt) - Yr).^2+ (sa_z(tt) - Zr).^2);
Rotate_erorr_x(index(tt)) = xa_rot_corr(tt) - sa_x(tt);
Rotate_erorr_y(index(tt)) = ya_rot_corr(tt) - sa_y(tt);
end
k = (xa_rot_corr - Xr)./(dists);
l = (ya_rot_corr - Yr)./(dists);
m = (za_rot_corr -Zr)./(dists);
dtrop=2.47./sind(elevation(index))+0.0121;%
L = rou_real(index) - dists + C*sa_t +C*system_delay+ dtrop;
p_epoch = p(index);
P = diag([p_epoch]);
A = [-k' -l' -m' ones(length(index),1)];
n = A'*P*A;
W = A'*P*L';
X = inv(n)*W;
V = A*X - L';
r = length(index) -4;
sigma0(ks) = sqrt(V'*P*V/r);
Xr = Xr+X(1);
Yr = Yr+ X(2);
Zr = Zr+X(3);
Tr = X(4);
Xs(ks) = Xr;
Ys(ks) = Yr;
Zs(ks) = Zr;
Qxx(:,:,ks) = inv(n);
Q = inv(n);
GDOP(ks) = sqrt(Q(1,1) + Q(2,2) + Q(3,3) + Q(4,4));
PDOP(ks) = sqrt(Q(1,1) + Q(2,2) + Q(3,3));
TDOP(ks) = sqrt(Q(4,4));
end
