%% 根据广播的卫星星历结算卫星的瞬时位置
% 输入:
% satID -> Satellite PRN
% eph_epoch -> Pointer to the proper ephemerides paragraph
% receipt_time -> Time when the signal reached the receiver
% transit_time -> Signal transit time from a specific satellite to the receiver.
% mEphTime (global) -> Time tag of each ephemerides paragraph
% mEphemerides (global) -> Matrix having overall ephemerides parameters
% 输出:
% vSatPos -> 卫星坐标,未旋转
% sat_clock_cor_new -> 卫星钟差校正
% group_delay -> 全局延迟
function [vSatPos, sat_clock_cor, group_delay]= GetSatPos(satID,eph_epoch,receipt_time,transit_time)
global mEphemerides
%%
Pi = 3.1415926535898;
GM = 3.986008e14; % 地心引力常数 m^3/s^2
omega_ie = 7.2921151467e-5; % 地球旋转率rad/s
F = -4.442807633e-10; %相对论效应矫正常数
%% 读取星历参数
M0 = mEphemerides(satID,eph_epoch,9);
sqrt_a = mEphemerides(satID,eph_epoch,13);
delta_n = mEphemerides(satID,eph_epoch,8);
e = mEphemerides(satID,eph_epoch,11);
omega = mEphemerides(satID,eph_epoch,20);
Cuc = mEphemerides(satID,eph_epoch,10);
Cus = mEphemerides(satID,eph_epoch,12);
Crc = mEphemerides(satID,eph_epoch,19);
Crs = mEphemerides(satID,eph_epoch,7);
i0 = mEphemerides(satID,eph_epoch,18);
idot = mEphemerides(satID,eph_epoch,22);
Cic = mEphemerides(satID,eph_epoch,15);
Cis = mEphemerides(satID,eph_epoch,17);
Omega0 = mEphemerides(satID,eph_epoch,16);
Omega_dot = mEphemerides(satID,eph_epoch,21);
toe = mEphemerides(satID,eph_epoch,14);
toc = mEphemerides(satID,eph_epoch,2);
a_f0 = mEphemerides(satID,eph_epoch,3);
a_f1 = mEphemerides(satID,eph_epoch,4);
a_f2 = mEphemerides(satID,eph_epoch,5);
TGD = mEphemerides(satID,eph_epoch,28);
%% 卫星信号开始传输时的时间 = 到达接收机时间 - 信号传播的时间
transmit_time = receipt_time-transit_time;
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 计算卫星的坐标 %%%%%%%%%%%%%%%%%%%%%%%%
%% 椭圆长半径
a = sqrt_a*sqrt_a;
%% 计算归一化时间
tk = Check_t(transmit_time-toe);
%% n0
n0 = sqrt(GM/a^3);
%% 平均角速度
n = n0 + delta_n;
%% tk时刻平近点角
Mk = M0 + n*tk;
Mk = rem(Mk + 2*Pi, 2*Pi);
%% tk时刻偏近点角的迭代解算
Ek = Mk;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for iter = 1:10
Ek_old = Ek;
Ek = Mk + e*sin(Ek);
dEk = rem(Ek-Ek_old,2*Pi);
if abs(dEk) < 1.e-12
break;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% iter=0; %迭代次数
% while 1
% iter = iter+1;
% Ek_old = Ek;
% Ek = Mk + e*sin(Ek);
% dEk = rem(Ek-Ek_old,2*Pi);
%
% if abs(dEk) < 1.0e-12
% break;
% end
% if iter > 10
% error('卫星位置解算不收敛!!!');
% end
% end
%% 0-360 deg
Ek = rem(Ek+2*Pi, 2*Pi);
%% 地心向径
rk = a*(1-e*sin(Ek));
%% tk时刻真近点角
fk = atan2(sqrt(1-e^2)*sin(Ek), cos(Ek)-e);
%% 升交点角距
phi_k = fk + omega;
phi_k = rem(phi_k,2*Pi);
%% 摄动校正项
delta_uk = Cuc*cos(2*phi_k)+Cus*sin(2*phi_k);
delta_rk = Crc*cos(2*phi_k)+Crs*sin(2*phi_k);
delta_ik = Cic*cos(2*phi_k)+Cis*sin(2*phi_k);
%%
uk = phi_k + delta_uk; %摄动矫正后升交点角距
rk = rk + delta_rk; %卫星地心向径
ik = i0 + idot*tk +delta_ik; %卫星轨道倾角
%% Longitude for ascending node
Omega_k = Omega0+(Omega_dot-omega_ie)*tk-omega_ie*toe;
Omega_k = rem(Omega_k+2*Pi,2*Pi);
%% Rotation matrix
mRotation=[cos(Omega_k) -sin(Omega_k)*cos(ik) sin(Omega_k)*sin(ik);
sin(Omega_k) cos(Omega_k)*cos(ik) -cos(Omega_k)*sin(ik);
0 sin(ik) cos(ik) ];
%% 卫星位置(未旋转)
vSatPos=mRotation*[rk*cos(uk); rk*sin(uk); 0 ];
%% %%%%%%%%%%%%%%%%%% 卫星钟差校正 %%%%%%%%%%%%%%%%%%
%% dt
dt = transmit_time- toc;
%% 归一化时间
dt = Check_t(dt);
%% 相对论效应校正
rel= F*e*sqrt_a*sin(Ek);
%% 卫星钟差校正
sat_clock_cor = a_f0 + a_f1*dt + a_f2*dt^2 + rel;
%% 群延迟估计
group_delay = TGD;
评论0