/** Converts a vector in WGS84 Earth Centered, Earth Fixed (ECEF) Cartesian * coordinates to the local North, East, Down (NED) frame of a reference point, * also given in WGS84 ECEF coordinates. * * Note, this function only \e rotates the ECEF vector into the NED frame of * the reference point, as would be appropriate for e.g. a velocity vector. To * determine the distance between the point and the reference point in the NED * frame of the reference point, see \ref wgsecef2ned_d. * * \see wgsecef2ned_d. * * \param ecef Cartesian coordinates of the point, passed as [X, Y, Z], * all in meters. * \param ref_ecef Cartesian coordinates of the reference point, passed as * [X, Y, Z], all in meters. * \param ned The North, East, Down vector is written into this array as * [N, E, D], all in meters. */ void wgsecef2ned(const double ecef[3], const double ref_ecef[3], double ned[3]) { double M[3][3]; ecef2ned_matrix(ref_ecef, M); matrix_multiply(3, 3, 1, (double *)M, ecef, ned); }
/** Converts a vector in local North, East, Down (NED) coordinates relative to * a reference point given in WGS84 Earth Centered, Earth Fixed (ECEF) Cartesian * coordinates to a vector in WGS84 ECEF coordinates. * * Note, this function only \e rotates the NED vector into the ECEF frame, as * would be appropriate for e.g. a velocity vector. To pass an NED position in * the reference frame of the NED, see \ref wgsned2ecef_d. * * \see wgsned2ecef_d. * * \param ned The North, East, Down vector is passed as [N, E, D], all in * meters. * \param ref_ecef Cartesian coordinates of the reference point, passed as * [X, Y, Z], all in meters. * \param ecef Cartesian coordinates of the point written into this array, * [X, Y, Z], all in meters. */ void wgsned2ecef(const double ned[3], const double ref_ecef[3], double ecef[3]) { double M[3][3], M_transpose[3][3]; ecef2ned_matrix(ref_ecef, M); matrix_transpose(3, 3, (double *)M, (double *)M_transpose); matrix_multiply(3, 3, 1, (double *)M_transpose, ned, ecef); }
static void compute_dops(const double H[4][4], const double pos_ecef[3], dops_t *dops) { /* PDOP is the norm of the position elements of tr(H) */ double pdop_sq = H[0][0] + H[1][1] + H[2][2]; dops->pdop = sqrt(pdop_sq); /* TDOP is like PDOP but for the time state. */ dops->tdop = sqrt(H[3][3]); /* Calculate the GDOP -- ||tr(H)|| = sqrt(PDOP^2 + TDOP^2) */ dops->gdop = sqrt(pdop_sq + H[3][3]); /* HDOP and VDOP are Horizontal and Vertical. We could rotate H * into NED frame and then take the separate components, but a more * computationally efficient approach is to find the vector in the * ECEF frame that represents the Down unit vector, and project it * through H. That gives us VDOP^2, then we find HDOP from the * relation PDOP^2 = HDOP^2 + VDOP^2. */ double M[3][3]; ecef2ned_matrix(pos_ecef, M); double down_ecef[4] = {M[2][0], M[2][1], M[2][2], 0}; double tmp[3]; matrix_multiply(3, 4, 1, (double *)H, down_ecef, tmp); double vdop_sq = vector_dot(3, down_ecef, tmp); dops->vdop = sqrt(vdop_sq); dops->hdop = sqrt(pdop_sq - vdop_sq); }