matrix_tools.c

GPS卫星导航接收机的仿真程序。用C语言实现

		if (A[i][i] < 0.0)
			return(ERROR2);

		if (fabs(A[i][i]) < 1.0e-15)
			return(ERROR3);
	}
								/* Perform Choleski decomposition */
	for (j=0; j<(int)n; j++) {
		for( k=0; k<j; k++)
			A[j][j] -= A[j][k] * A[j][k];

		if (A[j][j] < 0.0)
		  return(ERROR4);

		A[j][j] = sqrt(A[j][j]);

		for(i=j+1; i<(int)n; i++) {
			for (k=0; k<j; k++)
			  A[i][j] -= A[i][k]*A[j][k];

		  if (fabs(A[j][j]) < 1.0e-12)
			  return(ERROR5);

			A[i][j] /= A[j][j];
		}
	}
								/* Inversion of lower triangular matrix */
	for (j=0; j<(int)n; j++) {
		A[j][j] = 1.0/A[j][j];

		for (i=j+1; i<(int)n; i++) {
		  A[i][j] *= -A[j][j]/A[i][i];

		  for (k=j+1; k<i; k++)
			  A[i][j] -= A[i][k]*A[k][j]/A[i][i];
		}
	}

						/* Construction of lower triangular inverse matrix */
	for (j=0; j<(int)n; j++) {
		for (i=j; i<(int)n; i++) {
			A[i][j] *= A[i][i];

			for (k=i+1; k<(int)n; k++)
				A[i][j] += A[k][i]*A[k][j];
		}
	}

								/* fill upper diagonal */
	for (i=1; i<(int)n; i++) {
		for (j=0; j<i; j++)
			A[j][i] = A[i][j];
	}

	return SUCCESS;
}




void	xyz2llh( double xyz[], double llh[] )
{
	//	XYZ2LLH	Convert from ECEF cartesian coordinates to 
	//               latitude, longitude and height.  WGS-84
	//
	//	llh = XYZ2LLH(xyz)	
	//
	//    INPUTS
	//	xyz(1) = ECEF x-coordinate in meters
	//	xyz(2) = ECEF y-coordinate in meters
	//	xyz(3) = ECEF z-coordinate in meters
	//
	//    OUTPUTS
	//	llh(1) = latitude in radians
	//	llh(2) = longitude in radians
	//	llh(3) = height above ellipsoid in meters

	//	Reference: Understanding GPS: Principles and Applications,
	//	           Elliott D. Kaplan, Editor, Artech House Publishers,
	//	           Boston, 1996.
	//
	//	M. & S. Braasch 10-96
	//	Copyright (c) 1996 by GPSoft
	//	All Rights Reserved.
	//
	double x,y,z, x2,y2,z2,a,b,e,b2,e2,ep,r,r2,E2,F,G,c,s,P,Q,ro,tmp,U,V,zo,height,lat,lon;
	x = xyz[0];
	y = xyz[1];
	z = xyz[2];
	x2 = x*x;
	y2 = y*y;
	z2 = z*z;

	a = 6378137.0000;	// earth radius in meters
	b = 6356752.3142;	// earth semiminor in meters	
	e = sqrt(1-(b*b/(a*a)) );
	b2 = b*b;
	e2 = e*e;
	ep = e*a/b;
	r = sqrt(x2+y2);
	r2 = r*r;
	E2 = a*a - b*b;
	F = 54*b2*z2;
	G = r2 + (1-e2)*z2 - e2*E2;
	c = (e2*e2*F*r2)/(G*G*G);
	s = pow( ( 1 + c + sqrt(c*c + 2*c) ), 1/3 );
	P = F / (3 * (s+1/s+1)*(s+1/s+1) * G*G);
	Q = sqrt(1+2*e2*e2*P);
	ro = -(P*e2*r)/(1+Q) + sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2);
	tmp = (r - e2*ro)*(r - e2*ro);
	U = sqrt( tmp + z2 );
	V = sqrt( tmp + (1-e2)*z2 );
	zo = (b2*z)/(a*V);

	height = U*( 1 - b2/(a*V) );
	
	lat = atan( (z + ep*ep*zo)/r );

	lon = atan2(y,x);

	
	llh[0] = lat;
	llh[1] = lon;
	llh[2] = height;
}




