RTKLIB——求卫星高度角与方位角（satazel）
本文代码选自RTKLIB_2.4.2版本，文中所有代码均在rtkcmn.c源文件中，宏定义在头文件中。
文章目录
-
- 原函数
-
- `宏定义`
- `工具`函数
- `geodist`函数
- `satazel`函数
- 示例
-
- 源代码试例
- 个人测试示例
原函数
大致原理：首先使用
ecef2enu函数将卫星位置从 ECEF 坐标系（地心惯性坐标系）转换为接收机位置的局部东北天 (ENU) 坐标系。如果接收机的高度pos[2]大于-RE_WGS84，即接收机高度大于地球半径，就说明接收机的位置有效，然后利用ENU计算卫星的方位角和高度角。如果接收机位置高度pos[2]小于-RE_WGS84，则该点位置无效，则返回默认值az=0和el=π/2。
宏定义
#define PI 3.141592653589793
#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */
#define CLIGHT 299792458.0 /* speed of light (m/s) */
#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */
上述定义分别对应：圆周率PI，WGS84椭球长半轴，WGS84椭球扁率，光速和地球自转角速度。
工具函数
dot函数函数
参数声明：
- 向量
a,向量b，维数n意义：
求向量
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;
}
norm函数
参数声明：
- 向量
a,维数n调用函数：
dot：求向量
a和向量b的点积，将结果存储到c中作为返回值返回，其中n为维数。意义：
求向量的欧几里得范数，将
dot返回的点积结果进行开方。
/* euclid norm -----------------------------------------------------------------
* euclid norm of vector
* args : double *a I vector a (n x 1)
* int n I size of vector a
* return : || a ||
*-----------------------------------------------------------------------------*/
extern double norm(const double *a, int n)
{
return sqrt(dot(a, a, n));
}
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];
}
}
}
geodist函数
参数声明：
- rs：卫星位置
xyz，**rr：**接收机位置xyz- e：用于存储结果：卫星与接收机的单位向量
调用函数：
- norm：求向量的欧几里得范数，将
dot返回的点积结果进行开方。意义：
先对测站高程位置进行判定，若小于WGS84椭球长半轴，该位置无效。
将卫星位置
rs与测站位置rr做差，结果存储到e中。求空间几何距离
r，并将坐标差e除以r得到单位向量。返回接收机到几何距离，其中
OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;为sagnac效应修正，简单含义如下：
Sagnac效应是指在一个旋转的参考系中，一个光束在顺时针方向和逆时针方向行进所需要的时间不同。在导航领域中，当卫星信号经过旋转的地球表面时，由于地球自转的影响，卫星信号在顺时针和逆时针方向行进所需要的时间不同，因此接收机的时钟和卫星的时钟之间会出现一个偏差，这就是Sagnac效应。
/* geometric distance ----------------------------------------------------------
* compute geometric distance and receiver-to-satellite unit vector
* args : double *rs I satellilte position (ecef at transmission) (m)
* double *rr I receiver position (ecef at reception) (m)
* double *e O line-of-sight vector (ecef)
* return : geometric distance (m) (0>:error/no satellite position)
* notes : distance includes sagnac effect correction
*-----------------------------------------------------------------------------*/
extern double geodist(const double *rs, const double *rr, double *e)
{
double r;
int i;
if (norm(rs,3)<RE_WGS84) return -1.0;
for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
r=norm(e,3);
for (i=0;i<3;i++) e[i]/=r;
return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;
}
satazel函数
参数声明：
pos：测站的BLH，角度以弧度（rad）表示，高度以米表示
e：测站到卫星的单位向量（该向量e由
geodist函数得出，后文给出）azel：存储结算结果方位角与高度角，方位角范围是0～360度，高度角为-90～90度（高度角的定义范围是0～90度）
调用函数：
ecef2enu：将测站
pos的经纬度为旋转参数，将单位向量e转换为以测站为原点的enu坐标系，结果存储到enu中。dot：求向量
a和b的点积，n为维数。意义：
- 利用
ENU坐标求卫星的方位角和高度角。
/* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
* return : elevation angle (rad)
*-----------------------------------------------------------------------------*/
extern double satazel(const double *pos, const double *e, double *azel)
{
double az=0.0,el=PI/2.0,enu[3];
if (pos[2]>-RE_WGS84) {
ecef2enu(pos,e,enu);
az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);
if (az<0.0) az+=2*PI;
el=asin(enu[2]);
}
if (azel) {azel[0]=az; azel[1]=el;}
return el;
}
坐标系转换在其他篇已做介绍，这里不再重复介绍
详细内容请移步：RTKLIB——坐标系相互转换（ecef2pos，pos2ecef，ecef2enu，enu2ecef）
ecef2enu函数
/* 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];
xyz2enuq(pos, E);
matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
}
示例
源代码试例
1）
if (geodist(rs,rr,e)>0.0)
{
satazel(pos,e,azel);
az[i][j]=azel[0];
el[i][j]=azel[1];
}
- 上述代码首先对
geodist的返回值进行判定，判断改正后的几何距离是否有效，然后才进行高度角和方位角的计算。
2）
if (satazel(pos, e, azel) <= 0.0)
continue;
- 上述代码是对
satazel的返回的高度角进行判断，如果高度角小于0，则跳过本循环。
3）
if ((r=geodist(rs+i*6,rr,e))<=0.0||satazel(pos,e,azel+i*2)<opt->elmin)
continue;
- 上述代码同时对
几何距离和高度角进行了判断 - 其中，
opt->elmin表示卫星的截止高度角，单位为弧度。
个人测试示例
#define PI 3.141592653589793
#define rad2ang 180/PI
#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */
#define CLIGHT 299792458.0 /* speed of light (m/s) */
#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */
void test()
{
double e[3] = {0},pos[3] = {0},azel[3] = {0};
double rr[3] = {3899619.173000, 397366.871000, 5014736.979000};
double rs[3] = {-8701.958813, -15935.019504, -19494.837620};
rs[0] *= 1.0E3;
rs[1] *= 1.0E3;
rs[2] *= 1.0E3;
ecef2pos(rr, pos);
geodist(rs, rr, e);
satazel(pos, e, azel);
printf("%lf\t%lf\n", azel[0] * rad2ang, azel[1] * rad2ang);
}
- 上述代码中
rr为KOS1测站三维坐标，rs为某天SP3精密星厉的数据，单位为km。 ecef2pos为xyz与blh的转换。rad2ang为弧度到角度转换系数，方便打印。