gogps 利用广播星历解算卫星位置matlab函数satellite_orbits详细注解版
主要注释了广播星历计算GPS BDS卫星位置的。
function [satp, satv] = satellite_orbits(t, Eph, sat, sbas)% SYNTAX:
% [satp, satv] = satellite_orbits(t, Eph, sat, sbas);
%
% INPUT:
% t = clock-corrected GPS time
% Eph = ephemeris matrix
% sat = satellite index
% sbas = SBAS corrections
%
% OUTPUT:
% satp = satellite position (X,Y,Z)
% satv = satellite velocity
%
% DESCRIPTION:
% Computation of the satellite position (X,Y,Z) and velocity by means
% of its ephemerides.%----------------------------------------------------------------------------------------------
% goGPS v0.4.3
%
% Copyright (C) 2009-2014 Mirko Reguzzoni, Eugenio Realini
%----------------------------------------------------------------------------------------------
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%----------------------------------------------------------------------------------------------switch char(Eph(31))case 'G'Omegae_dot = goGNSS.OMEGAE_DOT_GPS; % OMEGAE_DOT_GPS = 7.2921151467e-5; GPS Angular velocity of the Earth rotation [rad/s]GPS卫星地球轨道的角速度case 'R'Omegae_dot = goGNSS.OMEGAE_DOT_GLO;%OMEGAE_DOT_GLO = 7.292115e-5;GLONASS Angular velocity of the Earth rotation [rad/s]case 'E'Omegae_dot = goGNSS.OMEGAE_DOT_GAL;%OMEGAE_DOT_GAL = 7.2921151467e-5; Galileo Angular velocity of the Earth rotation [rad/s]case 'C'Omegae_dot = goGNSS.OMEGAE_DOT_BDS;%OMEGAE_DOT_BDS = 7.292115e-5; BeiDou Angular velocity of the Earth rotation [rad/s]case 'J'Omegae_dot = goGNSS.OMEGAE_DOT_QZS;%OMEGAE_DOT_QZS = 7.2921151467e-5; QZSS Angular velocity of the Earth rotation [rad/s]otherwisefprintf('Something went wrong in satellite_orbits.m\nUnrecongized Satellite system!\n');Omegae_dot = goGNSS.OMEGAE_DOT_GPS;
end%consider BeiDou time (BDT) for BeiDou satellites北斗时和GPS时对其相差14秒，减去
if (strcmp(char(Eph(31)),'C'))t = t - 14;
end%GPS/Galileo/BeiDou/QZSS satellite coordinates computation
%伽利略的坐标系不一样要单独算，这几个系统的坐标系基本可以看做一样的
if (~strcmp(char(Eph(31)),'R'))%get ephemeridesroota = Eph(4); %轨道长半径的平方根ecc = Eph(6); %卫星轨道偏心率omega = Eph(7); %近地点角距 cuc = Eph(8); %升交距角u的余弦调和改正项的振幅 c是改正数 u是升交距角 c是余弦cus = Eph(9); %升交距角u的正弦调和改正项的振幅 c是改正数 u是升交距角 s是正弦crc = Eph(10); %卫星至地心的距离r的余弦调和改正项的振幅 r是卫星到地心的距离crs = Eph(11); %卫星至地心的距离r的余弦调和改正项的振幅 r是卫星到地心的距离i0 = Eph(12); %轨道倾角IDOT = Eph(13); %轨道倾角i的变化率cic = Eph(14);%轨道倾角的余弦调和改正项的振幅 i是轨道倾角 c是余弦cis = Eph(15);%轨道倾角的正弦调和改正项的振幅 i是轨道倾角 s是余弦Omega0 = Eph(16);%参考时刻升交点的赤经Omega_dot = Eph(17);%升交点赤经的变化率toe = Eph(18);%参考时刻time_eph = Eph(32);%当前观测时刻%SBAS satellite coordinate correctionsif (~isempty(sbas))%如果有SBAS星基增强dx_sbas = sbas.dx(sat);dy_sbas = sbas.dy(sat);dz_sbas = sbas.dz(sat);elsedx_sbas = 0;dy_sbas = 0;dz_sbas = 0;end%-------------------------------------------------------------------------------% ALGORITHM FOR THE COMPUTATION OF THE SATELLITE COORDINATES% (IS-GPS-200E)卫星坐标计算算法%-------------------------------------------------------------------------------%eccentric anomaly 偏近点角 （专业术语）[Ek, n] = ecc_anomaly(t, Eph);%cr = goGNSS.CIRCLE_RAD;cr = 6.283185307179600;%2πA = roota*roota; %semi-major axis 半长轴 （星历给出的是半长轴开根号）tk = check_t(t - time_eph); %time from the ephemeris reference epoch 距离历书参考时刻的δtfk = atan2(sqrt(1-ecc^2)*sin(Ek), cos(Ek) - ecc); %true anomaly 真近点角 （专业术语） 用偏近点角算真近点角phik = fk + omega; %argument of latitude 升交角距phik = rem(phik,cr); %对2π取余uk = phik + cuc*cos(2*phik) + cus*sin(2*phik); %corrected argument of latitude 用广播星历里的摄动参数进行摄动改正rk = A*(1 - ecc*cos(Ek)) + crc*cos(2*phik) + crs*sin(2*phik); %corrected radial distance 卫星矢径的改正ik = i0 + IDOT*tk + cic*cos(2*phik) + cis*sin(2*phik); %corrected inclination of the orbital plane 卫星轨道倾角的摄动改正%satellite positions in the orbital plane 计算卫星在轨道面坐标系中的位置%坐标原点位于地心，X轴指向升交点%卫星位置从此刻开始出现。。前面的程序在算计算轨道面坐标系中卫星位置所需的角度 和 半径 x1k = cos(uk)*rk; %。。不懂这个符号定义的，为啥要加个k。。大概懂了。。表示当前时刻y1k = sin(uk)*rk; %if GPS/Galileo/QZSS or MEO/IGSO BeiDou satellite 北斗的地球静止轨道卫星也要分开算if (~strcmp(char(Eph(31)),'C') || (strcmp(char(Eph(31)),'C') && Eph(1) > 5))%corrected longitude of the ascending node 观测瞬间升交点的经度LOmegak = Omega0 + (Omega_dot - Omegae_dot)*tk - Omegae_dot*toe;Omegak = rem(Omegak + cr, cr);%satellite Earth-fixed coordinates (X,Y,Z) 卫星在瞬时地球坐标系中的位置%已知升交点的大地经度L以及轨道平面的倾角i后，可通过两次旋转方便求得卫星在地固坐标系中的位置%卫星的轨道平面是斜着在地球坐标系里，先转个i,转平了，再转个经度，转到地球坐标系的x轴指向的经度，轨道平面内的x轴是指向升交点的xk = x1k*cos(Omegak) - y1k*cos(ik)*sin(Omegak);yk = x1k*sin(Omegak) + y1k*cos(ik)*cos(Omegak);zk = y1k*sin(ik);%apply SBAS corrections (if available)satp(1,1) = xk + dx_sbas;satp(2,1) = yk + dy_sbas;satp(3,1) = zk + dz_sbas;else %if GEO BeiDou satellite (ranging code number <= 5) %地球静止轨道确实该和倾斜轨道分开%corrected longitude of the ascending node 观测瞬间升交点的经度LOmegak = Omega0 + Omega_dot*tk - Omegae_dot*toe;Omegak = rem(Omegak + cr, cr);%satellite coordinates (X,Y,Z) in inertial system xgk = x1k*cos(Omegak) - y1k*cos(ik)*sin(Omegak);ygk = x1k*sin(Omegak) + y1k*cos(ik)*cos(Omegak);zgk = y1k*sin(ik);%store inertial coordinates in a vectorXgk = [xgk; ygk; zgk];%rotation matrices from inertial system to%CGCS2000从惯性坐标系转到中国大地2000坐标系Rx = [1 0 0;0 +cosd(-5) +sind(-5);0 -sind(-5) +cosd(-5)];oedt = Omegae_dot*tk;Rz = [+cos(oedt) +sin(oedt) 0;-sin(oedt) +cos(oedt) 0;0 0 1];%apply the rotationsXk = Rz*Rx*Xgk;xk = Xk(1);yk = Xk(2);zk = Xk(3);%store CGCS2000 coordinatessatp(1,1) = xk;satp(2,1) = yk;satp(3,1) = zk;end%-------------------------------------------------------------------------------% ALGORITHM FOR THE COMPUTATION OF THE SATELLITE VELOCITY (as in Remondi,% GPS Solutions (2004) 8:181-183 )%-------------------------------------------------------------------------------if (nargout > 1)Mk_dot = n;Ek_dot = Mk_dot/(1-ecc*cos(Ek));fk_dot = sin(Ek)*Ek_dot*(1+ecc*cos(fk)) / ((1-cos(Ek)*ecc)*sin(fk));phik_dot = fk_dot;uk_dot = phik_dot + 2*(cus*cos(2*phik)-cuc*sin(2*phik))*phik_dot;rk_dot = A*ecc*sin(Ek)*Ek_dot + 2*(crs*cos(2*phik)-crc*sin(2*phik))*phik_dot;ik_dot = IDOT + 2*(cis*cos(2*phik)-cic*sin(2*phik))*phik_dot;Omegak_dot = Omega_dot - Omegae_dot;x1k_dot = rk_dot*cos(uk) - y1k*uk_dot;y1k_dot = rk_dot*sin(uk) + x1k*uk_dot;xk_dot = x1k_dot*cos(Omegak) - y1k_dot*cos(ik)*sin(Omegak) + y1k*sin(ik)*sin(Omegak)*ik_dot - yk*Omegak_dot;yk_dot = x1k_dot*sin(Omegak) + y1k_dot*cos(ik)*cos(Omegak) - y1k*sin(ik)*ik_dot*cos(Omegak) + xk*Omegak_dot;zk_dot = y1k_dot*sin(ik) + y1k*cos(ik)*ik_dot;satv(1,1) = xk_dot;satv(2,1) = yk_dot;satv(3,1) = zk_dot;endelse %GLONASS satellite coordinates computation (GLONASS-ICD 5.1)time_eph = Eph(32); %ephemeris reference timeX = Eph(5); %satellite X coordinate at ephemeris reference timeY = Eph(6); %satellite Y coordinate at ephemeris reference timeZ = Eph(7); %satellite Z coordinate at ephemeris reference timeXv = Eph(8); %satellite velocity along X at ephemeris reference timeYv = Eph(9); %satellite velocity along Y at ephemeris reference timeZv = Eph(10); %satellite velocity along Z at ephemeris reference timeXa = Eph(11); %acceleration due to lunar-solar gravitational perturbation along X at ephemeris reference timeYa = Eph(12); %acceleration due to lunar-solar gravitational perturbation along Y at ephemeris reference timeZa = Eph(13); %acceleration due to lunar-solar gravitational perturbation along Z at ephemeris reference time%NOTE: Xa,Ya,Za are considered constant within the integration interval (i.e. toe ?}15 minutes)%integration stepint_step = 60; %[s]%time from the ephemeris reference epochtk = check_t(t - time_eph);%number of iterations on "full" stepsn = floor(abs(tk/int_step));%array containing integration steps (same sign as tk)ii = ones(n,1)*int_step*(tk/abs(tk));%check residual iteration step (i.e. remaining fraction of int_step)int_step_res = rem(tk,int_step);%adjust the total number of iterations and the array of iteration stepsif (int_step_res ~= 0)n = n + 1;ii = [ii; int_step_res];end%numerical integration steps (i.e. re-calculation of satellite positions from toe to tk)pos = [X Y Z];vel = [Xv Yv Zv];acc = [Xa Ya Za];for s = 1 : n%Runge-Kutta numerical integration algorithm%%step 1pos1 = pos;vel1 = vel;[pos1_dot, vel1_dot] = satellite_motion_diff_eq(pos1, vel1, acc, goGNSS.ELL_A_GLO, goGNSS.GM_GLO, goGNSS.J2_GLO, goGNSS.OMEGAE_DOT_GLO);%%step 2pos2 = pos + pos1_dot*ii(s)/2;vel2 = vel + vel1_dot*ii(s)/2;[pos2_dot, vel2_dot] = satellite_motion_diff_eq(pos2, vel2, acc, goGNSS.ELL_A_GLO, goGNSS.GM_GLO, goGNSS.J2_GLO, goGNSS.OMEGAE_DOT_GLO);%%step 3pos3 = pos + pos2_dot*ii(s)/2;vel3 = vel + vel2_dot*ii(s)/2;[pos3_dot, vel3_dot] = satellite_motion_diff_eq(pos3, vel3, acc, goGNSS.ELL_A_GLO, goGNSS.GM_GLO, goGNSS.J2_GLO, goGNSS.OMEGAE_DOT_GLO);%%step 4pos4 = pos + pos3_dot*ii(s);vel4 = vel + vel3_dot*ii(s);[pos4_dot, vel4_dot] = satellite_motion_diff_eq(pos4, vel4, acc, goGNSS.ELL_A_GLO, goGNSS.GM_GLO, goGNSS.J2_GLO, goGNSS.OMEGAE_DOT_GLO);%%final position and velocitypos = pos + (pos1_dot + 2*pos2_dot + 2*pos3_dot + pos4_dot)*ii(s)/6;vel = vel + (vel1_dot + 2*vel2_dot + 2*vel3_dot + vel4_dot)*ii(s)/6;end%transformation from PZ-90.02 to WGS-84 (G1150)satp(1,1) = pos(1) - 0.36;satp(2,1) = pos(2) + 0.08;satp(3,1) = pos(3) + 0.18;%satellite velocitysatv(1,1) = vel(1);satv(2,1) = vel(2);satv(3,1) = vel(3);
end