stdalone.c

根据GPS官方网站提供的gps定位方法而编写的C语言gps定位代码

#include <stdio.h>
#include <math.h>
#include "gpsconst.h"

/*****************************************************************************/

static void calcAzEl(double *Xs, double *Xu, double *Az, double *El, boolean *stat)
{
  	/* IN: 	satellite ECEF XYZ*/
  	/* IN: 	user ECEF XYZ*/
  	/* OUT: azimuth [rad]*/
  	/* OUT: elevation [rad]*/
  	/* OUT: calculation succeeded: stat = true*/
  	double R, p, x, y, z, s;
 	double e[3][3];
  	long i, k;
  	vec3 d;

  	x = Xu[0];
  	y = Xu[1];
  	z = Xu[2];
  	p = sqrt(x * x + y * y);
  	if (p == 0.0)
    		*stat = false;
  	else
    		*stat = true;

  	if (!*stat)
    		return;

  	R = sqrt(x * x + y * y + z * z);
  	e[0][0] = -(y / p);
  	e[0][1] = x / p;
  	e[0][2] = 0.0;
  	e[1][0] = -(x * z / (p * R));
  	e[1][1] = -(y * z / (p * R));
  	e[1][2] = p / R;
  	e[2][0] = x / R;
  	e[2][1] = y / R;
  	e[2][2] = z / R;

	/* matrix multiply vector from user */
  	for (k = 0; k <= 2; k++) 
	{
    		d[k] = 0.0;
    		for (i = 0; i <= 2; i++)
		{
      			d[k] += (Xs[i] - Xu[i]) * e[k][i];
		}
  	}

  	s = d[2] / sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
  	if (s == 1.0)
	{
    		*El = 0.5 * pi;
	}
  	else
	{
    		*El = atan(s / sqrt(1.0 - s * s));
	}

  	if (d[1] == 0.0 && d[0] > 0.0) 
	{
    		*Az = 0.5 * pi;
  	}
  	else if (d[1] == 0.0 && d[0] < 0.0) 
	{
    		*Az = 1.5 * pi;
  	}
	else
	{
  		*Az = atan(d[0] / d[1]);
  		if (d[1] < 0.0)
		{
    			*Az += pi;
		}
  		else if (d[1] > 0.0 && d[0] < 0.0)
		{
      			*Az += 2.0 * pi;
		}
 
	}
}  /*calcAzEl*/


/*****************************************************************************/

static void ionocorr
(
	double *ion, double Latu, double Lonu, double Az, double El, double Ttr, double *dTiono
)
{
  	/*ion 		iono correction coefficients from nav message*/
  	/*Latu		user's latitude [rad]*/
  	/*Lonu		user's longitude [rad]*/
  	/*Az		SV azimuth [rad]*/
  	/*El		SV elevation [rad]*/
  	/*Ttr		SV signal transmission time [sec]*/
  	/*dTiono	Ionospheric delay [sec]*/
  	double phi, Lati, Loni, Latm, T, F_, x, per, amp, a0, a1, a2, a3, b0, b1, b2, b3;


  	/*for clarity copy array*/
  	a0 = ion[0];
  	a1 = ion[1];
  	a2 = ion[2];
  	a3 = ion[3];
  	b0 = ion[4];
  	b1 = ion[5];
  	b2 = ion[6];
  	b3 = ion[7];

  	/*convert from radians to semicircles*/
  	Latu /= pi;
  	Lonu /= pi;
  	Az /= pi;
  	El /= pi;

  	/*calculation*/
  	phi = 0.0137 / (El + 0.11) - 0.022;
  	Lati = Latu + phi * cos(Az * pi);
  	if (Lati > 0.416)
	{
    		Lati = 0.416;
	}
  	else if (Lati < -0.416)
	{
    		Lati = -0.416;
	}
  	Loni = Lonu + phi * sin(Az * pi) / cos(Lati * pi);
  	Latm = Lati + 0.064 * cos((Loni - 1.617) * pi);
  	T = 4.32e+4 * Loni + Ttr;
  	if (T >= 86400L)
	{
    		T -= 86400L;
	}
  	else if (T < 0)
	{
    		T += 86400L;
	}
  	F_ = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  	per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;
  	if (per < 72000.0)
	{
    		per = 72000.0;
	}
  	x = 2 * pi * (T - 50400.0) / per;
  	amp = a0 + a1 * Latm + a2 * Latm * Latm + a3 * Latm * Latm * Latm;
  	if (amp < 0.0)
	{
    		amp = 0.0;
	}
  	if (fabs(x) >= 1.57)
	{
    		*dTiono = F_ * 5.0e-9;
	}
  	else
	{
    		*dTiono = F_ * (5.0e-9 + amp * (1.0 - x * x / 2.0 + x * x * x * x / 24.0));
  	}
}

