您现在的位置是：首页 >其他 >RTKLIB——求卫星高度角与方位角（geodist，satazel）网站首页其他

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

是王久久阿 2023-05-12 20:15:03
简介详细介绍了rtklib中求解卫星高度角的步骤，希望能给大家带来帮助。

本文代码选自RTKLIB_2.4.2版本，文中所有代码均在rtkcmn.c源文件中，宏定义在头文件中。

文章目录

原函数

大致原理：首先使用 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：原矩阵缩放因子

意义：

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	%lf
", azel[0] * rad2ang, azel[1] * rad2ang);
}
  • 上述代码中rr为KOS1测站三维坐标，rs为某天SP3精密星厉的数据，单位为km。
  • ecef2pos为xyz与blh的转换。
  • rad2ang为弧度到角度转换系数，方便打印。
风语者！平时喜欢研究各种技术，目前在从事后端开发工作，热爱生活、热爱工作。

站长推荐