RTKLIB——find satellite altitude and azimuth (geodist, satazel)

The code in this article is selected from the RTKLIB_2.4.2 version. All the codes in this article are in the rtkcmn.c source file, and the macros are defined in the header file.

Article Directory

Primitive

Rough principle : First use ecef2enuthe function to convert the satellite position from the ECEF coordinate system (Earth-centered inertial coordinate system) to the local Northeast-Universal (ENU) coordinate system of the receiver position. If the height of the receiver pos[2]is greater than -RE_WGS84, that is, the height of the receiver is greater than the radius of the earth, it means that the position of the receiver is valid, and then use it to ENUcalculate the azimuth and altitude of the satellite. If the receiver position height pos[2]is less than -RE_WGS84, then the point position is invalid, and the default value az=0and 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) */

The above definitions correspond to: PI, WGS84 ellipsoid major axis, WGS84 ellipsoid oblateness, light velocity and earth rotation angular velocity.

`工具`function

dot函数function

Parameter declaration:

vector a, vector b, dimensionn

significance:

Takes the dot product of vectors aand vectors , and returns the result stored in , where is the number of dimensions.bcn

/* 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;
}

normfunction

Parameter declaration:

vector a, dimensionn

Call functions:

dot : Calculate the dot product of a vector aand a vector b, and store the result in cas the return value, where nis the number of dimensions.

significance:

Find the Euclidean norm of the vector, and take dotthe square root of the returned dot product result.

/* 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));
}

matmulfunction:

Parameter declaration:

tr : transpose flag, n : rows of left matrix, k : columns of left matrix or rows of right matrix, m : columns of right matrix

A : left matrix, B : right matrix, C : original/result matrix

alpha : multiplication result scaling factor, beta : original matrix scaling factor

significance:

Finds the product of two matrices and scales the result. For a more detailed introduction, please move to: RTKLIB——matmul (matrix multiplication function)

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`function

Parameter declaration:

rs : satellite position xyz, **rr: **receiver positionxyz

e : used to store the result: unit vector of satellite and receiver

Call functions:

norm : Find the Euclidean norm of the vector, and take dotthe square root of the returned dot product result.

significance:

First judge the elevation position of the station, if it is less than the semi-major axis of the WGS84 ellipsoid, the position is invalid.

Do the difference between the satellite position rsand the station position , and store the result in .rre

Find the spatial geometric distance rand divide the coordinate difference eby rto get the unit vector.

Returns the receiver to the geometric distance, where OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;is sagnacthe effect correction, the simple meaning is as follows:

SagnacThe effect is the difference in the time it takes for a light beam to travel clockwise and counterclockwise in a rotating frame of reference. In the field of navigation, when the satellite signal passes through the rotating earth's surface, due to the influence of the earth's rotation, the time required for the satellite signal to travel clockwise and counterclockwise is different, so there will be a gap between the clock of the receiver and the clock of the satellite. One deviation, that's Sagnacthe effect.

/* 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`function

Parameter declaration:

pos : BLH of the station, angle in radians (rad) and height in meters

e : unit vector from the station to the satellite (the vector e is obtained by geodistthe function and will be given later)

azel : store the settlement result azimuth and altitude angle, the azimuth angle ranges from 0 to 360 degrees, and the altitude angle ranges from -90 to 90 degrees (the definition range of the altitude angle is 0 to 90 degrees)

call function :

ecef2enu : Use the latitude and longitude of the station posas the rotation parameter, convert the unit vector einto the enu coordinate system with the station as the origin, and store the result in enu.

dot : Find the dot product of the sum of vectors a, bas nthe number of dimensions.

significance:

Use ENUthe coordinates to find the azimuth and altitude of the satellite.

/* 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;
}

Coordinate system conversion has been introduced in other articles, so I won’t repeat it here

For details, please go to: RTKLIB——coordinate system conversion (ecef2pos, pos2ecef, ecef2enu, enu2ecef)

ecef2enufunction

/* 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);
}

example

source code example

1）

if (geodist(rs,rr,e)>0.0) 
{
    
    
        satazel(pos,e,azel);
        az[i][j]=azel[0];
        el[i][j]=azel[1];
}

The above code first geodistjudges the return value of , and judges whether the corrected geometric distance is valid, and then calculates the altitude angle and azimuth angle.

2）

if (satazel(pos, e, azel) <= 0.0)
           	continue;

The above code is satazelto judge the returned altitude angle, if the altitude angle is less than 0, skip this cycle.

3）

 if ((r=geodist(rs+i*6,rr,e))<=0.0||satazel(pos,e,azel+i*2)<opt->elmin) 
     		continue;

The above code simultaneously judges 几何距离and高度角
Among them, opt->elminrepresents the cut-off altitude angle of the satellite, and the unit is radian.

Personal Test Example

#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);
}

The above code rris KOS1the three-dimensional coordinates of the station, rswhich is the data of SP3 Precision Star on a certain day, and the unit is km.
ecef2posIt is the conversion between xyz and blh.
rad2angIt is the radian to angle conversion factor, which is convenient for printing.