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