svcalc.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void SVPredicts(int sv, int wk, double sec, double t[3][3],
*                           double obsx,double obsy, double obsz,
*                           float *elv, float *azi, float *dopp,
*                           float *erc, float *iono, float *tropo)
*
* Predicts a satellite's elevation, azimuth, Doppler and earth rotation,
* ionospheric and tropospheric correction to the pseudo-range.
*
* Input: sv - the satellite PRN number.
*        wk - the GPS week number.
*        sec - the GPS second in the week.
*        obsx - the observer x co-ordinate.
*        obsy - the observer y co-ordinate.
*        obsz - the observer z co-ordinate.
*
* Output: elv - the satellite elevation, radians.
*         azi - the satellite azimuth, radians.
*         dopp - the Doppler frequency, Hz.
*         erc - earth rotation pseudo-range correction, m.
*         iono - ionospheric pseudo-range correction, m.
*         tropo - tropospheric pseudo-range correction, m.
*
* Return Value: None.
****************************************************************************/
void SVPredicts(int sv, int wk, double sec, double t[3][3],
		double obsx,double obsy, double obsz,
		float *elv, float *azi, float *dopp,
		float *erc, float *iono, float *tropo)
{
    double n,e,u,rho,dx,dy,dz,sx,sy,sz,svx,svy,svz,svirel,rhox,rhoy,
	   es,a,phu,lmu,temp,f,psi,phi,lmi,phm,phm2,phm3,suma,sumb,xtemp,
	   x2,x4,tlocal,elvrad,azirad,dp,EarthRotCorr,ic,tr;

    /* Use either the ephemeris or almanac for the prediction. */

    if(ephs[sv-1].vflg==1)
	EphemerisPrediction(sv,sec,&svirel,&sx,&sy,&sz,&svx,&svy,&svz);
    else
	AlmanacPrediction(sv,wk,sec,&sx,&sy,&sz,&svx,&svy,&svz);

    /* Convert SV XYZ to NEU in observer's local astronomic system. */

    dx = sx - obsx;
    dy = sy - obsy;
    dz = sz - obsz;

    n = t[0][0]*dx + t[0][1]*dy + t[0][2]*dz;
    e = t[1][0]*dx + t[1][1]*dy + t[1][2]*dz;
    u = t[2][0]*dx + t[2][1]*dy + t[2][2]*dz;

    /* Compute SV elevation and azimuth. */

    rho = sqrt(n*n+e*e);
    elvrad = atan2(u,rho);
    azirad = atan2(e,n);

    /* Compute SV Doppler. */

    dp = - (dx*svx+dy*svy+dz*svz)/sqrt(dx*dx+dy*dy+dz*dz)*L1/SPEED_OF_LIGHT;

    /* Compute the SV's earth rotation correction. */

    rhox = sx - obsx;
    rhoy = sy - obsy;
    EarthRotCorr = WGS84oe / SPEED_OF_LIGHT * (sy*rhox-sx*rhoy);

    /* Compute the SV's ionospheric correction. */

    es = elvrad / PI;                      /* SV elevation in semicircles. */
    a = azirad;                                  /* SV azimuth in radians. */
    PROTECT++;
    phu = CurNavState.lat / PI;       /* Receiver latitude in semicircles. */
    lmu = CurNavState.lon / PI;      /* Receiver longitude in semicircles. */
    PROTECT--;

    /* Don't compute ionospheric correction for SV's below the horizon. */

    if(es < 0.0)
    {
	ic=0;
	goto TROPCALC;
    }

    /* Use nocturnal value when ionospheric correction is unavailablEphem. */

    if(ionoutc.vflg==0)
    {
	temp = 0.53 - es;
	f = 1.0 + 16.0*temp*temp*temp;
	ic = f * 5.0e-9 * SPEED_OF_LIGHT + 0.5;
	goto TROPCALC;
    }

    /* Compute receiver's local time of day in seconds
       = GMT + 43200 seconds per semicircle of longitude. */

    tlocal = sec + SECONDS_IN_DAY/2.0*lmu + SECONDS_IN_DAY;
    while(tlocal > SECONDS_IN_DAY)
	tlocal -= SECONDS_IN_DAY;

    /* Compute slant factor f. */

    temp = 0.53 - es;
    f = 1.0 + 16.0*temp*temp*temp;

    /* Compute Earth angle psi (semicircles). */

    psi = 0.0137/(es+0.11) - 0.022;

    /* Compute subionospheric latitude phi (semicircles). */
    
    phi = phu + psi*cos(a);

    /* Limit phi to between +75 degrees and - 75 degrees. */

    if(phi>0.416)
	phi = 0.416;
    else if(phi<(-0.416))
	phi = -0.416;

    /* Compute subionospheric longitude lmi (semicircles). */

    lmi = lmu + psi*sin(a)/cos(phi*PI);

    /* Compute local time in seconds
       = GMT + 43200 seconds per semicircle of longitude */

    tlocal = sec + SECONDS_IN_DAY/2.0*lmi;
    while(tlocal >= SECONDS_IN_DAY)
	tlocal -= SECONDS_IN_DAY;
    while(tlocal < 0.0)
	tlocal += SECONDS_IN_DAY;
 
    /* Compute subionospheric geomagnetic latitude phm (semicircles). */

    phm = phi + 0.064*cos((lmi-1.617)*PI);
    phm2 = phm*phm;
    phm3 = phm2*phm;

    /* Diurnal maximum time delay: suma. Diurnal period: sumb. */

    PROTECT++;
    suma = ionoutc.a0 + ionoutc.a1*phm + ionoutc.a2*phm2 + ionoutc.a3*phm3;
    sumb = ionoutc.b0 + ionoutc.b1*phm + ionoutc.b2*phm2 + ionoutc.b3*phm3;
    PROTECT--;

    if(sumb!=0.0)
	xtemp = 2.0*PI*(tlocal-50400.0)/sumb;
    else
	xtemp = 0.0;

    if(fabs(xtemp)<1.57)
    {
	x2 = xtemp*xtemp;
	x4 = x2*x2;
	temp = 1.0 - x2/2.0 + x4/24.0;
	ic = f*(5.0e-9 + suma*temp)*SPEED_OF_LIGHT + 0.5;
    }
    else
	ic = f * 5.0e-9 * SPEED_OF_LIGHT;

TROPCALC:

    /* Calculate tropospheric correction for SV's above the horizon. */

    if(elvrad<0.0)
	tr = 0;
    else
    {
	PROTECT++;
	tr = 2.4224*exp(-0.13346e-3*CurNavState.hgt)/(0.026+sin(elvrad));
	PROTECT--;
	if(tr < 0)
	    tr = 0;
	else if (tr > 200)
	    tr = 200;
    }

    *elv = elvrad;
    *azi = azirad;
    if(*azi < 0)
	*azi += 2.0*PI;
    *dopp = dp;
    *erc = EarthRotCorr;
    *iono = ic;
    *tropo = tr;
}

