You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

153 lines
5.4 KiB

function [xyzM,dtsvS] = GpsEph2Xyz(gpsEph,gpsTime)
%[xyzM,dtsvS] = GpsEph2Xyz(gpsEph,gpsTime)
% Calculate sv coordinates, in ECEF frame, at ttx = gpsTime
%
% Inputs:
% gpsEph: vector of GPS ephemeris structures, as defined in ReadRinexNav.m
%
% gpsTime = [gpsWeek, ttxSec]: GPS time at time of transmission, ttx
% ttx = trx - PR/c - dtsvS, where trx is time of reception (receiver clock),
% dtsvS is the satellite clock error (seconds), can be computed in advance
% using Eph2Dtsv.m or iterating this function: gps time = sat time - dtsvS
% gpsWeek, ttxSec must be vectors of length(gpsEph),
%
% outputs:
% xyzM = [i,j,k] matrix of coordinates of satellites (ecef meters)
% dtsvS = vector of satellite clock error (seconds)
% The row dimension of xyzM, dtsvS = length(gpsEph)
%
% xyzM = sat positions at time ttxSec, in terms of ecef coords at the same time
% Use FlightTimeCorrection.m to get xyzM & v in ecef coords at time of reception
%
% See IS-GPS-200 for details of data
%Author: Frank van Diggelen
%Open Source code for processing Android GNSS Measurements
xyzM=[]; dtsvS=[];
[bOk,gpsEph,gpsWeek,ttxSec] = CheckGpsEphInputs(gpsEph,gpsTime);
if ~bOk
return
end
p=length(gpsEph);
%Now we are done checking and manipulating the inputs
% the time vectors: gpsWeek, ttxSec are the same length as gpsEph
%-------------------------------------------------------------------------------
%set fitIntervalSeconds
fitIntervalHours = [gpsEph.Fit_interval]';
%Rinex says "Zero if not known", so adjust for zeros
fitIntervalHours(fitIntervalHours == 0) = 2;
fitIntervalSeconds = fitIntervalHours*3600;
%Extract variables from gpsEph, into column vectors
%orbit variable names follow RINEX 2.1 Table A4
%clock variables af0, af1, af2 follow IS GPS 200
TGD = [gpsEph.TGD]';
Toc = [gpsEph.Toc]';
af2 = [gpsEph.af2]';
af1 = [gpsEph.af1]';
af0 = [gpsEph.af0]';
Crs = [gpsEph.Crs]';
Delta_n = [gpsEph.Delta_n]';
M0 = [gpsEph.M0]';
Cuc = [gpsEph.Cuc]';
e = [gpsEph.e]';
Cus = [gpsEph.Cus]';
Asqrt = [gpsEph.Asqrt]';
Toe = [gpsEph.Toe]';
Cic = [gpsEph.Cic]';
OMEGA = [gpsEph.OMEGA]';
Cis = [gpsEph.Cis]';
i0 = [gpsEph.i0]';
Crc = [gpsEph.Crc]';
omega = [gpsEph.omega]';
OMEGA_DOT=[gpsEph.OMEGA_DOT]';
IDOT = [gpsEph.IDOT]';
ephGpsWeek = [gpsEph.GPS_Week]';
%Calculate dependent variables -------------------------------------------------
%Time since time of applicability accounting for weeks and therefore rollovers
%subtract weeks first, to avoid precision errors:
tk =(gpsWeek-ephGpsWeek)*GpsConstants.WEEKSEC + (ttxSec-Toe);
I = find(abs(tk)>fitIntervalSeconds);
if ~isempty(I)
numTimes = length(I);
fprintf(sprintf('WARNING in GpsEph2Xyz.m, %d times outside fit interval.',...
numTimes));
end
A = Asqrt.^2; %semi-major axis of orbit
n0=sqrt(GpsConstants.mu./(A.^3)); %Computed mean motion (rad/sec)
n=n0+Delta_n; %Corrected Mean Motion
h = sqrt(A.*(1-e.^2).*GpsConstants.mu);
Mk=M0+n.*tk; %Mean Anomaly
Ek=Kepler(Mk,e); %Solve Kepler's equation for eccentric anomaly
%Calculate satellite clock bias (See ICD-GPS-200 20.3.3.3.3.1)
%subtract weeks first, to avoid precision errors:
dt =(gpsWeek-ephGpsWeek)*GpsConstants.WEEKSEC + (ttxSec-Toc);
%Calculate satellite clock bias
sin_Ek=sin(Ek);
cos_Ek=cos(Ek);
dtsvS = af0 + af1.*dt + af2.*(dt.^2) + ...
GpsConstants.FREL.*e.*Asqrt.*sin_Ek -TGD;
%true anomaly:
vk=atan2(sqrt(1-e.^2).*sin_Ek./(1-e.*cos_Ek),(cos_Ek-e)./(1-e.*cos_Ek));
Phik=vk + omega; %Argument of latitude
sin_2Phik=sin(2*Phik);
cos_2Phik=cos(2*Phik);
% The next three terms are the second harmonic perturbations
duk = Cus.*sin_2Phik + Cuc.*cos_2Phik; %Argument of latitude correction
drk = Crc.*cos_2Phik + Crs.*sin_2Phik; %Radius Correction
dik = Cic.*cos_2Phik + Cis.*sin_2Phik; %Correction to Inclination
uk = Phik + duk; %Corrected argument of latitude
rk = A.*((1-e.^2)./(1+e.*cos(vk))) + drk; %Corrected radius
ik = i0 + IDOT.*tk + dik; %Corrected inclination
sin_uk=sin(uk);
cos_uk=cos(uk);
xkp = rk.*cos_uk; % Position in orbital plane
ykp = rk.*sin_uk; % Position in orbital plane
% Wk = corrected longitude of ascending node,
Wk = OMEGA + (OMEGA_DOT - GpsConstants.WE).*tk - GpsConstants.WE*[gpsEph.Toe]';
%for dtflight, see FlightTimeCorrection.m
sin_Wk = sin(Wk);
cos_Wk = cos(Wk);
xyzM=zeros(p,3);
% The matrix xyzM contains the ECEF coordinates of position
sin_ik=sin(ik);
cos_ik=cos(ik);
xyzM(:,1)=xkp.*cos_Wk-ykp.*cos_ik.*sin_Wk;
xyzM(:,2)=xkp.*sin_Wk+ykp.*cos_ik.*cos_Wk;
xyzM(:,3)=ykp.*sin_ik;
end %end of function GpsEph2Xyz
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright 2016 Google Inc.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%     http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.