本文代码选自RTKLIB_2.4.2版本，文中所有代码均在rtkcmn.c源文件中，宏定义在头文件中。
宏定义与工具函数
宏定义
#define PI 3.141592653589793
#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */
上述定义分别对应：圆周率PI，WGS84椭球长半轴，WGS84椭球扁率。
工具函数
dot函数函数
参数声明：
- 向量
a,向量b，维数n意义：文章来源地址https://www.toymoban.com/news/detail-645784.html
- 求向量
a和向量b的点积，将结果存储到c中作为返回值返回，其中n为维数。
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args : double *a,*b I vector a,b (n x 1)
* int n I size of vector a,b
* return : a'*b
*-----------------------------------------------------------------------------*/
extern double dot(const double *a, const double *b, int n)
{
double c=0.0;
while (--n>=0) c+=a[n]*b[n];
return c;
}
matmul矩阵乘法函数
参数声明：
- tr：转置标志，n：左矩阵行，k：左矩阵列或右矩阵的行，m：右矩阵的列
- A：左矩阵，B：右矩阵，C：原矩阵/结果矩阵
- alpha：乘法结果缩放因子，beta：原矩阵缩放因子
意义：
- 求得两矩阵的积，并且对其结果进行缩放。更详细的介绍请移步:RTKLIB——matmul(矩阵乘法函数)
extern void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C)
{
double d;
int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);
for (i = 0; i < n; i++)
{
for (j = 0; j < k; j++)
{
d = 0.0;
switch (f)
{
case 1:
for (x = 0; x < m; x++)
{
d += A[i + x * n] * B[x + j * m];
}
break;
case 2:
for (x = 0; x < m; x++)
{
d += A[i + x * n] * B[j + x * k];
}
break;
case 3:
for (x = 0; x < m; x++)
{
d += A[x + i * m] * B[x + j * m];
}
break;
case 4:
for (x = 0; x < m; x++)
{
d += A[x + i * m] * B[j + x * k];
}
break;
}
if (beta == 0.0)
C[i + j * n] = alpha * d;
else
C[i + j * n] = alpha * d + beta * C[i + j * n];
}
}
}
地固坐标系（xyz）→ 地理坐标系（BLH）
基础公式原理
经度直接求解：
t
a
n
L
=
y
x
tanL = \frac{y}{x}
tanL=xy
在代码计算中，RTKLIB使用了atan2函数，将数值范围归化到-90度～90之间。因为分母不能为0，因此在计算前还需要判断坐标的合理性，于是上述公式转换成如下形式：
L
=
{
{
arctan
y
x
x
>
0
arctan
y
x
+
π
x
<
0
,
y
≥
0
arctan
y
x
−
π
x
<
0
,
y
<
0
π
2
x
=
0
,
y
>
0
−
π
2
x
=
0
,
y
<
0
x
≠
0
,
y
≠
0
0
x
=
0
,
y
=
0
L = \begin{cases} \begin{cases} \arctan\frac{y}{x} & x > 0 \\ \arctan\frac{y}{x}+\pi & x < 0 , y \ge 0 \\ \arctan\frac{y}{x}-\pi & x < 0 , y < 0 \\ \frac{\pi}{2} & x = 0 , y > 0 \\ -\frac{\pi}{2} & x = 0 , y < 0 \\ \end{cases} & x≠0,y≠0\\ 0 & x = 0 , y = 0 \\ \end{cases} \\
L=⎩
⎨
⎧⎩
⎨
⎧arctanxyarctanxy+πarctanxy−π2π−2πx>0x<0,y≥0x<0,y<0x=0,y>0x=0,y<00x=0,y=0x=0,y=0
使用atan2计算时，该函数会自动根据x,y的正负进行判断，因此写程序时只需要对x=0,y=0的情况进行额外判断即可。
纬度与大地高迭代求解：
r
=
x
2
+
y
2
s
i
n
B
=
z
r
2
+
z
2
v
=
a
1
−
e
2
sin
2
B
z
=
r
s
i
n
B
+
e
2
v
s
i
n
B
r=x^2+y^2 \\ sinB = \frac{z}{\sqrt{r^2+z^2}} \\ v = \frac{a}{\sqrt{1-e^2\sin^2B}} \\ z = rsinB + e^2vsinB \\
r=x2+y2sinB=r2+z2zv=1−e2sin2Baz=rsinB+e2vsinB
其中，
e
e
e表示WGS84椭球体的偏心率；
v
v
v表示地球表面上的纬度对应的曲率半径；
B
B
B表示计算得到的大地纬度，
h
h
h表示计算得到的大地高。不难看出，大地维度是关于自身的函数，求解大地高需要
v
v
v，而
v
v
v随着维度而变化。因此需要进行迭代计算，在计算初，需要初始化：
v
=
a
z
=
y
v=a \\ z=y
v=az=y
于是，当
z
z
z收敛后，可以对大地维度进行求解。
B
=
arctan
z
r
2
h
=
r
2
+
z
2
−
v
B = \arctan\frac{z}{\sqrt{r^2}} \\ h = \sqrt{r^2+z^2}-v
B=arctanr2zh=r2+z2−v
最终使用atan2求解大地维度，比asin求解更加稳定，并且从运算角度上，求解atan2中运算的过程比asin要少一步。代码中还将超限的值进行了归化，避免出现NAN无法计算，于是大地维度最终为：
B
=
{
arctan
z
r
2
r
2
≥
1
0
−
12
{
π
2
z
>
0
−
π
2
z
<
0
r
2
<
1
0
−
12
B = \begin{cases} \arctan\frac{z}{\sqrt{r^2}} & r^2 \ge 10^{-12} \\ \begin{cases} \frac{\pi}{2} & z > 0 \\ -\frac{\pi}{2} & z < 0 \end{cases}&r^2<10^-12\\ \end{cases}\\
B=⎩
⎨
⎧arctanr2z{2π−2πz>0z<0r2≥10−12r2<10−12
ecef2pos代码
参数声明：
- r：测站的
xyz- pos：用于存储计算得到的
BLH调用函数：文章来源：https://www.toymoban.com/news/detail-645784.html
- dot：求向量
a和向量b的点积，将结果存储到c中作为返回值返回，其中n为维数。
void ecef2posq(const double *r, double *pos)
{
double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp;
for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;)
{
zk = z;
sinp = z / sqrt(r2 + z * z);
v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
z = r[2] + v * e2 * sinp;
}
pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0;
pos[2] = sqrt(r2 + z * z) - v;
}
上述代码中，a?b:c表示三目操作符，如果a为真，则执行b，反之执行c
地理坐标系（BLH）→ 地固坐标系（xyz）
基础原理
通过BLH反求XYZ不需要迭代，直接带入公式求解即可：
x
=
(
v
+
h
)
c
o
s
B
c
o
s
L
y
=
(
v
+
h
)
c
o
s
B
s
i
n
L
z
=
(
v
⋅
(
1
−
e
2
)
+
h
)
s
i
n
B
x=(v+h)cosBcosL \\ y=(v+h)cosBsinL \\ z=(v⋅(1−e^2)+h)sinB
x=(v+h)cosBcosLy=(v+h)cosBsinLz=(v⋅(1−e2)+h)sinB
其中：
v
=
a
1
−
e
2
∗
s
i
n
2
B
v =\frac{a}{\sqrt{1 - e^2 * sin^2B}} \\
v=1−e2∗sin2Ba
pos2ecef函数
参数声明：
- pos：测站的BLH
- r：用于存储计算得到的xyz
/* transform geodetic to ecef position -----------------------------------------
* transform geodetic position to ecef position
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *r O ecef position {x,y,z} (m)
* return : none
* notes : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/
extern void pos2ecef(const double *pos, double *r)
{
double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
r[0] = (v + pos[2]) * cosp * cosl;
r[1] = (v + pos[2]) * cosp * sinl;
r[2] = (v * (1.0 - e2) + pos[2]) * sinp;
}
地固坐标系（xyz）→ 站心坐标系（ENU）
基础原理
[ E N U ] = [ − s i n L c o s L 0 − s i n B c o s L − s i n B s i n L c o s B c o s B c o s L c o s B s i n L s i n B ] [ X s − X r Y s − Y r Z s − Z r ] \begin{bmatrix} E \\ N \\ U \end{bmatrix} = \begin{bmatrix} -sinL & cosL & 0\\-sinB cosL & -sinB sinL & cosB\\cosB cosL & cosB sinL & sinB \end{bmatrix} \begin{bmatrix} Xs-Xr \\ Ys-Yr \\ Zs-Zr \end{bmatrix} ENU = −sinL−sinBcosLcosBcosLcosL−sinBsinLcosBsinL0cosBsinB Xs−XrYs−YrZs−Zr
其中，L表示参考点对大地经度，B表示参考点的大地维度。
- 当我们要求一个测站的ENU时，参考点应该为其本身，因此求出来的E应该为0。
**这是因为：**站心坐标系通常以站点所在地的经线为站心坐标系的东向轴，以经线的垂线方向（即纬线）为站心坐标系系的北向轴，而天向轴则垂直于本地坐标系的东北平面向上。因此，东向分量与站点所在的经线重合，因此为0。
- 而当我们需要求卫星的高度角时，那么参考点应该为测站，xyz则为卫星地固坐标系下的三维向量，这样就可以求出卫星以测站为原点的站心坐标，进而求出其方位角、仰角。
ecef2enu代码
参数声明：
- pos：参考点经纬度坐标（弧度）
- r：待求点与参考点三维坐标差
- e：用于存储计算得到的ENU
调用函数：
- xyz2enu：用于求旋转矩阵
- matmul：用于进行矩阵运算
意义：
- 给定参考点经纬度和待求点与参考点的坐标差，返回待求点ENU
/* ecef to local coordinate transfromation matrix ------------------------------
* compute ecef to local coordinate transfromation matrix
* args : double *pos I geodetic position {lat,lon} (rad)
* double *E O ecef to local coord transformation matrix (3x3)
* return : none
* notes : matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern void xyz2enu(const double *pos, double *E)
{
double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
E[0] = -sinl; E[3] = cosl; E[6] = 0.0;
E[1] = -sinp * cosl; E[4] = -sinp * sinl; E[7] = cosp;
E[2] = cosp * cosl; E[5] = cosp * sinl; E[8] = sinp;
}
/* transform ecef vector to local tangental coordinate -------------------------
* transform ecef vector to local tangental coordinate
* args : double *pos I geodetic position {lat,lon} (rad)
* double *r I vector in ecef coordinate {x,y,z} s-r
* double *e O vector in local tangental coordinate {e,n,u}
* return : none
*-----------------------------------------------------------------------------*/
extern void ecef2enu(const double *pos, const double *r, double *e)
{
double E[9];
xyz2enu(pos, E);
matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
}
站心坐标系（ENU）→ 地固坐标系（xyz）
基础原理：
RTKLIB中xyz与enu相互转化通过矩阵转置即可完成，不过需要注意：求得的结果是待求点与参考点的坐标差，还需要将结果加上参考点的xyz，才是待求点的xyz坐标。
[ X s − X r Y s − Y r Z s − Z r ] = [ − s i n L c o s L 0 − s i n B c o s L − s i n B s i n L c o s B c o s B c o s L c o s B s i n L s i n B ] T [ E N U ] \begin{bmatrix} Xs-Xr \\ Ys-Yr \\ Zs-Zr \end{bmatrix} = \begin{bmatrix} -sinL & cosL & 0\\-sinB cosL & -sinB sinL & cosB\\cosB cosL & cosB sinL & sinB \end{bmatrix}^T \begin{bmatrix} E \\ N \\ U \end{bmatrix} Xs−XrYs−YrZs−Zr = −sinL−sinBcosLcosBcosLcosL−sinBsinLcosBsinL0cosBsinB T ENU
enu2ecef函数
参数声明：
- pos：参考点经纬度坐标（弧度）
- e：待求点的
ENU- r：用于存储计算得到的
xyz坐标差调用函数：
- xyz2enu：用于求旋转矩阵
- matmul：用于进行矩阵运算(TN表示旋转矩阵E需要转置，后者不需要转置)
意义：
- 给定参考点经纬度和待求点与参考点的坐标差，返回待求点ENU
/* transform local vector to ecef coordinate -----------------------------------
* transform local tangental coordinate vector to ecef
* args : double *pos I geodetic position {lat,lon} (rad)
* double *e I vector in local tangental coordinate {e,n,u}
* double *r O vector in ecef coordinate {x,y,z}
* return : none
*-----------------------------------------------------------------------------*/
extern void enu2xzy(const double *pos, const double *e, double *r)
{
double E[9];
xyz2enu(pos,E);
matmul("TN",3,1,3,1.0,E,e,0.0,r);
}
到了这里，关于RTKLIB——坐标系相互转换（ecef2pos，pos2ecef，ecef2enu，enu2ecef）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
