nav.c

GPS导航定位程序

    solution_matrix[NumSatsInSoln][1] = dy/range;
    solution_matrix[NumSatsInSoln][2] = dz/range;
    solution_matrix[NumSatsInSoln][3] = 0.0;

    NumSatsInSoln++;
}

/****************************************************************************    
*
* Function: void GetCovMat(double solution_matrix[MAXCHANNELS][4],
*                          double covariance_matrix[4][4])
*
* Calculates the covarince matrix from the solution matrix.
*
* Input: solution_matrix - the direction cosines to the satellites.
*
* Output: covariance_matrix - the covariance matrix for the solution.
*
* Return Value: None
****************************************************************************/
void GetCovMat(double solution_matrix[MAXCHANNELS][4],
	       double covariance_matrix[4][4])
{
    int j1,j2,j3;
    double temp;

    for(j1=0;j1<4;j1++)
    {
	for(j2=0; j2<4; ++j2)
	{
	    temp = 0.0;
	    for(j3=0; j3<NumSatsInSoln; ++j3)
		temp += solution_matrix[j3][j1]*solution_matrix[j3][j2];
	    covariance_matrix[j1][j2] = temp;
	}
    }
}

/****************************************************************************    
*
* Function:  int GetInvCovMat(navstatestruc *NewNavState, ompstruc *omp,
*                             double solution_matrix[MAXCHANNELS][4],
*                             double inverse_covariance_matrix[4][4])
*
* Calculates the inverse covarince matrix from the solution matrix.
*
* Input: *NewNavState - Current observer position.
*        *omp - Observed minus predicted range/range-rate.
*        solution_matrix - the direction cosines to the satellites.
*
* Output: inverse_covariance_matrix.
*
* Return Value: FALSE - Inverse is singular.
*               TRUE - Good inverse.
****************************************************************************/
int GetInvCovMat(navstatestruc *NewNavState, ompstruc *omp,
		 double solution_matrix[MAXCHANNELS][4],
		 double inverse_covariance_matrix[4][4])
{
    int singular;
    
    /* Get the covariance matrix. */

    GetCovMat(solution_matrix,inverse_covariance_matrix);

    /* Invert it and check if it's singular. */

    if(InverseMatrix(4,&inverse_covariance_matrix[0][0])==SINGULAR)
    {
	/* If altitude aiding has not already been applied then apply it. */

	if(NewNavState->altitude_aiding==NO && AltitudeAided==TRUE) 
	{
	    AltitudeAiding(NewNavState,omp,solution_matrix);
	    GetCovMat(solution_matrix,inverse_covariance_matrix);
	    singular = InverseMatrix(4,&inverse_covariance_matrix[0][0]);
	}

	/* If the inverse of the covariance matrix is still singular then a
	   navigation solution cannot be calculated so return. */

	if(singular)
	{
	    OutputString(2,23,
	       "NO NAV: Singular Covariance Matrix                       ");
	    NewNavState->gdop = NewNavState->pdop = NewNavState->tdop
	      = NewNavState->vdop = NewNavState->hdop = BIG_DOP; 
	    NewNavState->tofnavmat = 0;          /* Invalidate nav matrix. */
	    NewNavState->navmode = FIX_NO;
	    PROTECT++;
	    CurNavState = *NewNavState;
	    PROTECT--;
	    return(FALSE);
	}
    }
    return(TRUE);
}

/****************************************************************************    
*
* Function: void GetNavMat(navstatestruc *NewNavState,
*                          double solution_matrix[MAXCHANNELS][4],
*                          double inverse_covariance_matrix[4][4])
*
* Calculates the navigation matrix - the product of the inverse covarince
* matrix with the transpose of the solution matrix.
*
* Input: solution_matrix - the direction cosines to the satellites.
*        inverse_covariance_matrix   
*
* Output: *NewNavState - the navigation matrix.
*
* Return Value: None.
****************************************************************************/
void GetNavMat(navstatestruc *NewNavState,
	       double solution_matrix[MAXCHANNELS][4],
	       double inverse_covariance_matrix[4][4])
{
    int j1,j2,j3;
    double temp;

    for(j1=0; j1<4; ++j1)
    {
	for(j2=0;j2<NumSatsInSoln;j2++)
	{
	    temp = 0.0;
	    for(j3=0; j3<4; ++j3)
		temp += inverse_covariance_matrix[j1][j3]
			*solution_matrix[j2][j3];
	    NewNavState->navmat[j1][j2] = temp;
	}
    }
}

