Lagrange for SP3 Satellite Orbit and Clock Correction Interpolation

function [xsp3,ysp3,zsp3,clksat,P1,P2,sp3Vx,sp3Vy,sp3Vz ...
,RV1,RV2,RVIO,saas,Elev,AZIMUTH,PH1RV,PH2RV,PHRVIO] ...
= LG_SP3(DOY,rT,PH1,PH2,PR1,PR2,SP3time,SP3X,SP3Y,SP3Z,SATclk,x,SunX,SunY,SunZ,PTRH)

% Lagrange:satellite orbit and satellite clock interpolation
% called by EKF. Computes PHI (dynamic model) for receiver clock
%
% %%%%%%%%%% HELP %%%%%%%%%%
%
% [xsp3,ysp3,zsp3,clksat,P1,P2,sp3Vx,sp3Vy,sp3Vz ...
% ,RV1,RV2,RVIO,saas,Elev,AZIMUTH,PH1RV,PH2RV,PHRVIO] ...
% = LG_SP3(DOY,rT,PH1,PH2,PR1,PR2,SP3time,SP3X,SP3Y,SP3Z,SATclk,x,SunX,SunY,SunZ,PTRH)
%
% Input data
% DOY = day of year
% rT = nominal (RINEX) time for epoch ep [GPSweek GPSsec], [numep x 2]
% PH1 = Carrier phase L1,cycles [numep+1 x numsat]
% PH2 = Carrier phase L2,cycles [numep+1 x numsat]
% PR1 = Pseudorange C/A code, metres [numep+1 x numsat]
% PR2 = Pseudorange P code , metres [numep+1 x numsat]
% SP3time = GPS time of SP3 file [GPSweek GPSsec], [numep x 2]
% SP3X, SP3Y, SP3Z = Satellite coordinate (WGS84), metres [numep+1 x numsat]
% x = Aproximate station coordinates, metres [3 x 1]
% SATclk = Precise clock correction, microseconds [numep+1 x numsat]
% SunX, SunY, SunZ = Sun coordinate (WGS84), metres [numep+1 x numsat]
% PTRH = [P TC RH]
%
% OUTPUT
% xsp3
% ysp3
% zsp3
% clksat
% P1
% P2
% sp3Vx
% sp3Vy
% sp3Vz
% RV1
% RV2
% RVIO
% saas
% Elev
% AZIMUTH
% PH1RV
% PH2RV
% PHRVIO
%
% Modified by Phakphong Homniam
% October 13, 2002
% Original Mathcad source code by Boonsap Witchayangkoon, 2000

%%%%%%%%%% CONSTANT DECLARATION %%%%%%%%%%

% Global constants of the relevant parameters in constant_global.m file
constant_global

global maskangle antenna_offset IIR

% Satellite mask angle
maskangle = 15*deg;

% Satellite Antenna Offset
antenna_offset = [0.279 0 1.023]';

% GPS Block IIR satellite
IIR = 13;
% For GPS Block IIR satellite, IGS has agreed upon to
% the satellite phase center offset [0 0 0]

% Langrange InterPolation Constants
% Number of points used in LG orbit interpolation
nXYZ = 9;
% Number of points used in LG clock interpolation
nCLK = 5;

% Environmental data for this station
P = PTRH(1); % Pressure P in milibar (mb)
TC = PTRH(2); % Temperature TC in degree CELCIUS
RH = PTRH(3); % Humedity H(%)
T = TC+273.2; % Temperature T in Kelvin

%%%%%%%%%% BEGIN %%%%%%%%%%

PRN = PH1(1,:);
PH1(1,:) = []; SP3X(1,:) = [];
PH2(1,:) = []; SP3Y(1,:) = [];
PR1(1,:) = []; SP3Z(1,:) = [];
PR2(1,:) = []; SATclk(1,:) = [];
rT = rT(:,2); SP3time = SP3time(:,2);