void	enu2xyz(double enu[], double orgxyz[], double xyz[])
{
	//	ENU2XYZ	Convert from rectangular local-level-tangent ('East'-'North'-Up)
	//               coordinates to WGS-84 ECEF cartesian coordinates.
	//               
	//
	//	xyz = ENU2XYZ(enu,orgxyz)	
	//
	//    INPUTS
	//	enu(1) = 'East'-coordinate relative to local origin (meters)
	//	enu(2) = 'North'-coordinate relative to local origin (meters)
	//	enu(3) = Up-coordinate relative to local origin (meters)
	//
	//	orgxyz(1) = ECEF x-coordinate of local origin in meters
	//	orgxyz(2) = ECEF y-coordinate of local origin in meters
	//	orgxyz(3) = ECEF z-coordinate of local origin in meters
	//
	//    OUTPUTS
	//       enu:  Column vector
	//		xyz(1) = ECEF x-coordinate in meters
	//		xyz(2) = ECEF y-coordinate in meters
	//		xyz(3) = ECEF z-coordinate in meters

	//	Reference: Alfred Leick, GPS Satellite Surveying, 2nd ed.,
	//	           Wiley-Interscience, John Wiley & Sons, 
	//	           New York, 1995.
	//
	//	M. & S. Braasch 10-96
	//	Copyright (c) 1996 by GPSoft
	//	All Rights Reserved.

	double	orgllh[3], xyz_dif[3], phi, lam, sinphi, cosphi, sinlam, coslam;

	xyz2llh(orgxyz, orgllh);
	phi = orgllh[0];
	lam = orgllh[1];
	sinphi = sin(phi);
	cosphi = cos(phi);
	sinlam = sin(lam);
	coslam = cos(lam);

	xyz_dif[0] = -sinlam*enu[0] - sinphi*coslam*enu[1] + cosphi*coslam*enu[2];
	xyz_dif[1] = coslam*enu[0]	- sinphi*sinlam*enu[1] + cosphi*sinlam*enu[2];
	xyz_dif[2] =				cosphi*enu[1] + sinphi*enu[2];

	xyz[0]	=	orgxyz[0]	+	xyz_dif[0];
	xyz[1]	=	orgxyz[1]	+	xyz_dif[1];
	xyz[2]	=	orgxyz[2]	+	xyz_dif[2];

}



//******************************************************************************
//**	Function:	cartesian2geo																**
//**	Input:		Array containing the point coordinates in cartesian form 	**
//**					the array to contain the same point in curvilinear 			**
//**					coordinates.																**
//**	Tasks:		Function computes the curvilinear coordinates equivalent to	**
//**					those given by the cartesian coordinates.  The coordinates 	**
//**					are determined using WGS84 parameters.								**
//**	Notes:		Angles returned will be in units of radians.						**
//**																									**
//**	Author:		Mark G. Petovello															**
//**	Date:			July, 1998																	**
//******************************************************************************

void xyz2lla( double	*xyz,		// cartesian coordinates
						  double	*plh )	// curvilinear coordinates
{

#ifndef		A_ELL

#define		A_ELL 6378137.0						// semi-major axis of WGS84 ellipsoid
#define		B_ELL 6356752.3141					// semi-minor axis of WGS84 ellipsoid
#define		E2 (A_ELL*A_ELL - B_ELL*B_ELL)/(A_ELL*A_ELL)	// eccentricity of WGS84 ellipsoid
#define		Wedot        7.2921151467e-5 

#endif

	int   	i;				// counter
	double	N;				// prime vertical radius of curvature
	double	p;				// distance of point from rotation axis
	double 	lat_temp;	// temporary latitude value
	double	hgt_temp;	// temporary height value
	double 	lat_old;		// last latitude solution
	double	hgt_old;		// last height solution

	// assign dummy values to previous lat & hgt solutions
	lat_old = 0.0;
	hgt_old = 0.0;

	// zero lat, lon, hgt values
	for(i=0;i<3;i++)
		plh[i] = 0.0;

	//***************************************************************************
	//**                     Compute longitude directly                        **
	//***************************************************************************

	plh[1] = atan2(xyz[1],xyz[0]);


	//***************************************************************************
	//**                        Test for singularity                           **
	//***************************************************************************

	// compute distance from rotation axis
	p = sqrt(xyz[0]*xyz[0] + xyz[1]*xyz[1]);

	// if the range is approximately zero (<1mm), then there is a singularity (at pole)
	if( p<0.001 )
	{
		// test for hemisphere using 'Z' coordinate and assign latitude value
		if(xyz[2]>0)	// northern hemisphere
			plh[0] = PI/2.0;
		else		// southern hemisphere
			plh[0] = -PI/2.0;

		// determine height from 'Z' coordinate and semi-minor axis of ellipsoid
		plh[2] = fabs(xyz[2]) - B_ELL;

		// return to calling function
		return;
	}


	//***************************************************************************
	//**                  Compute initial latitude estimate                    **
	//***************************************************************************

	lat_temp = atan2(xyz[2],p*(1.0-E2));


	//***************************************************************************
	//**                         Iterate to a solution                         **
	//***************************************************************************

	for(i=0;i<10;i++)
	{
		// compute prime vertical radius of curvature
		N = A_ELL / sqrt(1.0-E2*sin(lat_temp)*sin(lat_temp));

		// compute estimate for height
		hgt_temp = p/cos(lat_temp)-N;

		// compute new estimate for latitude
		lat_temp = atan2( xyz[2] , p*(1.0-E2*N/(N+hgt_temp)) );

		// check for convergence (changes in height AND latitude < 0.1mm)
		if( fabs(hgt_temp-hgt_old)<0.0001 && fabs(lat_temp-lat_old)<1.5e-11 )
		{
			// store final values
			plh[0] = lat_temp;
			plh[2] = hgt_temp;

			// return to calling function
			return;
		}

		// update previous estimates of latitude and height
		lat_old = lat_temp;
		hgt_old = hgt_temp;
	}		//*** end of for loop ***
}		//*** end of function ***




/*******************************************************************************
%XYZ2ENU	Convert from WGS-84 ECEF cartesian coordinates to 
%               rectangular local-level-tangent ('East'-'North'-Up)
%               coordinates.
%
%	enu = XYZ2ENU(xyz,orgxyz)	
%
%    INPUTS
%	xyz(1) = ECEF x-coordinate in meters
%	xyz(2) = ECEF y-coordinate in meters
%	xyz(3) = ECEF z-coordinate in meters
%
%	orgxyz(1) = ECEF x-coordinate of local origin in meters
%	orgxyz(2) = ECEF y-coordinate of local origin in meters
%	orgxyz(3) = ECEF z-coordinate of local origin in meters
%
%    OUTPUTS
%       enu:  Column vector
%		enu(1,1) = 'East'-coordinate relative to local origin (meters)
%		enu(2,1) = 'North'-coordinate relative to local origin (meters)
%		enu(3,1) = Up-coordinate relative to local origin (meters)

%	Reference: Alfred Leick, GPS Satellite Surveying, 2nd ed.,
%	           Wiley-Interscience, John Wiley & Sons, 
%	           New York, 1995.
%
%	M. & S. Braasch 10-96
%	Copyright (c) 1996 by GPSoft
%	All Rights Reserved.
*********************************************************************************/
void xyz2enu(double * xyz, double * orgxyz, double *enu)
{

	double difxyz[3];
	double orglla[3];
	double	phi,lam,sinphi,cosphi,sinlam,coslam;

	difxyz[0] = xyz[0] - orgxyz[0];
	difxyz[1] = xyz[1] - orgxyz[1];
	difxyz[2] = xyz[2] - orgxyz[2];

	xyz2lla(orgxyz, orglla);
	phi = orglla[0];
	lam = orglla[1];
	sinphi = sin(phi);
	cosphi = cos(phi);
	sinlam = sin(lam);
	coslam = cos(lam);

	enu[0]	= -sinlam*difxyz[0] + coslam*difxyz[1];
	enu[1]	= -sinphi*coslam*difxyz[0]  - sinphi*sinlam*difxyz[1] + cosphi*difxyz[2];
	enu[2]	= cosphi*coslam*difxyz[0] + cosphi*sinlam*difxyz[1] + sinphi*difxyz[2];
}


double	randn( void )
{
	double U1,U2, V1, V2, S, X;
	do
	{
		U1	=	rand();
		U1	/=	RAND_MAX;				/* U1=[0,1] */
		U2	=	rand();
		U2	/=	RAND_MAX;			/* U2=[0,1] */
		V1	=	2 * U1 -1;				/* V1=[-1,1] */
		V2	=	2 * U2 - 1 ;				/* V2=[-1,1] */
		S	=	V1 * V1 + V2 * V2;
	} while ( S >=1 );
	
	X	=	sqrt(-2 * log(S) / S) * V1;
//   Y=sqrt(-2 * log(S) / S) * V2
	
	return	X;
}