本章首先介绍空间直角坐标系与大地坐标系，然后列出XYZ转换BLH的公式，最后基于C语言完成该部分代码设计。

参考书籍：

董大男，陈俊平，王解先等，GNSS高精度定位原理，科学出版社

黄丁发，熊永良，周乐韬等，GPS卫星导航定位技术与方法，科学出版社。

公式原理 

空间直角坐标系

空间直角坐标系原点位于参考椭球的中心，Z轴指向参考椭球的北极，X轴指向首子午面与赤道的交点，Y轴位于赤道面上，且按右手坐标系与X轴呈90°夹角。某点在空间中坐标可用该店在次坐标系的各个坐标轴上的投影来表示。（如下图所示）

大地坐标系

空间大地坐标系是采用大地经纬度和大地高来描述空间位置的（下图）。维度是指P点的法线与赤道面的夹角，用B（-90°~ 90°）表示，向北为正称为北纬，向男为负称为南纬。经度是指P点的参考椭球子午面与起始子午面的二面角，用L表示（-180°~180°），由起始子午面起算，向东为正称为东京，向西为负称为西经。大地高是空间点沿该法线到椭球面的距离，用H表示，向上为正，向下为负。

XYZ转BLH

空间直角坐标系（X,Y,Z）与大地坐标系 （B,L,H）关系如下：

式中：N为某点P的卯酉圈半径；（B,L,H）为P点的大地坐标系，卯酉圈半径N公式如下：

式中：a为椭球长半轴，b为椭球短半轴，为第一离心率，B为大地维度。不难看出，大地维度是关于自身的函数，需要迭代求解。计算方式如下：

初始迭代时，设：

 于是：

当ΔH＜0.001m，B的经度保证在0.001秒即可停止迭代。

关于上述代码的详细推导过程，请参考charlee44博主的大地经纬度坐标与地心地固坐标的的转换

 

程序设计

#define a  6378137.0//长半轴
#define f  (1 / 298.257222101)//扁率
#define b  (a - a * f)//短半轴
#define e2  (f*(2-f))//第一偏心率平方

//经纬度转换(弧度)
BLH XYZtoLB(double X, double Y, double Z)
{
	BLH res = { 0 };
	double B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
	R0 = sqrt(pow(X, 2) + pow(Y, 2));
	R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
	//经度直接求解
	res.L = atan2(Y, X);
	//迭代求大地维度和大地高
	N = a;
	H = R1 - sqrt(a * b);
	B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
	do
	{
		deltaH = N;//判断收敛所用
		deltaB = B;
		N = a / sqrt(1 - e2 * pow(sin(B), 2));
		H = R0 / cos(B) - N;
		B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
	} while (fabs(deltaH - H) > 0.001 && fabs(deltaB - B) > 1.0e-9);
	res.B = B;
	res.H = H;
	return res;
}

其中，BLH为用于传参的结构体，代码如下：

//经纬度函数传参
typedef struct BLH
{
	double B;//维度
	double L;//经度
	double H;//高
}BLH;

代码部分与上述公式部分对应，代码并不复杂按公式来即可，求出测站的经纬度后，即可求出卫星的高度角、方位角，从而进行定权、误差改正等内容。

卫星高度角、方位角求解可参考我的博客：求解卫星高度角、方位角