根据星历计算卫星位置

admin

[mw_shl_code=applescript,true]function main
clear;
clc;
clear all;

%%

% 星历 参考GPS原理及接收机设计 3.4

eph.rcvr_tow = 100;
eph.sqrtA = 5153.65531;     %长半轴开平方根
eph.toe = 244800;           %toe为星历参考时间
eph.dn = 4.249105564E-9;
eph.m0 = -1.064739758;      %toe时的平近点角
eph.e = 0.005912038265;     %偏心率
eph.w = -1.717457876;       %近地点角距
eph.i0 = 0.9848407943;      %toe时的轨道倾角
eph.idot = 7.422851197E-51; %轨道倾角对时间的变化率
eph.omg0 = 1.038062244;     %升交点赤经
eph.odot = -8.151768125E-9;
eph.cus = 2.237036824E-6;
eph.cuc = 3.054738045E-7;
eph.crs = 2.53125;
eph.crc = 350.53125;
eph.cis = 8.940696716E-8;
eph.cic = -8.381903172E-8;

%下面是c代码里的参数
% eph.sqrtA = 5153.673528671;              %长半轴开平方根
% eph.m0    = -0.0096998666212359998;      %toe时的平近点角
% eph.e     = 0.0076935507822779997;
% eph.i0    = 0.97102462005589996;         %toe时的轨道倾角
% eph.w     = 0.61974934089910005;         %近地点角距
% eph.omg0  = -2.3710541493399999;
% eph.toe   = 28800;
% t=28802;

%[x y z] = get_satellite_position(eph,t, 1);
[x y z] = get_satellite_position(eph, 239050.7223, 1);
pos = [x y z]
end

function [x y z] = get_satellite_position(eph, t, compute_harmonic_correction)
% get_satellite_position: computes the position of a satellite at time (t) given the
% ephemeris parameters.
% Usage: [x y z] =  get_satellite_position(eph, t, compute_harmonic_correction)
% Input Args: eph: ephemeris data
%             t: time
%             compute_harmonic_correction (optional): 1 if harmonic
%             correction should be applied, 0 if not.
% Output Args: [x y z] in ECEF in meters
% ephmeris data must have the following fields:
% rcvr_tow (receiver tow)
% svid (satellite id)
% toc (reference time of clock parameters)
% toe (referece time of ephemeris parameters)
% af0, af1, af2: clock correction coefficients
% ura (user range accuracy)
% e (eccentricity)
% sqrtA (sqrt of semi-major axis)
% dn (mean motion correction)
% m0 (mean anomaly at reference time)
% w (argument of perigee)
% omg0 (lontitude of ascending node)
% i0 (inclination angle at reference time)
% odot (rate of right ascension)
% idot (rate of inclination angle)
% cus (argument of latitude correction, sine)
% cuc (argument of latitude correction, cosine)
% cis (inclination correction, sine)
% cic (inclination correction, cosine)
% crs (radius correction, sine)
% crc (radius correction, cosine)
% iod (issue of data number)

% set default value for harmonic correction
switch nargin
    case 2
        compute_harmonic_correction=1;
end
mu = 3.986005e14;                               %地球引力常数GM
omega_dot_earth = 7.2921151467e-5; %(rad/sec)   %地球自转角速度

%-----------step 1:计算规划时间----------------%
tk = t - eph.toe;
% account for beginning of end of week crossover
if (tk > 302400)
    tk = tk-604800;
end
if (tk < -302400)
    tk = tk+604800;
end

%-----------step 2:计算卫星平均角速度----------------%
% Now follow table 20-IV
A = eph.sqrtA^2;
cmm = sqrt(mu/A^3); % computed mean motion
% apply mean motion correction
n = cmm + eph.dn;
%-----------step 3:计算平近点角----------------%
% Mean anomaly
mk = eph.m0 + n*tk;
%-----------step 4:计算偏近点角----------------%
% solve for eccentric anomaly
% 迭代3次
Ek = mk;
Ek = mk + eph.e*sin(Ek);
Ek = mk + eph.e*sin(Ek);
Ek = mk + eph.e*sin(Ek);

% syms E;
% eqn = E - eph.e*sin(E) == mk;
% solx = vpasolve(eqn, E);
% Ek = double(solx);

%-----------step 5:计算真近点角----------------%
% True anomaly:
nu = atan2((sqrt(1-eph.e^2))*sin(Ek)/(1-eph.e*cos(Ek)), (cos(Ek)-eph.e)/(1-eph.e*cos(Ek)));
%Ek = acos((e  + cos(nu))/(1+e*cos(nu)));
%-----------step 6:计算升交距角----------------%
%升交点角距
Phi = nu + eph.w;
%-----------step 7:计算摄动校正量----------------%
du = 0;
dr = 0;
di = 0;
% 摄动校正量
if (compute_harmonic_correction == 1)
    % compute harmonic corrections
    du = eph.cus*sin(2*Phi) + eph.cuc*cos(2*Phi);
    dr = eph.crs*sin(2*Phi) + eph.crc*cos(2*Phi);
    di = eph.cis*sin(2*Phi) + eph.cic*cos(2*Phi);
end
%-----------step 8:计算摄动校正后的升交距角u,卫星矢径r和轨道倾角i----------------%
u = Phi + du;
r = A*(1-eph.e*cos(Ek)) + dr;
% inclination angle at reference time
i = eph.i0 + eph.idot*tk + di;

%-----------step 9:计算卫星在轨道平面位置----------------%
x_prime = r*cos(u);
y_prime = r*sin(u);
%-----------step 10:计算卫星的升交点赤经----------------%
omega = eph.omg0 + (eph.odot - omega_dot_earth)*tk - omega_dot_earth*eph.toe;

%-----------step 10:将轨道平面直角坐标系转变成WGS-84地心地固直角坐标系----------------%
x = x_prime*cos(omega) - y_prime*cos(i)*sin(omega);
y = x_prime*sin(omega) + y_prime*cos(i)*cos(omega);
z = y_prime*sin(i);

end[/mw_shl_code]