matlab计算卫星位置及运行速度

%--------------------------------------------------------------------------
% 定义常量
%--------------------------------------------------------------------------
GM_WGS84 = 3986005e+8;            %WGS84引力常数
omega_e_WGS84 = 7.2921151467e-5;  %WGS84地球自转角速度
sqrt_a = 0.515365531e+4;          %电文中给出的卫星轨道椭圆长半轴的平方根
delta_n = 4.249105564e-9;         %电文中给出的平地点角校正值
toe = 2.448e+5;                   %电文中给出的参考时刻
M_zero = -1.064739758;            %电文中给出的参考时刻平近点角
es = 0.5912038265e-2;             %电文中给出的轨道椭圆偏心率
omega = -0.1717457876e+1;         %电文中给出的轨道近地点角距
Cus = 0.2237036824e-5;            %电文中给出的升交点角距正弦调和校正振幅
Cuc = 0.3054738045e-6;            %电文中给出的升交点角距余弦调和校正振幅
Cis = 0.8940696716e-7;            %电文中给出的轨道倾角正弦调和校正振幅
Cic = -0.8381903172e-7;           %电文中给出的轨道倾角余弦调和校正振幅
Crc = 0.35053125e+3;              %电文中给出的轨道半径余弦调和校正振幅
Crs = 0.253125e+1;                %电文中给出的轨道半径正弦调和校正振幅
i_zero = 0.9848407943;            %电文中给出的参考时刻轨道倾角
i_dot = 0.7422851197e-50;         %电文中给出的轨道倾角变化率
OMEGA_zero = 0.1038062244e+1;     %电文中给出的参考时刻升交点赤经
OMEGA_dot = -0.8151768125e-8;     %电文中给出的升交点赤经变化率
t = 2.390507223e+5;               %卫星信号发射时刻

%--------------------------------------------------------------------------
% 计算观测时刻GPS卫星在WGS84坐标系的坐标
%--------------------------------------------------------------------------
n_zero = sqrt（GM_WGS84） / （sqrt_a * sqrt_a * sqrt_a）;
n = n_zero + delta_n;               % 计算平均角速度

t_k = t - toe;
while （t_k > 302400）
    t_k = t_k - 604800;
end
while （t_k < -302400）
    t_k = t_k + 604800;
end                                 % 计算观测时刻到参考时刻的规化时间

M_k = M_zero + n * t_k + 2*pi;             % 观测时刻的卫星平近点角

E_k = M_k;
E_k1 = M_k - es*sin（E_k）;
while （abs（E_k - E_k1） > 1e-9）
    E_k1 = E_k;
    E_k = M_k + es * sin（E_k1）;
end;                                % 计算观测时刻的偏近点角

v_k = 2*atan（sqrt（（1+es）/（1-es））*tan（Ek/2））;    %计算真近点角

phi_k = v_k + omega;                % 计算升交距角