Nav-Soft an OpenSource Software GNSS company.

GPS Receiver Position, Velocity and Time(PVT) Calculation.

Here we will go through the code that derives the GPS Receiver position, the theory behind the method explained here has already been described and you will see that much of the code is very similar to those equations. As we said before this code has been developed using the C programming language as that is the easiest for embedded programming.

We begin the calculation by defining several variables that will be used during the function, and then initialise the receiver clock, loop counter and error. we then begin the main loop and either repeat this until the change in error is less than 50.0 or we have done this loop 30 times. Normally this iterative process will converge to an answer very quickly, even with setting the position estimate to close to the center of the earth. So if the loop counter hits 30 it means there is a problem with the calculation.

double pos_calc_lsqf(int no_sats,int sats[])
{
GNSS_structSatPVT *sat_pos;
GNSS_structPVT *spvt;
GNSS_structPVT *rpvt;
double	delta_error,prev_error,
		error,alpha,
		doppler_freq;
double	time_of_flight;
double	X[N_CHANNELS][4];
double	y[N_CHANNELS],c[4] ;
int		sat, chan,
		count;
int		no_iono=1;

	rpvt = &rx_pos.pvt;
	rpvt->clk = 0.0;
	count = 0;
	prev_error=0.0;
	delta_error=1000000.0;
	while(delta_error>50.0 && count < 30)
	{
		for(sat=0;sat<no_sats;sat++)
		{
			chan = sats[sat];
			sat_pos = &rx_meas[chan].sat_pos;
			spvt = &sat_pos->pvt;
			rx_meas[chan].range = rx_meas[chan].psr_smoothed;

We now loop over all the satellites that are included in the calculation, this means the receiver has a lock on them and a valid pseudorange value. We can either use the measured pseudorange directly or use a pseudorange that has been smoothed by adding the ADR to it and using a lowpass filter. The satellite clock correction factor is subtracted from the pseudorange to correct for inaccuracies in the satellite clock.

			time_of_flight = rx_meas[chan].range;
			time_of_flight /= SPEED_OF_LIGHT;
			sat_clk(chan,rx_meas[chan].tow - time_of_flight);
			rx_meas[chan].range += rx_meas[chan].corrections.prcSatClk;
			time_of_flight += rx_meas[chan].corrections.prcSatClk;

			if(rx_meas[chan].system == GNSS_GPS)
				eph_sat_pos(chan,rx_meas[chan].tow - time_of_flight);
			else if(rx_meas[chan].system == GNSS_WAAS)
				eph_geo_pos(chan,rx_meas[chan].tow - time_of_flight);

Next the satellite positions are calculated, but for the current time estimate minus the time of flight, this means the position of the satellite when the signal was transmitted.

			// Calculate corrections to pseudorange 
			sat_el_angle(sat_pos);
			sat_az_angle(sat_pos);
			sat_doppler(sat_pos);
			// Need at least a rough position to do these calculations

We now calculate the Elevation and Azimuth angles of the satellites from the estimated receiver position for use in the Ionospheric and Tropospheric correction factor calculations. The satellite relative doppler frequency is also calculated at this time for use later in the GPS receiver velocity and clock drift calculations.

			if(count > 0 || rx_pos.isValid)
			{
				tropo_model(chan);
				iono_model(chan);
			}
			rx_meas[chan].range -= rx_meas[chan].corrections.prcIono;
			rx_meas[chan].range -= rx_meas[chan].corrections.prcTropoDry;

The ionospheric and tropospheric correction factors are calculated for the current estimated GPS Receiver position and used to adjust the measured pseudorange to allow for these effects.

			spvt->x +=OMEGA_E*rpvt->y*time_of_flight;
			spvt->y -=OMEGA_E*rpvt->x*time_of_flight;

The satellite position x and y co-ordinates are adjusted to allow for the Earth's rotation. These updated values are then used in the determination of the factors used in the GPS Receiver position calculation.

			alpha = sqrt(dist(spvt,rpvt)) - rpvt->clk;
			X[sat][0] = (spvt->x - rpvt->x)/alpha;
			X[sat][1] = (spvt->y - rpvt->y)/alpha;
			X[sat][2] = (spvt->z - rpvt->z)/alpha;
			X[sat][3] = 1.0;

			y[sat] = alpha - rx_meas[chan].range;
		}

		if(solve_linear_eqns(X, y, c, no_sats) == 1) return(1);

Once the factors for the GPS receiver calculation have been determined for each of the satellites in use, these are included in the inputs to the routine that solves the system of equations and produces the corrections to the current estimated receiver position.

		rpvt->x += c[0];
		rpvt->y += c[1];
		rpvt->z += c[2];
		rpvt->clk += c[3];

		for(sat=0,error=0.0;sat<no_sats;sat++)
		{
			chan = sats[sat];
			sat_pos = &rx_meas[chan].sat_pos;
			spvt = &sat_pos->pvt;
			error += y[sat];
			error -= rx_meas[chan].range*rx_meas[chan].range;
		}

		error *= 100000.0; 
		delta_error = prev_error - error;
		delta_error = fabs(delta_error);
		prev_error = error;
		count++;
	}
	rx_pos.isValid = 1;

The results from the solution to the set of equations is used to improve the accuracy of the estimated solution, then the error and the change in the error are calculated to see if another iteration is required or not. If not then it is assumed a valid solution was determined.

	for(sat=0;sat<no_sats;sat++)
	{
		chan = sats[sat];
		sat_pos = &rx_meas[chan].sat_pos;
		spvt = &sat_pos->pvt;
		alpha = sqrt(dist(spvt,rpvt)) - rpvt->clk;
		X[sat][0] = (spvt->x - rpvt->x)/alpha;
		X[sat][1] = (spvt->y - rpvt->y)/alpha;
		X[sat][2] = (spvt->z - rpvt->z)/alpha;
		X[sat][3] = 1.0;

		y[sat] = rx_meas[chan].sat_pos.doppler;
		y[sat] -= rx_meas[chan].doppler;
		y[sat] *= L1_LAMBDA;

	}

	solve_linear_eqns(X, y, c, no_sats);

	rpvt->vx = c[0];
	rpvt->vy = c[1];
	rpvt->vz = c[2];
	rpvt->clkdrift = c[3];

Now the GPS receiver velocity and clock drift are determined but since we now know the receiver position only a single pass through the equations is required, these equations are similar to the position equations only now we use doppler frequency instead of range.

	if(count >= 30)
	{
		rpvt->clk = 0.0;
		rpvt->x = 0.0;
		rpvt->y = 0.0;
		rpvt->z = 0.0;
		error = 15000000.0;
		rx_pos.isValid = 0;
	}

	return(error);
} 
© 2018