原函数
宏定义
#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函数函数
/* 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函数
/* 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函数：
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函数
/* 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函数
/* 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;
}
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为弧度到角度转换系数，方便打印。