void stdalone_input_phase
(
	double * Xlla, double * Trc, boolean * SV, double eph[32][16], 
	double clk[32][5], vec8 ion, double *Praw
)
{
	FILE * inp;
	int i;
	int prn;

  	/*the following data should be available:
    	1. Pseudorange with receiver time of reception for each SV
    	2. Ephemeris and almanac for each SV
    	3. Iono coefficients*/

  	/*open input datafile*/
  	inp = fopen("inp.txt","r");
  	fscanf(inp, "%*[^\n]");   /*skip comment line*/

  	/*read GPS time of reception*/
  	getc(inp);
  	fscanf(inp, "%lg%*[^\n]", Trc);
  	getc(inp);

  	/*read iono coefficients*/
  	fscanf(inp, "%*[^\n]");   /*skip comment line*/
  	getc(inp);
  	for (i = 1; i <= 8; i++) 
	{
    		fscanf(inp, "%lg%*[^\n]", &ion[i - 1]);
    		getc(inp);
 	}

  	fscanf(inp, "%*[^\n]");   /*skip comment line*/
  	getc(inp);
  	/*read pseudoranges*/
  	for (prn = 1; prn <= 32; prn++)
	{
    		SV[prn - 1] = false;
	}
  	do 
	{
    		fscanf(inp, "%ld", &prn);
    		if (prn != 0) 
		{
      			fscanf(inp, "%lg%*[^\n]", &Praw[prn - 1]);
      			getc(inp);
      			SV[prn - 1] = true;
    		} 
		else 
		{
      			fscanf(inp, "%*[^\n]");
      			getc(inp);
    		}
  	} while (prn != 0);

  	fscanf(inp, "%*[^\n]");   /*skip comment line*/
  	getc(inp);
  	/*read ephemeris- and clock data*/
  	while (  fscanf(inp, "%ld%*[^\n]", &prn) == 1)
	{
    		getc(inp);
    		for (i = 1; i <= 16; i++) 
		{
      			fscanf(inp, "%lg%*[^\n]", &eph[prn - 1][i - 1]);
      			getc(inp);
    		}
    		for (i = 1; i <= 5; i++) 
		{
      			fscanf(inp, "%lg%*[^\n]", &clk[prn - 1][i - 1]);
      			getc(inp);
   		}
  	}

  	/*user input of start position*/
  	printf("Start position Lat [deg.dec], Lon [deg.dec], Alt [m] : ");
  	scanf("%lg%lg%lg%*[^\n]", Xlla, &Xlla[1], &Xlla[2]);
  	getchar();

    	fclose(inp);
}
void stdalone_calc_phase
(
	double * Xlla, double Trc, boolean * SV, double eph[32][16], 
	double clk[32][5], vec8 ion, double *Praw
)
{
	FILE * out;
	static boolean status;
	static vec3 tmp3;
	static double Ttr, tau, T, Trel, Az, El, Cr, alpha, dTclck, dTiono,
	      dRtrop, Lat, Lon;
	
	static vec32 Pcor;
	static mat96 Xs;
	static vec32 P;
	static vec3 Xr;

	int prn;
  	Xlla[0] = Xlla[0] * pi / 180.0; /* convert angle from degrees to radians - david */
  	Xlla[1] = Xlla[1] * pi / 180.0; /* convert angle from degrees to radians - david  */
  	/*convert lat, ln, alt to ECEF X, Y, Z*/
  	LLA2XYZ(Xlla, Xr);

  	/*open output data file*/
  	out = fopen("out.txt","w");
	if (out == NULL)
	{
		fprintf(stderr,"Unable to open file out.txt for writing\n");
	}

  	/*assuming the receiver clock error and initial position not sufficiently
    	good known, I make two passes through the processing steps*/

  	/*PASS 1*/
  	fprintf(out, "PASS 1\n");

  	for (prn = 1; prn <= 32; prn++) 
	{
    		if (SV[prn - 1]) 
		{  	/*do for each SV*/
      			/*set all transit times to nominal value and calculate time of transmission*/
      			tau = 0.075;

      			Ttr = Trc - tau;

      			/*calculate SV position and correct for earth rotation*/
      			satpos(eph[prn - 1], Ttr, &Trel, tmp3);
      			alpha = tau * We;

   	   		Xs[prn - 1][0] = tmp3[0] * cos(alpha) + tmp3[1] * sin(alpha);
      			Xs[prn - 1][1] = tmp3[1] * cos(alpha) - tmp3[0] * sin(alpha);
      			Xs[prn - 1][2] = tmp3[2];
      			fprintf(out, "SV     : %2ld%15.3f%15.3f%15.3f\n",
	      			prn, Xs[prn - 1][0], Xs[prn - 1][1], Xs[prn - 1][2]);

      			/*calculate azimuth and elevation*/
      			calcAzEl( Xs[prn - 1] , Xr, &Az, &El, &status);
      			if (!status) 
			{
				printf("Error in calcAzEl - check input data\n");
				return;
      			}
      			fprintf(out, "Az, El : %2ld%11.3f%10.3f\n",
	      			prn, Az * 180.0 / pi, El * 180.0 / pi);

      			/*calculate pseudorange corrections and apply to pseudoranges*/

      			/*clock correction*/
      			dTclck = clk[prn - 1][4] + clk[prn - 1][3] * (Ttr - clk[prn - 1][1]) +
	       			clk[prn - 1][2] * (Ttr - clk[prn - 1][1]) * (Ttr - clk[prn - 1]
		 		[1]) + Trel - clk[prn - 1][0];

      			/*iono correction*/
     	 		Lat = Xlla[0];
      			Lon = Xlla[1];
      			ionocorr(ion, Lat, Lon, Az, El, Ttr, &dTiono);

      			/*tropo correction using standard atmosphere values*/
      			dRtrop = 2.312 / sin(sqrt(El * El + 1.904e-3)) +
	       			0.084 / sin(sqrt(El * El + 0.6854e-3));

      			fprintf(out, "Corr   : %2ld%11.3f%10.3f%10.3f\n",
	      			prn, dTclck * c, dTiono * c, dRtrop);

      			/*correct pseudorange*/
      			Pcor[prn - 1] = Praw[prn - 1] + dTclck * c - dTiono * c - dRtrop;

		}  /*do for each SV*/

  	}

  	/*calculate receiver position*/
  	solve(Xs, SV, Pcor, Xr, &Cr, &status);
  	if (!status) 
	{
		int i;
		printf("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx\n");
		for(i=0;i<32;++i)
		{
			/* printf("SV[%d] = %d\n",i,SV[i]); */
			if ( SV[i] )
			{
printf("sat_xyz(%d) = (%f,%f,%f) r = %f usr_xyz=(%f,%f,%f) dclk = %f status=%d\n",
	i,
	Xs[i][0],Xs[i][1],Xs[i][2],
	Pcor[i],
	Xr[0],Xr[1],Xr[2],
	Cr,  
	status
	);
			}
		}
    		printf("Error in solve 1 - check input data\n");
		printf("yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy\n");
    		return;
  	}
  	fprintf(out, "Pos XYZ: %12.3f%12.3f%12.3f%12.3f\n", Xr[0], Xr[1], Xr[2], Cr);
  	/*convert back to Lat, Lon, Alt*/
  	XYZ2LLA(Xr, Xlla);

  	/*PASS 2 - The receiver position and -clock error is now well enough known
    	to calculate the final pseudorange corrections*/
  	fprintf(out, "\nPASS 2\n");

  	for (prn = 1; prn <= 32; prn++) 
	{
    		if (SV[prn - 1]) 
		{  	/*do for each SV*/
      			/*recalculate transit time and time of transmission*/
      			tau = (Pcor[prn - 1] + Cr) / c;
      			Ttr = Trc - tau;

      			/*recalculate SV position and correct for earth rotation*/

      			satpos(eph[prn - 1], Ttr, &Trel, tmp3);
      			alpha = tau * We;
      			Xs[prn - 1][0] = tmp3[0] * cos(alpha) + tmp3[1] * sin(alpha);
      			Xs[prn - 1][1] = tmp3[1] * cos(alpha) - tmp3[0] * sin(alpha);
      			Xs[prn - 1][2] = tmp3[2];
      			fprintf(out, "SV     : %2ld%15.3f%15.3f%15.3f\n",
	      			prn, Xs[prn - 1][0], Xs[prn - 1][1], Xs[prn - 1][2]);

      			/*recalculate azimuth and elevation*/
      			calcAzEl( Xs[prn - 1], Xr, &Az, &El, &status);
     		 	if (!status) 
			{
				printf("Error in calcAzEl - check input data\n");
				return;
      			}
      			fprintf(out, "Az, El : %2ld%11.3f%10.3f\n",
	      			prn, Az * 180.0 / pi, El * 180.0 / pi);

      			/*recalculate pseudorange corrections and apply to pseudoranges*/

      			/*clock correction*/
      			dTclck = clk[prn - 1][4] + clk[prn - 1][3] * (Ttr - clk[prn - 1][1]) +
	       			clk[prn - 1][2] * (Ttr - clk[prn - 1][1]) * (Ttr - clk[prn - 1]
			 	[1]) + Trel - clk[prn - 1][0];

      			/*iono correction*/
      			Lat = Xlla[0];
      			Lon = Xlla[1];
      			ionocorr(ion, Lat, Lon, Az, El, Ttr, &dTiono);

      			/*tropo correction using standard atmosphere values*/
      			dRtrop = 2.312 / sin(sqrt(El * El + 1.904e-3)) +
	       			0.084 / sin(sqrt(El * El + 0.6854e-3));

      			fprintf(out, "Corr   : %2ld%11.3f%10.3f%10.3f\n",
	      			prn, dTclck * c, dTiono * c, dRtrop);

      			/*correct pseudorange*/
      			Pcor[prn - 1] = Praw[prn - 1] + dTclck * c - dTiono * c - dRtrop + Cr;

    		}  /*do for each SV*/

  	}

  	/*calculate receiver position*/
  	solve(Xs, SV, Pcor, Xr, &Cr, &status);
  	if (!status) 
	{
    		printf("Error in solve 2 - check input data\n");
    		return;
  	}
#define TERMINAL_OUTPUT 0

  	fprintf(out, "Pos XYZ: %12.3f%12.3f%12.3f%12.3f\n", Xr[0], Xr[1], Xr[2], Cr);
#if TERMINAL_OUTPUT
  	fprintf(stdout, "Pos XYZ: %12.3f%12.3f%12.3f%12.3f\n", Xr[0], Xr[1], Xr[2], Cr);
#endif
  	/*convert back to Lat, Lon, Alt*/
  	XYZ2LLA(Xr, Xlla);
  	fprintf(out, "Pos LLA: %15.8f%15.8f%12.3f\n",
	  	Xlla[0] * 180.0 / pi, Xlla[1] * 180.0 / pi, Xlla[2]);
#if TERMINAL_OUTPUT
 	fprintf(stdout, "Pos LLA: %15.8f%15.8f%12.3f\n",
	  	Xlla[0] * 180.0 / pi, Xlla[1] * 180.0 / pi, Xlla[2]);

	calc_error( Xlla[0] * 180.0 / pi, Xlla[1] * 180.0 / pi, Xlla[2] );
#endif
    	fclose(out);
}