sdsmooth.c

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

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




#define slip            15.0   	/*criterion for cycle slip determination*/
#define Nmax            100   	/*limit value for number of observations*/

void take_smooth_note
( 
	int sv, boolean *SV, double *P, double *C, double *S, long *N,
	double Pn, double Cn
)
{
  	SV[sv] = true;   				/* declare prn valid */
    	N[sv]++;   		/*raise counter for number of observations*/
    	if (N[sv] > Nmax)   	/*limit value of N[prn]*/
	{
      		N[sv] = Nmax;
	}

    	if (N[sv] > 1 && fabs(Pn - P[sv] - Cn + C[sv]) > slip)
	{
	  	/*cycle slip found, re-initialize filter*/
	    	N[sv] = 1;
	}

    	if (N[sv] == 1)
	{
      		S[sv] = Pn;   /*first observation*/
	}
    	else  /*following observation*/
	{
      		S[sv] = Pn / N[sv] +
	  	(S[sv] + Cn - C[sv]) * (N[sv] - 1) / N[sv];
	}

    	P[sv] = Pn;   /*store Pn for next epoch*/
    	C[sv] = Cn;   /*store Cn for next epoch*/
}
int readsmooth
(
	FILE **inp, double *T, boolean *SV, double *P, double *C, double *S, long *N
)
{
  	/* IN: 	input file to read from		inp	*/
  	/* OUT: observation time		T	*/
  	/* OUT:	valid SV's			SV	*/
  	/* OUT:	raw pseudoranges		P	*/
  	/* OUT:	carrier ranges			C	*/
  	/* OUT:	smoothed pseudoranges		S	*/
  	/* OUT:	number of observations		N	*/

  	/*this procedure reads pseudoranges and carrier ranges from file, smoothes
    	the pseudoranges and returns the smoothed pseudoranges for further
    	processing*/
  	long i, k, prn;
  	double Pn, Cn;


  	for (prn = 0; prn <= 31; prn++)   /*initialize array*/
    		SV[prn] = false;

  	if ( fscanf(*inp, "%lg%ld", T, &k) != 2 )
		return 0;	/* no more input */

      	/* read observation time and number of observations in epoch */

  	for (i = 1; i <= k; i++) 
	{
    		fscanf(*inp, "%ld%lg%lg", &prn, &Pn, &Cn);   	/* read data of prn  */
		take_smooth_note( prn - 1, SV, P, C, S, N, Pn, Cn);
  	}
  	fscanf(*inp, "%*[^\n]");   /*read to end of line*/
  	getc(*inp);
	return 1;
} 

#undef slip
#undef Nmax


sdsmooth_input_support(vec3 Xr,vecb32 SVe, double eph[32][16])
{
	FILE *ine;
	int prn;
	int i;

  	/*open support data file and read data*/
  	ine = fopen("support.txt","r");
  	fscanf(ine, "%*[^\n]");   /*skip comment line*/
  	/*read ref rcvr coordinates*/
  	getc(ine);
  	fscanf(ine, "%lg%lg%lg%*[^\n]", Xr, &Xr[1], &Xr[2]);
  	getc(ine);
 	fscanf(ine, "%*[^\n]");   /*skip comment line*/
  	getc(ine);
  	for (prn = 1; prn <= 32; prn++)   /*initialize array with eph's*/
	{
    		SVe[prn - 1] = false;
	}
  	/*read ephemeris data*/
  	while ( fscanf(ine, "%ld%*[^\n]", &prn) == 1 )
	{
    		getc(ine);
    		for (i = 1; i <= 16; i++) 
		{
      			fscanf(ine, "%lg%*[^\n]", &eph[prn - 1][i - 1]);
      			getc(ine);
    		}
    		SVe[prn - 1] = true;
  	}

    	fclose(ine);	 /*close support data file*/
}
sdsmooth_calc_phase
(
	FILE * out,
	vec3 Xr,
	vecb32 SVe, vecb32 SVr, vecb32 SVm,
	vec32 Sr, vec32 Sm,
	double eph[32][16],
	double Tr
)
{
	int Nc;
	int prn;
	vecb32 SVc;
	vec3 tmp3;
  	mat96 Xs;
	double tau;
	double Ttr;
	double Trel;
	double alpha, Rr, dClock;
 	vec32 Smc;
 	vec3 Xm;
	int i;
  	boolean status;
	vec3 Xlla;

    	/*determine SV's with valid ephemeris, ref data and mov data*/
    	Nc = 0;   /*number of SV's which passes the criterion below*/
    	for (prn = 1; prn <= 32; prn++) 
	{
		/* valid refernce, mobile, and ephemeris */

      		if (SVe[prn - 1] && SVr[prn - 1] && SVm[prn - 1]) 
		{
			Nc++;
			SVc[prn - 1] = true;
      		} 
		else
		{
			SVc[prn - 1] = false;
		}
   	 }

    	for (prn = 1; prn <= 32; prn++) 
	{
      		if (SVc[prn - 1]) 
		{  
			/*do for each valid SV*/
			/*calculate transit time and time of transmission*/
			tau = Sr[prn - 1] / c;	/* smoothed reference pseudo range - david */
			Ttr = Tr - 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];

			/*calculate differential corrected pseudorange*/
			Rr = sqrt((Xr[0] - Xs[prn - 1][0]) * (Xr[0] - Xs[prn - 1][0]) +
		  		(Xr[1] - Xs[prn - 1][1]) * (Xr[1] - Xs[prn - 1][1]) +
		  		(Xr[2] - Xs[prn - 1][2]) * (Xr[2] - Xs[prn - 1][2]));
			Smc[prn - 1] = Sm[prn - 1] - Sr[prn - 1] + Rr;

      		}  /*do for each SV*/

    	}

    	for (i = 1; i <= 3; i++)   /*initial guess for mov rcvr position*/
	{
      		Xm[i - 1] = Xr[i - 1];
	}

    	if (Nc > 3) 
	{   
		/*sufficient SV's to solve position*/
      		/*calculate receiver position*/
      		solve(Xs, SVc, Smc, Xm, &dClock, &status);

      		if (status) 
		{
			/*convert back to Lat, Lon, Alt*/
			XYZ2LLA(Xm, Xlla);

			fprintf(out, "%10.3f%12.3f%12.3f%12.3f%15.8f%15.8f%12.3f\n",
				Tr, Xm[0], Xm[1], Xm[2], Xlla[0] * 180.0 / pi,
				Xlla[1] * 180.0 / pi, Xlla[2]);
      		}
		return status;
    	}
	else
	{
		return 0;
	}
}