x = x(:);
llh = ecef2lla(x',1);
wetzenith = SWZD(P,T,RH);
dryzenith = SHZD(P,T,RH);
[tep,NumSat] = size(PR1);
for j = 1:NumSat
fprintf('\nProcessing satellite number %s\n',num2str(PRN(j)));
CLKsat(1:tep,1) = NAJ;
sp3x(1:tep,1) = NAJ;
sp3y(1:tep,1) = NAJ;
sp3z(1:tep,1) = NAJ;
Prange1(1:tep,1) = NAJ;
Prange2(1:tep,1) = NAJ;
Vx(1:tep,1) = NAJ;
Vy(1:tep,1) = NAJ;
Vz(1:tep,1) = NAJ;
R1(1:tep,1) = NA;
R2(1:tep,1) = NA;
RIO(1:tep,1) = NA;
Tropo(1:tep,1) = NA;
ELEV(1:tep,1) = NA;
azimuth(1:tep,1) = NA;
PH1R(1:tep,1) = NA;
PH2R(1:tep,1) = NA;
PHRIO(1:tep,1) = NA;

r = size(IIR,1);
% Assume no satellite offset
NoOffset = 1;
% NoOffset = 0;
% for h = 1:r
% if PRN(j) == IIR(h)
% NoOffset = 1;
% end
% end
for i = 1:tep
% i
rT(i);
if (PR1(i,j) ~= NA) & (PR2(i,j) ~= NA) & (PH1(i,j) ~= NA) & (PH2(i,j) ~= NA)
CLKsat(i) = SVCLK(rT(i),SP3time,SATclk(:,j) ...
,PR1(i,j),nCLK,NAJ);
if CLKsat(i) ~= NAJ
[X,V] = SatPo(rT(i),PR1(i,j),CLKsat(i) ...
,SP3time,SP3X(:,j),SP3Y(:,j),SP3Z(:,j) ...
,x,nXYZ,[SunX(i),SunY(i),SunZ(i)]',NoOffset,NAJ);
rho = sqrt((X(1)-x(1))^2+(X(2)-x(2))^2+(X(3)-x(3))^2);
elev = elevation(X,x);
if elev < maskangle
continue
end
azimuth(i) = Az(llh(1),llh(2),X-x);
ELEV(i) = elev;
sp3x(i) = X(1);
sp3y(i) = X(2);
sp3z(i) = X(3);
Vx(i) = V(1);
Vy(i) = V(2);
Vz(i) = V(3);
nmf = NeillMF(DOY,llh(1)/deg,llh(3),elev/deg);
Tropo(i) = dryzenith*nmf(1)+wetzenith*nmf(2);
Relativity = -2*dot(X(:),V(:))/c;
h = rho-c*CLKsat(i)*10^-6+Tropo(i)-Relativity;
R1(i) = PR1(i,j)-h;
R2(i) = PR2(i,j)-h;
% See eq.9.46
RIO(i) = cf1*PR1(i,j)-cf2*PR2(i,j)-h;
PH1R(i) = lamda1*PH1(i,j)-h;
PH2R(i) = lamda2*PH2(i,j)-h;
% See eq.9.51
PHRIO(i) = lamda1*(cf1*PH1(i,j)-cf1f2*PH2(i,j))-h;
% if (abs(R1(i)) < rclock) & (abs(R2(i)) < rclock)
Prange1(i) = PR1(i,j);
Prange2(i) = PR2(i,j);
% end
end
end
end
xsp3(1:tep,j) = sp3x(:);
ysp3(1:tep,j) = sp3y(:);
zsp3(1:tep,j) = sp3z(:);
clksat(1:tep,j) = CLKsat(:);
P1(1:tep,j) = Prange1(:);
P2(1:tep,j) = Prange2(:);
sp3Vx(1:tep,j) = Vx(:);
sp3Vy(1:tep,j) = Vy(:);
sp3Vz(1:tep,j) = Vz(:);
RV1(1:tep,j) = R1(:);
RV2(1:tep,j) = R2(:);
RVIO(1:tep,j) = RIO(:);
saas(1:tep,j) = Tropo(:);
Elev(1:tep,j) = ELEV(:);
AZIMUTH(1:tep,j) = azimuth(:);
PH1RV(1:tep,j) = PH1R(:);
PH2RV(1:tep,j) = PH2R(:);
PHRVIO(1:tep,j) = PHRIO(:);
end

%%%%%%%% RESULT %%%%%%%%

xsp3 = [PRN;xsp3];
ysp3 = [PRN;ysp3];
zsp3 = [PRN;zsp3];
clksat = [PRN;clksat];
P1 = [PRN;P1];
P2 = [PRN;P2];
sp3Vx = [PRN;sp3Vx];
sp3Vy = [PRN;sp3Vy];
sp3Vz = [PRN;sp3Vz];
RV1 = [PRN;RV1];
RV2 = [PRN;RV2];
RVIO = [PRN;RVIO];
saas = [PRN;saas];
Elev = [PRN;Elev];
AZIMUTH = [PRN;AZIMUTH];
PH1RV = [PRN;PH1RV];
PH2RV = [PRN;PH2RV];
PHRVIO = [PRN;PHRVIO];

%%%%%%%%%%END%%%%%%%%%%