nav.c

GPS导航定位程序

* Function: int ObservedMinusPredicted(int SV, unsigned long obsTIC,
*                                      navstatestruc NavState,
*                                      pseudostruc *pseudo,
*                                      double *OMPrange, double *OMPrrate,
*                                      double *OMPicp, double lmnr[4])
*
* Calculates the following quantities for the navigation solution: observed
* minus predicted values for range, range rate and the integrated carrier
* phase as a range, plus the direction cosines to the satellites and the
* differerential corrections to the range and range rate (if available).
*
* Input:  SV - The satellite being processed.
*         obsTIC - Time of the observation (TIC).
*         NavState - current observer position.
*
* Output: pseudo - pointer to the struct of pseudo-range data and corrections.
*         OMPrange - pointer to the Observed minus predicted range.
*         OMPrrate - pointer to the Observed minus predicted range rate.
*         OMPicp - pointer to the Observed minus predicted ICP as range.
*         lmnr - Observer-satellite direction cosines.
*
* Return Value: TRUE - All data was good.
*               FALSE - The differential correction data was bad.
****************************************************************************/
int ObservedMinusPredicted(int SV, unsigned long obsTIC,
			   navstatestruc NavState, pseudostruc *pseudo,
			   double *OMPrange, double *OMPrrate,
			   double *OMPicp, double lmnr[4])
{
    int observation_week;                      /* GPS week of observation. */
    int iod;                         /* IOD of the current ephemeris data. */

    double sx,sy,sz;                     /* Satellite ECEF psoiton vector. */
    double svx,svy,svz;                 /* Satellite ECEF velocity vector. */
    double dx,dy,dz;                      /* Observer to satellite vector. */
    double range;                                      /* Satellite range. */
    double relativistic;                   /* SV Relavtivistic correction. */
    double clock_correction;                       /* SV clock correction. */
    double relative_velocity;            /* SV-Observer relative velocity. */
    double propagation_delay;           /* Total signal propagation delay. */

    double observation_second;                     /* Time of observation. */
    double transmission_second;                   /* Time of transmission. */
    double delta_time;          /* Time between observation and ephemeris. */
    double SV_delta_frequency;               /* SV clock frequency offset. */
    double SV_delta_time;                        /* SV clock time offsets. */

    ephstruc E;                      /* Satellite ephemeris data structure */
	
    /* Make a local copy of the ephemeris data for the SV in question. */

    PROTECT++;      
    E = ephs[SV-1];
    PROTECT--;

    /* GPS time of the observation. */

    TICToGpsTime(obsTIC,&observation_week,&observation_second);

    /* SV position at time of the pseudorange observation. */

    EphemerisPrediction(SV,observation_second,
			&relativistic,&sx,&sy,&sz,&svx,&svy,&svz);
 
    /* Observer to satellite vector and the satellite range. */

    dx = sx - NavState.x;
    dy = sy - NavState.y;
    dz = sz - NavState.z;
    range = sqrt(dx*dx + dy*dy + dz*dz);

    /* SV position at time signal was transmitted. */

    transmission_second = observation_second - range/SPEED_OF_LIGHT;
    if(transmission_second < 0.0)
	transmission_second += SECONDS_IN_WEEK;

    EphemerisPrediction(SV,transmission_second,
			&relativistic,&sx,&sy,&sz,&svx,&svy,&svz);

    /* New observer to satellite vector and satellite range. */

    dx = sx - NavState.x;
    dy = sy - NavState.y;
    dz = sz - NavState.z;
    range = sqrt(dx*dx + dy*dy + dz*dz);

    lmnr[0] = dx/range;                              /* Direction cosines. */
    lmnr[1] = dy/range;
    lmnr[2] = dz/range;
    lmnr[3] = range;

    iod = E.iode;                             /* Issue of data being used. */

    /* Time between transmission and ephemeris validty. */

    delta_time = transmission_second - E.toc;

    if(delta_time > SECONDS_IN_WEEK/2.0)
	delta_time -= SECONDS_IN_WEEK;
    else if (delta_time < -SECONDS_IN_WEEK/2.0)
	delta_time += SECONDS_IN_WEEK;

    /* Satellite clock frequency and time correction at transmission. */

    SV_delta_frequency = E.af1 + 2.0*delta_time*E.af2;  
    SV_delta_time = E.af0 + delta_time*(E.af1 + delta_time*E.af2)
		    + relativistic;  

    /* Total clock correction term (m). */

    clock_correction = SPEED_OF_LIGHT*(SV_delta_time - E.tgd);

    /* Total propagation delay. */

    propagation_delay = range + sverc[SV-1] - clock_correction;

    /* Observed minus predicted range. */

    *OMPrange = pseudo->range - propagation_delay; 

    /* Compute the predicted relative velocity of SV and receiver */

    relative_velocity = (svx-NavState.velx)*lmnr[0]
	     + (svy-NavState.vely)*lmnr[1]
	     + (svz-NavState.velz)*lmnr[2];

    /* Observed minus predicted range rate. */

    *OMPrrate = pseudo->range_rate - relative_velocity
	      + SV_delta_frequency*SPEED_OF_LIGHT + NavState.clockdrift;

    /* Observed minus predicted integrated carrier phase range. */

    *OMPicp = pseudo->icp_range - propagation_delay;

    /* See if there are valid differential corrections available for this
       measurement block. */

    {
	double dt, dt2;   /* Delta time & double delta time btwn obs & MZ. */

	beacstruc B;

	/* Make a local copy of the DGPS beacon data structure. */

	PROTECT++;      
	B = BEAC;
	PROTECT--;

	pseudo->differential_range_correction = NO_DGPS;
	pseudo->differential_range_rate_correction = NO_DGPS;
    
	if(B.sig && B.data && B.hlth!=7 && B.DC[SV-1].vflg==TRUE)
	{
	    if(B.DC[SV-1].dontuse==FALSE)         /* Check don't use flag. */
	    {
		dt = MZDifference(observation_second,B.DC[SV-1].mzcount);

		/* Don't use if DGPS data older than 60 seconds. */

		if(fabs(dt)<=60.0)
		{
		    if(B.DC[SV-1].iod==iod)
		    {
			pseudo->differential_range_rate_correction = 
							      B.DC[SV-1].rrc;
			pseudo->differential_range_correction = 
			       B.DC[SV-1].prc + 
			       dt*pseudo->differential_range_rate_correction;
		    }
		    else if(B.DDC[SV-1].iod==iod && B.DDC[SV-1].vflg==TRUE)
		    {
			dt2 = MZDifference(observation_second,
							B.DDC[SV-1].mzcount);

			if(fabs(dt)<=120.0)
			{
			    pseudo->differential_range_rate_correction = 
					   B.DC[SV-1].rrc + B.DDC[SV-1].drrc;
			    pseudo->differential_range_correction = 
				      B.DC[SV-1].prc + B.DDC[SV-1].dprc +
				      dt*B.DC[SV-1].rrc + dt2*B.DDC[SV-1].drrc;
			}
		    }                                   /* IODs don't match. */
		}                              /* dt larger than 60 seconds. */
	    }                                             /* Don't use data. */
	}                                              /* No data available. */
  
	/* Don't use corrections greater in magnitude than 1000 meters
	   (range) or 100 meters/sec (range rate). */

	if(fabs(pseudo->differential_range_correction)>1000.0)
	    pseudo->differential_range_correction = NO_DGPS;
	if(fabs(pseudo->differential_range_rate_correction)>100.0)
	    pseudo->differential_range_rate_correction = NO_DGPS;

	/* If we are receiving RTCM-104 differential corrections inputs,
	   omit any satellites which do not have a correction. */

	if(B.sig && B.data && (B.hlth<7))
	{
	    if(pseudo->differential_range_correction>1000.0)
	    {
		WarningMessage("Differential Correction invalid in "
			       "ObservedMinusPredicted");
		return(FALSE);
	    }
	}
    }
    return(TRUE);
}