/****************************************************************************    
*
* Function: void CalcDOPS(navstatestruc *NewNavState)
*
* Calculate the dilution of precision for the satellites tracked.
*
* Input: *NewNavState - the navigation matrix.
*
* Output: *NewNavState - the DOPs.
*
* Return Value: None.
****************************************************************************/
void CalcDOPS(navstatestruc *NewNavState)
{
    int j1,j2;
    
    double gdopsq,pdopsq,tdopsq,hdopsq,vdopsq,temp;
    double vertx,verty,vertz;

    /* Compute geometric dilution of precision factors from the navigation
       matrix. */

    pdopsq = 0.0;
    for(j1=0; j1<3; ++j1)
    {
	for(j2=0; j2<NumSatsInSoln; ++j2)
	{
	    pdopsq += DSquare(NewNavState->navmat[j1][j2]);
	}
    }
    tdopsq = 0.0;
    for(j2=0; j2<NumSatsInSoln; ++j2)
    {
	tdopsq += DSquare(NewNavState->navmat[3][j2]);
    }
    temp = sqrt(DSquare(NewNavState->x)+DSquare(NewNavState->y)
		+ DSquare(NewNavState->z));
    vertx = NewNavState->x / temp;
    verty = NewNavState->y / temp;
    vertz = NewNavState->z / temp;
    vdopsq = 0.0;
    for(j2=0; j2<NumSatsInSoln; ++j2)
    {
	vdopsq += DSquare(NewNavState->navmat[0][j2]*vertx
			 + NewNavState->navmat[1][j2]*verty
			 + NewNavState->navmat[2][j2]*vertz);
    }
    gdopsq = pdopsq + tdopsq;
    hdopsq = pdopsq - vdopsq;
    NewNavState->gdop = sqrt(gdopsq);
    NewNavState->pdop = sqrt(pdopsq);
    NewNavState->tdop = sqrt(tdopsq);
    NewNavState->hdop = sqrt(hdopsq);
    NewNavState->vdop = sqrt(vdopsq);
}

