网站导航
> 文章列表 > RTKLIB——求卫星高度角与方位角（geodist，satazel）

RTKLIB——求卫星高度角与方位角（geodist，satazel）

网友: 文章列表 2024-03-21 17:14:04

RTKLIB——求卫星高度角与方位角（geodist，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的点积，将结果存储到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;elseC[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为弧度到角度转换系数，方便打印。
网络标签：

相关问题