/****************************************************************************    
*
* Function: double MZDifference(double gsec, int mzcount)
*
* Gets the difference in seconds between an RTCM-104 modified Z count and a
* specified GPS time. The assumption is that the Z count (which is modulo
* 3600 seconds) is within +/- 1/2 hour of the specified GPS time.                                                             
*
* Input:  gps_second - GPS time of week, seconds
*         mzcount - modified Z count from RTCM-104 message
*
* Output: None.
*
* Return Value: Difference between GPS time and mzcount.
****************************************************************************/
double MZDifference(double gps_second, int mzcount)
{
    double time_difference;

    time_difference = fmod(gps_second,SECONDS_IN_HOUR) 
		      - mzcount*SECONDS_PER_MZCOUNT;

    if(time_difference>SECONDS_IN_HALF_HOUR)
	time_difference-= SECONDS_IN_HOUR;
    else if(time_difference<-SECONDS_IN_HALF_HOUR)
	time_difference += SECONDS_IN_HOUR;

    return(time_difference);
}

/****************************************************************************    
*
* Function: int BuildNavMat(double solution_matrix[MAXCHANNELS][4],
*                           navstatestruc *NewNavState, ompstruc *omp)
*                       
* Gets the navigation matrix - the product of the covarince matrix and the
* solution matrix.
*
* Input: solution_matrix - the direction cosines to the satellites.
*        NewNavState - pointer to the navigation state structure.
*        omp - pointer to the observed minus predicted range/range-rate.
*
* Output: NewNavState - update for the navigation matrix.
*
* Return Value: FALSE - GDOP big or inverse covariance matrix singular.
*               TRUE - GDOP and inverse covariance matrix good.
****************************************************************************/
int BuildNavMat(double solution_matrix[MAXCHANNELS][4],
		navstatestruc *NewNavState, ompstruc *omp)
{
    double inverse_covariance_matrix[4][4];
    
    /* If there are only 3 satellites in the solution then use altitude
       aiding for 2D fix. */

    if(NumSatsInSoln==3 && AltitudeAided)
	AltitudeAiding(NewNavState,omp,solution_matrix);

    /* Get the inverse of the covariance matrix and check if it's singular.
       If the singularity can't be corrected by applying altitude aiding then
       return because a navigation solution can't be calculated */

    if(GetInvCovMat(NewNavState,omp,solution_matrix,
		    inverse_covariance_matrix)==SINGULAR)
	return(FALSE);

    /* Build navigation matrix from the inverted covariance matrix and the
       solution matrix. Multiply this matrix by the observed-predicted
       pseudoranges vector to get the XYZT navigation update.  The same
       navigation matrix also works to give velocity updates from the
       observed-predicted pseudorange rate errors. */

    GetNavMat(NewNavState,solution_matrix,inverse_covariance_matrix);

    CalcDOPS(NewNavState);

    /* Check for acceptable GDOP. If GDOP is too big, try augmenting the
       nav matrix before giving up. */

    if(NewNavState->gdop>GdopMask && NewNavState->altitude_aiding==NO &&
       AltitudeAided)
    {
	AltitudeAiding(NewNavState,omp,solution_matrix);
	if(GetInvCovMat(NewNavState,omp,solution_matrix,
			inverse_covariance_matrix)==SINGULAR)
	    return(FALSE);

	GetNavMat(NewNavState,solution_matrix,inverse_covariance_matrix);
	CalcDOPS(NewNavState);
    }
    return(TRUE);
}                              

