In this post, we will discuss the coordinate systems commonly used in autonomous driving. In practice, different positioning providers may define their own coordinate systems. This post will introduce the fundamental concepts of these coordinate systems and explain how to convert between them for your specific needs.

Pose on the Earth

When we talk about pose, we refer to the position and orientation of an object in the world. In the context of autonomous driving, the world is the Earth. Therefore, pose describes the position and orientation of an object relative to the Earth.

Global Coordinate System

There are two types of coordinate systems: global coordinate systems and local coordinate systems. Global systems use the Earth as their reference, while local systems use a specific reference point.

Geodetic System

Geodetic coordinates are a type of curvilinear orthogonal coordinate system used in geodesy based on a reference ellipsoid. They include geodetic latitude (north/south) ϕ, longitude (east/west) λ, and ellipsoidal height h (also known as geodetic height[1]).

To specify geodetic coordinates, we need to define the origin of latitude and longitude, as well as the center of the ellipsoid. Different definitions lead to different coordinate systems. Several geographic coordinate systems are used in industry, such as WGS84, European ED50, and the British OSGB36. WGS84 is the most widely used geodetic system globally.

ECEF (Earth-Centered, Earth-Fixed)

"The Earth-centered, Earth-fixed coordinate system (acronym ECEF), also known as the geocentric coordinate system, is a cartesian spatial reference system that represents locations in the vicinity of the Earth (including its surface, interior, atmosphere, and surrounding outer space) as X, Y, and Z measurements from its center of mass" [2].

Local Coordinate System

ENU (East-North-Up) and NED (North-East-Down)

“Local tangent plane coordinates (LTP) are part of a spatial reference system based on the tangent plane defined by the local vertical direction and the Earth’s axis of rotation. They are also known as local ellipsoidal system,local geodetic coordinate system, local vertical, local horizontal coordinates (LVLH), or topocentric coordinates. It consists of three coordinates: one represents the position along the northern axis, one along the local eastern axis, and one represents the vertical position. Two right-handed variants exist: east, north, up (ENU) coordinates and north, east, down (NED) coordinates.” [3].

From Geodetic to ECEF Coordinates

Geodetic coordinates (latitude $\phi$, longitude $\lambda$, height $h$) can be converted into ECEF coordinates using the following equations [5]:

$$ X = (N(\phi) + h) \cos \phi \cos \lambda $$$$ Y = (N(\phi) + h) \cos \phi \sin \lambda $$$$ Z = \left( \frac{b^2}{a^2} N(\phi) + h \right) \sin \phi $$

which can also be written as:

$$ Z = \left( (1 - e^2) N(\phi) + h \right) \sin \phi $$

or:

$$ Z = \left( (1 - f)^2 N(\phi) + h \right) \sin \phi $$

where:

$$ N(\phi) = \frac{a^2}{\sqrt{a^2 \cos^2 \phi + b^2 \sin^2 \phi}} = \frac{a}{\sqrt{1 - e^2 \sin^2 \phi}} = \frac{a}{\sqrt{1 - e^2 \cdot \frac{1}{1 + \cot^2 \phi}}} $$

and $a$ and $b$ are the equatorial radius (semi-major axis) and the polar radius (semi-minor axis), respectively. The parameter $e^2 = 1 - \frac{b^2}{a^2}$ is the square of the first numerical eccentricity of the ellipsoid. The flattening of the ellipsoid is given by $f = 1 - \frac{b}{a}$.

The prime vertical radius of curvature $N(\phi)$ represents the distance from the surface to the Z-axis along the ellipsoidal normal.

From ECEF to NED

NED coordinates are similar to ECEF in that they’re Cartesian. However, they can be more convenient due to the relatively small numbers involved, and also because of the intuitive axes. NED and ECEF coordinates can be related with the following formula [4]:

$$ \mathbf{p}_{\text{NED}} = \mathbf{R} \left( \mathbf{p}_{\text{ECEF}} - \mathbf{p}_{\text{Ref}} \right) $$

where:

• $\mathbf{p}_{\text{NED}}$ is a 3D position in the NED system.
• $\mathbf{p}_{\text{ECEF}}$ is the corresponding ECEF position.
• $\mathbf{p}_{\text{Ref}}$ is the reference ECEF position (where the local tangent plane originates).
• $\mathbf{R}$ is a rotation matrix whose rows represent the north, east, and down axes.

The rotation matrix $R$ can be conveniently defined using latitude $\phi$ and longitude $\lambda$ corresponding to $\mathbf{p}_{\text{Ref}}$:

$$ \mathbf{R} = \begin{bmatrix} -\sin(\phi) \cos(\lambda) & -\sin(\phi) \sin(\lambda) & \cos(\phi) \\ -\sin(\lambda) & \cos(\lambda) & 0 \\ -\cos(\phi) \cos(\lambda) & -\cos(\phi) \sin(\lambda) & -\sin(\phi) \end{bmatrix} $$

From ECEF to ENU [5]

To transform from ECEF coordinates to the local coordinates, we need a local reference point.

$$ \mathbf{p}_{ENU} = \mathbf{R} \left( \mathbf{p}_{ECEF} - \mathbf{p}_{Ref} \right) $$$$ \mathbf{R} = \begin{bmatrix} -\sin(\lambda) & \cos(\lambda) & 0 \\ -\sin(\phi) \cos(\lambda) & -\sin(\phi) \sin(\lambda) & \cos(\phi) \\ \cos(\phi) \cos(\lambda) & \cos(\phi) \sin(\lambda) & \sin(\phi) \end{bmatrix}$$

Orientation of the Vehicle

The orientation of a vehicle can be defined in several ways. This can be complex, as different positioning device providers often use their own definitions.

Vehicle Coordinate System

Common vehicle coordinate system includes: right x, forward y, and up z; forward x, left y, and up z; forward x, right y, and down z.

Euler Angle Representation

In this section, we use right hand coordinate system by default.

Roll, Pitch, Yaw

Roll, pitch, yaw are rotations around x, y, z axes, respectively.

Intrinsic and Extrinsic Rotation

Please refer to Extrinsic & intrinsic rotation: Do I multiply from right or left? for a better understanding of the difference between intrinsic and extrinsic rotation, and their relationship with right and left multiplication.

Steps to Get the Correct Orientation of the Vehicle

1. Identify the global coordinate system of the vehicle. This is usually the tangent plane of the Earth (ENU or NED).
2. Align the vehicle coordinate system with the global coordinate system.
3. Carefully examine how positioning device providers describe Euler angles. They may not use rigorous definitions as expected. For example, they might define yaw as the angle between the vehicle’s heading and north no matter how they define the vehicle coordinate system. Pay special attention to the sign the angles.
4. Construct the rotation matrix from the Euler angles. Intrinsic rotation is commonly used as it is more intuitive.

References

[1] https://en.wikipedia.org/wiki/Geodetic_coordinates

[2] https://en.wikipedia.org/wiki/Earth-centered,_Earth-fixed_coordinate_system

[3] https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates

[4] https://www.seas.harvard.edu/news/2014/02/03/ecef-ned-coordinates

[5] https://en.wikipedia.org/wiki/Geographic_coordinate_conversion