/****************************************************************************    
* Function: void AlmanacPrediction(int sv, int wk, double sec,
*                                  double *x, double *y, double *z,
*                                  double *vx, double *vy, double *vz)
*
* Predictions a satellite position and velocity vector from the almanac.
*
* Input: sv - the satellite PRN number.
*        wk - the GPS week number.
*        sec - second in the GPS week.
*
* Output: x - the satellite x co-ordinate, m.
*         y - the satellite y co-ordinate, m.
*         z - the satellite z co-ordinate, m.
*         vx - the satellite x co-ordinate velocity, m/s.
*         vy- the satellite y co-ordinate velocity, m/s.
*         vz - the satellite z co-ordinate velocity, m/s.
*
* Return Value: None.
****************************************************************************/
void AlmanacPrediction(int sv, int wk, double sec,
		       double *x, double *y, double *z,
		       double *vx, double *vy, double *vz)
{
    float a,n0,tk,mk,ek,cek,sek,denom,cvk,svk,vk,pk,uk,
	  rk,ik,cuk,suk,xpk,ypk,ok,sik,cik,sok,cok,ekold;
    float  ecc,slr,tmp,xdot,ydot;

    almstruc A;

    PROTECT++;
    A = alms[sv-1];               /* Get the almanac for the SV concerned. */
    PROTECT--;

    /* Semimajor axis (m). */

    a = A.sqrta;
    a = a * a;

    /* Mean motion (rad/sec). */

    n0 = sqrt((double)(GravConstant/(a*a*a)));

    /* Compute the difference, in seconds, between the epoch and wk & sec. */

    tk = (wk-A.refweek)*SECONDS_IN_WEEK + sec - A.toa;

    /* Mean anomaly (rads). */

    mk = A.manom + n0*tk;

    /* Obtain eccentric anomaly by solving Kepler's equation. */

    ecc = A.ecc;
    ek = mk;
    ekold = ek + 1.0E0;
    while(fabs((double)(ek-ekold)) > 1.0E-13)
    {
	ekold = ek;
	ek = ekold + (mk-ekold+ecc*sin((double)ekold))
		     / (1.0E0-ecc*cos((double)ekold));
    }

    cek = cos((double)ek);
    sek = sin((double)ek);

    /* Compute the true anomaly (rads). */

    denom = 1.0E0 - ecc*cek;
    cvk = (cek-ecc)/denom;
    svk = sqrt((double)(1.0-ecc*ecc)) * sek / denom;
    vk = atan2((double)svk,(double)cvk);
    if (vk < 0.0E0)
	vk = vk + 2.0*PI;

    /* Argument of latitude (rads). */

    pk = vk + A.argpg;

    /* Corrected argument of latitude (rads). */

    uk = pk;

    /* Corrected radius (rads). */

    rk = denom * a;

    /* Corrected inclination (rads). */

    ik = A.inclin;

    cuk = cos((double)uk);
    suk = sin((double)uk);

    /* Position in the orbital plane. */

    xpk = rk*cuk;
    ypk = rk*suk;

    slr = a*(1.0 - ecc*ecc);
    tmp = sqrt((double)(GravConstant/slr));

    xdot = -tmp*suk;
    ydot = tmp*(ecc + cuk);

    /* Corrected longitude of the ascending node (rads). */

    ok = A.ratoa + tk*(A.rora-WGS84oe) - WGS84oe*A.toa;

    sik = sin((double)ik);
    cik = cos((double)ik);