/****************************************************************************    
*
* Function: void AltitudeAiding(navstatestruc *NewNavState,ompstruc *omp,
*                               double solution_matrix[MAXCHANNELS][4])
*
* Adds a fictitious pseudo-range to the solution matrix to enable altitude
* aiding.
*
* Input: *NewNavState - Current observer position.
*        *omp - Observed minus predicted range/range-rate.
*
* Output: solution_matrix - the direction cosines to the satellites.
*
* Return Value: None.
****************************************************************************/
void AltitudeAiding(navstatestruc *NewNavState,ompstruc *omp,
		      double solution_matrix[MAXCHANNELS][4])
{
    double dx,dy,dz;                      /* Observer to satellite vector. */
    double range;                                      /* Satellite range. */

    NewNavState->altitude_aiding = YES;

    /* Do altitude aiding assuming a given WGS-84 ellipsoid height by
       creating a fictitious satellite at the center of the earth and
       determining the equivalent pseudorange. */

    dx = -NewNavState->x;
    dy = -NewNavState->y;
    dz = -NewNavState->z;
    range = sqrt(dx*dx + dy*dy + dz*dz);

    omp->range[NumSatsInSoln] = 0.0;
    omp->range_rate[NumSatsInSoln] = 0.0;
 
    /* Direction cosines of fictitious satellite. */

    solution_matrix[NumSatsInSoln][0] = dx/range;