RTKLIB——求卫星高度角与方位角(satazel)

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

文章目录

    • 原函数
      • `宏定义`
      • `工具`函数
      • `geodist`函数
      • `satazel`函数
    • 示例
      • 源代码试例
      • 个人测试示例

原函数

大致原理:首先使用 ecef2enu 函数将卫星位置从 ECEF 坐标系(地心惯性坐标系)转换为接收机位置的局部东北天 (ENU) 坐标系。如果接收机的高度 pos[2] 大于 -RE_WGS84,即接收机高度大于地球半径,就说明接收机的位置有效,然后利用ENU计算卫星的方位角和高度角。如果接收机位置高度 pos[2] 小于-RE_WGS84,则该点位置无效,则返回默认值 az=0el=π/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:测站到卫星的单位向量(该向量egeodist函数得出,后文给出)

  • azel:存储结算结果方位角与高度角,方位角范围是0~360度,高度角为-90~90度(高度角的定义范围是0~90度)

调用函数

  • ecef2enu:将测站pos的经纬度为旋转参数,将单位向量e转换为以测站为原点的enu坐标系,结果存储到enu 中。

  • dot:求向量ab的点积,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);
}
  • 上述代码中rrKOS1测站三维坐标,rs为某天SP3精密星厉的数据,单位为km。
  • ecef2pos为xyz与blh的转换。
  • rad2ang为弧度到角度转换系数,方便打印。

你可能感兴趣的:(RTKLIB函数,c语言,导航,rtklib,坐标转换,GNSS)