/****************************************************************************    
*
* Function: int SolutionUpdate(navstatestruc *NewNavState,
*                              clockmodelstruc *NewClkModel,
*                              ompstruc omp, unsigned long obsTIC)
*
* Calculate the new position solution and clock model.
*
* Input: *NewNavState - the navigation matrix.
*        omp - structure of the observed minus predicted data.
*        obsTIC - time of the observation (TICs).
*
* Output: *NewNavState - the new solution.
*         *NewClkModel - the new clock model.
*
* Return Value: FALSE - a new solution did not occur.
*               TRUE - a new solution was made.
****************************************************************************/
int SolutionUpdate(navstatestruc *NewNavState, clockmodelstruc *NewClkModel,
		   ompstruc omp, unsigned long obsTIC)
{
    char buff[80];                              /* For messages to screen. */

    long clock_error_at_L1;                          /* Clock error at L1. */

    double position_update[4];           /* Update to the position vector. */
    double velocity_update[4];           /* Update to the velocity vector. */
    double clock_error;                        /* The current clock error. */
    double delta_time;          /* Time between observation & clock model. */
    double filter;                     /* The position filter coefficient. */

    /* It is usual to apply some sort of filter to position and clock
       corrections prior to their application. The type of filtering
       applied, and the coefficients used, can be very user application
       specific for an optimum solution. Many factors such as the anticipated
       recevier dynamics, and the inclusion of position aiding from another
       source other than GPS, effects the filter design used.
       
       Hence, to maintain the generality of GPS Builder-2, only a simple
       single coefficient filter is used where the position/clock corrections
       are scaled down by this factor. By default, the filter is disabled
       (coefficient = 1.0). The user can change this, and re-compile, but
       must consider what effect it will have for the user's particular
       application (a value of 8.0 is about right for a static receiver). */

    filter = 1.0;               /* At the moment don't filter the updates. */

    if(NewNavState->gdop > GdopMask)
    {
	sprintf(buff,
	    "NO NAV: GDOP exceeds GDOP mask value                     ");
	OutputString(2,23,buff);
	NewNavState->navmode = FIX_NO;
	PROTECT++;
	CurNavState = *NewNavState;
	PROTECT--;
	return(FALSE);
    }

    /* Compute position update by multiplying the navigation matrix by the
       vector of pseudorange errors and the velocity update by multiplying
       the navigation matrix by the vector of pseudorange rate errors. */

    {
	int j1,j2;
	double position,velocity;

	for(j1=0;j1<4;j1++)
	{
	    position = 0.0;
	    velocity = 0.0;
	    for(j2=0;j2<NumSatsInSoln;j2++)
	    {
		position += NewNavState->navmat[j1][j2]*omp.range[j2];
		velocity += NewNavState->navmat[j1][j2]*omp.range_rate[j2];
	    }
	    position_update[j1] = position;
	    velocity_update[j1] = velocity;
	}
    }

    if(DontMove==FALSE)
    {
	NewNavState->x -= position_update[0]/filter;
	NewNavState->y -= position_update[1]/filter; 
	NewNavState->z -= position_update[2]/filter; 
    }
    NewNavState->clockoff = position_update[3]/SPEED_OF_LIGHT;

    NewNavState->velx -= velocity_update[0]/filter; 
    NewNavState->vely -= velocity_update[1]/filter;
    NewNavState->velz -= velocity_update[2]/filter;
    NewNavState->clockdrift -= velocity_update[3]/filter/8.0; 

    /* Update the estimates of the Doppler offset due to the clock. */

    clock_error = NewNavState->clockdrift/SPEED_OF_LIGHT*L1
		  - NewClkModel->DoppFromClk;
    clock_error_at_L1 = NewClkModel->DoppFromClk + clock_error/filter;

    NewClkModel->DoppFromClk = (int)clock_error_at_L1;

    if(TrackMode != COLD_START)               /* All modes but Cold Start. */
    {
	disable();
	CodeDoppFromClk = 
	clock_error_at_L1/L1_TO_CA/CARRIER_DCO_RESOLUTION*CODE_DCO_SCALE+0.5;
	CarrDoppFromClk = 
	     -clock_error_at_L1/CARRIER_DCO_RESOLUTION*CARRIER_DCO_SCALE+0.5;
	enable();

	clock_error = clock_error/L1/1000.0;            /* Convert to ppm. */
	NewClkModel->VARClkError +=
	  (clock_error*clock_error-NewClkModel->VARClkError)/64.0;
	if(NewClkModel->VARClkError<=0.0)
	    NewClkModel->VARClkError = 1.0;
    }
    else                                                    /* Cold Start. */
    {
	disable(); 
	CodeDoppFromClk = 0;
	CarrDoppFromClk = 0;
	enable();
	NewClkModel->DoppFromClk = 0;
	NewClkModel->VARClkError = 1.0;
    }

    /* Update GPS time estimate by clock bias. */

    NewClkModel->RCOrate +=
		  (-NewNavState->clockdrift/SPEED_OF_LIGHT 
		   - NewClkModel->RCOrate)/8.0;
    delta_time = ((double)obsTIC-(double)NewClkModel->RCOtic)
				     *TIC_PERIOD*(1.0-NewClkModel->RCOrate);
    NewClkModel->RCOsec += delta_time*NewClkModel->RCOrate 
			   + NewNavState->clockoff/16.0;

    if(NewClkModel->RCOsec>=SECONDS_IN_WEEK)
    {
	NewClkModel->RCOsec -= SECONDS_IN_WEEK;
	NewClkModel->RCOwk++;
    }
    if(NewClkModel->RCOsec<0.0)
    {
	NewClkModel->RCOsec += SECONDS_IN_WEEK;
	NewClkModel->RCOwk--;
    }
    NewClkModel->RCOtic = obsTIC;

    return(TRUE);
}