satelliteposition.m

关于对卫星导航电文进行位置计算

%This routine provide in output the single satellite position using the
%parameters contained extrapolated by the subframe 2 and 3.
%Satallites ID
%Pseudorange
%Ttr 
%Tau
%Toe
%rootA
%M0
%dn
%ec
%W0
%i0
%w
%Wdot
%Idot
%Cuc
%Cus
%Crc
%Crs
%Cic
%Cis

function [DeltaTr,SatPos]=SatellitePosition(Satallites ID,Pseudorange,Ttr,Tau,Toe,rootA,M0,dn,ec,W0,i0,w,Wdot,Idot,Cuc,Cus,Crc,Crs,Cic,Cis)

%Constant definition
F       = -4.442807633e-10;      %Relativistic correction term constant. 
WE      = 7.292115e-5;          %WGS84 Earth rotation rate.                                  
WEDOT	= 7.2921151467e-5;      %WGS84 Value of earth's rotation rate.
MU      = 3.986004418e+14;      %WGS84 Value of earth's universal gravitation parametr
%Ttr = Trc - Tau;
T = Ttr - Toe;
if(T>302400)
    T = T-604800;
end

if(T<-302400)
    T = T+604800;
end

if(rootA==0) 
    return;
end

A = rootA^2; 

n = sqrt(MU/(A^3)) + dn; 
M = M0+(n*T);
E = M;

Eold = E;
E = M + (ec * sin(E));
while(abs(E-Eold)>1.0e-9);
    Eold = E;
	E = M + (ec * sin(E));
end

ec2 = ec^2;
snu = (sqrt(1-ec2)*sin(E))/(1 - (ec*cos(E)));
cnu = (cos(E)-ec)/(1 - (ec*cos(E)));

if(cnu==0)
    return;
end

nu  = atan2(snu,cnu);

phi = nu + w;

du  = Cuc*cos(2*phi) + Cus*sin(2*phi);   
dr  = Crc*cos(2*phi) + Crs*sin(2*phi);
di  = Cic*cos(2*phi) + Cis*sin(2*phi);

u   = phi + du;
r   = (A * (1 - (ec * cos(E)))) + dr;
i   = i0 + (Idot*T) + di;

Xdash = r*cos(u);
Ydash = r*sin(u);

Wc  = W0 + ((Wdot - WEDOT)*T) - (WEDOT*Toe);

SvPos(1) = (Xdash*cos(Wc)) - (Ydash*cos(i)*sin(Wc));
SvPos(2) = (Xdash*sin(Wc)) + (Ydash*cos(i)*cos(Wc));
SvPos(3) = Ydash*sin(i);

Alpha=Tau*WE;
SatPos(1)=SvPos(1)*cos(Alpha) + SvPos(2)*sin(Alpha);
SatPos(2)=SvPos(2)*cos(Alpha) - SvPos(1)*sin(Alpha);
SatPos(3)=SvPos(3);

DeltaTr = F*ec*rootA*sin(E);