How to convert from ECEF to ENU (local frame)

To convert the odometry output of the Vision-RTK2 from the Earth-Centered, Earth-Fixed (ECEF) geocentric-Cartesian coordinate system to the East-North-Up (ENU) local tangent plane, perform the following steps:

1. Determine the reference point’s ellipsoidal (LLH) coordinatesin the ECEF system. This point will be the origin of the ENU system. Let's denote the latitude as , longitude as , and altitude as. The following transformation function can be used:

with

,

where

• is the semi-major axis of the reference ellipsoid.

• is the semi-minor axis of the reference ellipsoid.

• is the first numerical eccentricity of the ellipsoid and.

For example, when using the WGS84 reference geometric datum, the following ellipsoid values are used: 6378137 m, 0.0818191908426. Note that the quantities , , are dependent on each other. Therefore, they have to be calculated by iteration starting with an approximate value :

This transformation can be visually represented as:

For more information on this transformation, see the following guide:

coordinates_swisstopo_formulas-and-constants_refsys_e.pdf

2. Given the current position in ellipsoidal coordinates, compute the rotation matrix between the ECEF and local ENU frames (source https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates):

,

whereis longitudeand is latitude . Longitude and latitude can be either taken from the FP_A-LLH message or converted from the ECEF position coordinates from the FP_A-ODOMETRY message.

The Vision-RTK2 LLH output assumes a WGS84 geodetic datum.

3. Convert a position from ECEF to ENU, it is necessary to track the origin of the ENU systemin either ellipsoidal coordinates (LLH) or geocentric-Cartesian coordinates (ECEF). Then, subtract the vector to the ENU origin in ECEF coordinates to get the position of the body relative to the ENU origin, expressed in the ECEF frame. Lastly, using the rotation matrix from ECEF to ENU0, we get position of body relative to ENU0:

Note that Fixposition provides a C++ and Eigen based coordinate transformation library in https://github.com/fixposition/fixposition_gnss_tf. All functions required to convert to and from ECEF, LLH and ENU coordinate systems are provided here.

4. Convert a vector (e.g., velocity) from ECEF to ENU:

Note that some systems calculate the velocity in a local ENU frame while others compute this vector with respect to a fixed ENU frame (i.e., sometimes called ENU0). Choose this transformation depending on your application. We recommend employing a fixed ENU frame.

5. Convert an orientation (e.g., rotation matrix from body to ECEF) from ECEF to ENU:

,

where can be obtained by converting the orientation quaternion provided in the FP_A-ODOMETRY message into a rotation matrix. To achieve this, the following known relationship can be employed:

,

where and is the real component of the unit quaternion. For practical applications, if using C++, we recommend using the Eigen library for this operation (https://eigen.tuxfamily.org/index.php?title=Main_Page). If using Python, we recommend the Numpy-quaternion library (https://quaternion.readthedocs.io/en/latest/).

As with the velocity vector, we recommend employing a fixed ENU frame (ENU0) for this operation.

For more information on these transformations, please refer to:

• <https://www.swisstopo.admin.ch/en/maps-data-online/calculation-services/m2m.html​>

• <https://www.use-snip.com/kb/knowledge-base/converting-ecef-to-llh-and-back/>

• <https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates>