llh2xyz.m

惯性导航系统传递对准系统模型程序

function xyz = llh2xyz(llh)

	phi = llh(1);
	lambda = llh(2);
	h = llh(3);

	a = 6378137.0000;	% earth semimajor axis in meters
	b = 6356752.3142;	% earth semiminor axis in meters	
	e = sqrt (1-(b/a).^2);

	sinphi = sin(phi);
	cosphi = cos(phi);
	coslam = cos(lambda);
	sinlam = sin(lambda);
	tan2phi = (tan(phi))^2;
	tmp = 1 - e*e;
	tmpden = sqrt( 1 + tmp*tan2phi );

	x = (a*coslam)/tmpden + h*coslam*cosphi;

	y = (a*sinlam)/tmpden + h*sinlam*cosphi;

	tmp2 = sqrt(1 - e*e*sinphi*sinphi);
	z = (a*tmp*sinphi)/tmp2 + h*sinphi;

	xyz(1) = x;
	xyz(2) = y;
	xyz(3) = z;