solve.c

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

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

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

/*At many places in the following procedure solve the subdeterminant value of a
  4 x 4 array is required. For convenience the function sub is defined below*/

static double sub(mat16 A, long r, long c_)
{
  	/* IN:	A	input 4 x 4 array*/
  	/* IN:	r	row number to be deleted*/
  	/* IN:  c_	column number to be deleted*/
  	/* OUT:		value of 3 x 3 subdeterminant*/
  	double B[3][3];
  	long i, i1, j, j1;

  	i1 = 0;
  	for (i = 0; i <= 3; i++) 
	{
    		if (i + 1 != r) 	
		{
      			i1++;
      			j1 = 0;
      			for (j = 1; j <= 4; j++) 
			{
				if (j != c_) 
				{
	  				j1++;
	  				B[i1 - 1][j1 - 1] = A[i][j - 1];
				}
      			}
    		}
  	}
  	return (B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1]) + B[1]
	    	[0] * (B[2][1] * B[0][2] - B[0][1] * B[2][2]) + B[2]
	    	[0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]));
} 

void solve(double Xs[][3], boolean *SV, double *P, double *Xr, double *Cr, boolean *status)
{
  	/*  IN: Xs	array with 3 columns and 32 rows for the coordinates of the sat's*/
  	/*  IN: SV	valid prn's*/
  	/*  IN: P	pseudoranges*/
  	/*  IN: Xr 	input of initial final position*/
  	/* OUT: Cr	receiver clock error*/
  	/* OUT: status	true: calculation OK, false: no solution*/

  	long prn, it, i, j, k;
  	double R[32], L[32];
  	double A[32][4];
  	double AL[4];
  	mat16 AA, AAi;
  	long n;
  	double det;
  	double D[4];



  	it = 0;   /*iteration counter*/

  	do 
	{   /*iterations*/

    		it++;   /*increase iteration counter*/

    		for (prn = 0; prn <= 31; prn++) 
		{
      			if (SV[prn])
			{
				R[prn] = sqrt((Xr[0] - Xs[prn][0]) * (Xr[0] - Xs[prn][0]) +
		      			(Xr[1] - Xs[prn][1]) * (Xr[1] - Xs[prn][1]) +
		      			(Xr[2] - Xs[prn][2]) * (Xr[2] - Xs[prn][2]));
	    			/*range from receiver to satellite*/
				L[prn] = P[prn] - R[prn];   /*range residual value*/
				for (k = 0; k <= 2; k++)
				{
	  				A[prn][k] = (Xr[k] - Xs[prn][k]) / R[prn];
				}
	  			/* normalised user->satellite vectors */

				A[prn][3] = -1.0;   /*A is the geometry matrix or model matrix*/
      			}
    		}

    		for (k = 0; k <= 3; k++) 
		{   	/*calculate A.L*/
      			AL[k] = 0.0;
      			for (prn = 0; prn <= 31; prn++) 
			{
				if (SV[prn])
	  				AL[k] += A[prn][k] * L[prn];
      			}
    		}

    		for (k = 0; k <= 3; k++) 
		{
      			for (i = 0; i <= 3; i++) 
			{   	/*calculate A.A*/
				AA[k][i] = 0.0;
				for (prn = 0; prn <= 31; prn++) 
				{
	  				if (SV[prn])
					{
	    					AA[k][i] += A[prn][k] * A[prn][i];
					}
				}
      			}
    		}

    		/*invert A.A*/
    		det = AA[0][0] * sub(AA, 1L, 1L) - AA[1][0] * sub(AA, 2L, 1L) + AA[2]
	    		[0] * sub(AA, 3L, 1L) - AA[3][0] * sub(AA, 4L, 1L);
    		if (det == 0.0)
		{
	  		/*there is something wrong if more than 6 iterations are required*/
	    		*status = false;
		}
   		else 
		{
      			*status = true;

      			for (k = 1; k <= 4; k++)
			{
				for (i = 1; i <= 4; i++) 
				{
	  				n = k + i;
	  				if (n & 1)
					{
	    					j = -1;
					}
	  				else
					{
	    					j = 1;
					}
	  				AAi[k - 1][i - 1] = j * sub(AA, i, k) / det;
				}
      			}

      			/*calculate (invA.A).(A.L)*/
      			for (k = 0; k <= 3; k++) 
			{
				D[k] = 0.0;
				for (i = 0; i <= 3; i++)
				{
	  				D[k] += AAi[k][i] * AL[i];
				}
      			}

      			/*update position*/
      			for (k = 0; k <= 2; k++)
			{
				Xr[k] += D[k];
			}
    		}
  	}
	while 
	(
		it != 6 /* there is something wrong if more than 6 iterations are required*/
		&& fabs(D[0]) + fabs(D[1]) + fabs(D[2]) >= 1.0e-2    /*iteration criterion*/
		&& *status				       /*calculation not succeeded*/
	);
      

  	*Cr = D[3];   /*receiver clock error*/

  	if (it == 6) 
	{
    		printf("solve it : %12ld\n", it);
    		*status = false;
  	}  /*iteration not succeeded*/

}