getsatposecef.m

利用gps多天线载波相位技术

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Calculate the satelltie position from the broadcast ephemerides
%% Input:
%%      satID -> Satellite PRN
%%      eph_epoch -> Pointer to the proper ephemerides paragraph 
%%      receipt_time -> Time when the signal reached the receiver
%%      transit_time ->  Signal transit time from a specific satellite to
%%                                the receiver.
%%      mEphTime (global) -> Time tag of each ephemerides paragraph
%%      mEphemerides (global) -> Matrix having overall ephemerides parameters
%% Output:
%%      vSatPos -> Satellite coordinate in ECEF before applying the earth
%%                          rotation    
%%      sat_clock_cor_new -> Satellite clock correction
%%      group_delay -> Group delay
%%  References:
%%      G. Strang, K.Borre. Linear Algebra, Geodesy, and GPS. 1997,
%%      Wellesley-cambridge press. pp.482-486

function [vSatPos, sat_clock_cor, group_delay]= GetSatPosECEF(satID,eph_epoch,receipt_time,transit_time)

global mEphTime mEphemerides
speed_light=299792458;

%% Signal transit time
transmit_time=receipt_time-transit_time;
 
%% totalrec reflects how many ephemerides are recorded
%% Ephemeris are normally written with a 2-hour interval
%% For global NAV file from IGS, it is 13.
%% For NAV file from receiver, it depends on the observation session
totalrec=size(mEphemerides,2);

%% Earth gravitational parameter  in m^3/s^2
gra = 3.986008e14;    
%% Earth rotation rate in rad/s
rot_rate = 7.2921151467e-5; 
 
%% Read the parameters needed for the calculation of Sat position 
M0=mEphemerides(satID,eph_epoch,9);
root_a=mEphemerides(satID,eph_epoch,13);
Delta_n=mEphemerides(satID,eph_epoch,8);
e=mEphemerides(satID,eph_epoch,11);
omega=mEphemerides(satID,eph_epoch,20);
Cuc=mEphemerides(satID,eph_epoch,10);
Cus=mEphemerides(satID,eph_epoch,12);
Crc=mEphemerides(satID,eph_epoch,19);
Crs=mEphemerides(satID,eph_epoch,7);
i0=mEphemerides(satID,eph_epoch,18);
IDOT=mEphemerides(satID,eph_epoch,22);
Cic=mEphemerides(satID,eph_epoch,15);
Cis=mEphemerides(satID,eph_epoch,17);
Omega0=mEphemerides(satID,eph_epoch,16);
OMEGA_DOT=mEphemerides(satID,eph_epoch,21);
TOE=mEphemerides(satID,eph_epoch,14);
TOC=mEphemerides(satID,eph_epoch,2);
clockbias=mEphemerides(satID,eph_epoch,3);
clockdrift=mEphemerides(satID,eph_epoch,4);
clockdriftrate=mEphemerides(satID,eph_epoch,5);
TGD=mEphemerides(satID,eph_epoch,28);

%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%%           Satellite coordinate calculation
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
a = root_a*root_a;
%% Time elapsed since toe
tj=transmit_time-TOE;
%% Format the time
 if tj >  302400, 
     tj= tj-604800; 
 elseif tj < -302400,
     tj = tj+604800; 
 end 
%% Mean anomaly time tj
muj= M0+(sqrt(gra/a^3)+Delta_n)*tj; 
%% Iterative solution for Ej
Ej = muj;
iter=0;
while(1)
   iter=iter+1;
   Ej_backup = Ej;
   Ej = muj+e*sin(Ej);
   dEj = Ej-Ej_backup;
   if abs(dEj) < 1e-13,
      break;
   end
   if iter>10,
       error('The calculation of satellite position can not converge!!!');
   end
end
%% True anomaly
fj = atan2(sqrt(1-e^2)*sin(Ej), cos(Ej)-e);
%% omega+fj
o_fj = fj+omega;
%% Argument of perigee
wj=o_fj+ Cuc*cos(2*o_fj)+Cus*sin(2*o_fj);
%% Radial distance
rj=a*(1-e*cos(Ej))+Crc*cos(2*o_fj)+Crs*sin(2*o_fj);
%% Inclination
ij=i0+IDOT*tj+Cic*cos(2*o_fj)+Cis*sin(2*o_fj);
%% Longitude for ascending node
Omega = Omega0+(OMEGA_DOT-rot_rate)*tj-rot_rate*TOE;
%% Rotation matrix
mRotation=[cos(Omega)    -cos(ij)*sin(Omega); ...
                    sin(Omega)      cos(ij)*cos(Omega);...
                    0                       sin(ij)  ];
%% Satellite position in ECEF in meters , before earth rotation 
vSatPos=mRotation*[cos(wj)*rj; sin(wj)*rj];

%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%%               Satellite clock correction
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% dt
dt=  transmit_time- TOC; 
%% Format the time
if dt >  302400,
    dt= dt-604800;
elseif dt < -302400,
    dt = dt+604800;
end
%% Relativistic correction  
rel= ( -4.442807633e-10)*e*root_a*sin(Ej); 
%% Satellite clock correction
sat_clock_cor = clockbias + clockdrift*dt + clockdriftrate*dt*dt+rel ;                                
%% Group delay                               
group_delay=TGD;