Coordinate Systems

This is Lecture 3 from my Robotic Perception, Cognition and Navigation course. Also related to Coordinate Frame.

Geographic Coordinate System ($\varphi ,\lambda ,h$)

• Geographic Latitude (φ)
• the angle between the ellipsoidal normal through point "P" and the equatorial plane
• Geographic Longitude (λ)
• angle in the equatorial plane between the zero median and the meridian of point "P"
• Elipsoidal Height (h)
• the distance along the normal from the surface of the ellipsoid.
• Geometric surface (Geoid): An equipotential surface where gravity is constant everywhere. It approximates mean sea level extended through continents.
• Reference surface (Ellipsoid): A mathematical approximation of the geoid used for navigation and mapping. It's a smooth ellipsoid shape.

• Lines of equal latitude are called parallels. They form circles on the surface of the ellipsoid.
• Lines of equal longitude are called meridians and they form ellipses (meridian ellipses) on the ellipsoid of the Earth.

• The Earth is an ellipsoid (or a spheroid)

Geocentric Coordinate System ($X,Y,Z$)

• Cartesian coordinate system
• Origin (0,0,0) is at the center of the Earth (goecentric)

• Earth-centered, Earth-fixed (ECEF)
• OZ Axis – along the rotational axis
• OX Axis - lies on the equatorial plane and intersect prime meridian
• OY Axis – lies on the equatorial plane perpendicular to OX and OZ such that OX, OY, OZ form a right-handed coordinate system

Geographic coordinates to Geocentric

• The distance is $v(N{P}^{\prime })=\frac{a}{\sqrt{1-e^2{\sin }^2\phi }}$
• The eccentricity squared is $e^2=\frac{a^2-b^2}{b^2}$. Ellipsoid!
• Required is the projection of $OQ=(v+h)\cos \phi$ onto $XY$ plane, and $PQ$ on $Z$ axis:

$$X=(v+h)\cos \phi \cos \lambda$$ $$Y=(v+h)\cos \phi \sin \lambda$$ $$Z=(v(1-e^2)+h)\sin \phi$$

IMU Measurement, BODY FRAME

• Gyroscopes: 3 angular velocities in body frame (=b) with respect to Earth inertial frame (=i) ${\omega }_{ib}^b=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$
• Accelerometers, 3 specific forces in body frame. $f^b=\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]$
• $f^b=a^b-g^b$.

FROM EARTH INERTIAL TO BODY FRAME

• All IMU measurements are in body frame but relative to the inertial frame.
• How do we connect these two frames? We’ll use the following frames
  • Earth-centered Inertial Frame (Inertial frame)
  • Earth-centered Earth-fixed Frame (ECEF)
  • Local-level (navigation) Frame
  • Body Frame

EARTH-CENTERED INERTIAL FRAME (INERTIAL FRAME)

• The origin ($X=0,Y=0,Z=0$) is at the center of mass of the Earth ⇒ a geocentric coordinate system
• Z-axis: Along Earth’s rotation axis through the Conventional Terrestrial Pole (CTP).
• X-axis: In the equatorial plane pointing toward the vernal equinox (a fixed direction in space).
  • Does not rotate with respect to the stars (inertial).
• Y-axis: Completes a right-handed coordinate system.

This frame is “inertial” because the x-axis remains fixed relative to distant stars rather than rotating with Earth. This is different from Earth-fixed frames (like latitude/longitude coordinates) which rotate along with the planet. The inertial frame is “inertial” precisely because it doesn’t accelerate or rotate - it maintains a constant orientation relative to distant stars.

EARTH-CENTERED EARTH-FIXED FRAME (ECEF)

• The origin (X=0, Y=0, Z=0) is at the center of mass of the Earth => a geocentric coordinate system
• The Z-axis is along axis of the Earth’s rotation through the conventional terresterial pole (CTP).
• The X-axis passes through the intersection of the equatorial plane and the reference maridian (Greenwich)
• Rotates as Earth rotates.
• The y-axis completes a right-handed system in the equatorial plane.
• Handy, since our robot moves with the Earth’s rotation!

LOCAL-LEVEL (NAVIGATION) FRAME, NEU

• The origin coincides with the center of the sensor frame.
• The Y-axis points to true north.
• The X-axis points to east.
• The Z-axis completes the left-handed coordinate systems but pointing up
• Determine the height above the chosen reference from GNSS (e.g., ellipsoid) or other measurements (e.g. terrain).
• Could be also used in right-handed NED format. Then down is positive, so height decreases as altitude increases (e.g., height = 0 at sea level, height = -500 m for an object 500 m above sea level

BODY FRAME

• The origin usually coincides with the center of mass of the vehicle.
• The y-axis points towards the forward direction.
• The x-axis points toward the right side of the platform.
• The z-axis points toward the vertical direction completing a right-handed system.

How to transform between the frames?

• Transformations from one frame into another are rotations for
  • Acceleration
  • Angular Velocity
• The subscript of the rotation matrix is the original frame and the superscript is the destination frame: $v^i=R_e^i{v}^e$.
• Rotations are linear transformations.

Rotation Matrices

• Attitude, velocity, acceleration and angular velocity are all vectors in 3D space: $v^a\in R^3$.

Frame indices: i (inertial), e (ECEF), l (local), b (body).

The rule of the right hand

• For a right-handed coordinate system with axes x, y, z, we have the following cross product rules:
• Y x Z = X
• X x Y = Z
• Z x X = Y

Rotation from one system to another

1. ECEF to Inertial (Time-Dependent)

• Earth rotation rate vector: ${\omega }_{ie}^i=[0,0,{\omega }_e{]}^T$.
• This transformation is time-dependent because ECEF rotates with Earth while the inertial frame stays fixed relative to stars.
• Rotation matrix (time-dependent)

${\mathbf{R}}_e^i=\left[\begin{array}{ccc}\cos {\omega }_{e}t& \sin {\omega }_{e}t& 0\\ -\sin {\omega }_{e}t& \cos {\omega }_{e}t& 0\\ 0& 0& 1\end{array}\right]$

2. Local to ECEF

${\mathbf{R}}_l^e={\mathbf{R}}_3(-\lambda -90){\mathbf{R}}_1(\varphi -90)$

${\mathbf{R}}_l^e=\left[\begin{array}{ccc}-\sin \lambda & -\sin \varphi \cos \lambda & \cos \lambda \cos \lambda \\ \cos \lambda & -\sin \varphi \sin \lambda & \cos \lambda \sin \lambda \\ 0& \cos \varphi & \sin \varphi \end{array}\right]$

3. Body to Local

${\mathbf{R}}_b^l=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right]\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]$

Derivative of the Rotation Matrix

• Let’s consider the position vector $r^i=R_b^i{r}^b$.
• If we want to get the velocity, we do ${\dot{r}}^i={\dot{R}}_b^i{r}^b+R_b^i{\dot{r}}^b$.

Why do we need the derivative of the rotation matrix?

• To transform velocities from one frame to another
• To transform accelerations from one frame to another
• To transform angular velocities from one frame to another
• So basically, the derivatives (not position or attitude, those use the basic Rotation Matrix).

The key challenge is to determine the time derivative of the rotation matrix ${\dot{R}}_b^i$. In Rotation Matrix Time Derivative I covered the mathematics behind finding the following solution:

${\dot{r}}^i=R_b^i({\Omega }_{ib}^b{r}^b+{\dot{r}}^b),{\Omega }_{ib}^b=S(\omega )=\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right)$

Also, the second derivative (the acceleration) will be:

$${\ddot{r}}^i=R_b^i({\ddot{r}}^b+2{\Omega }_{ib}^b{\dot{r}}^b+{\dot{\Omega }}_{ib}^b{r}^b+{\Omega }_{ib}^b{\Omega }_{ib}^b{r}^b)$$

The last 3 terms are the Coriolis force, the Euler force and Centrifugal force respectively.

FICTITIOUS FORCES

These deserve their own dedicated page. Will make one!

GIMBAL LOCK

Covered in Gimbal Lock. Basically, we lose one degree of freedom when two axes align in parallel.

The solution is using Quaternions

QUATERNIONS

Covered in Quaternions. It’s a 4D way of representing rotations and orientations.

Homogenous Coordinates

Without homogenous coordinates, if we have 3 points

$$p_B=R_1{p}_A+t_1$$ $$p_C=R_2{p}_B+t_2$$ $$p_D=R_3{p}_C+t_3$$

then $p_D=R_3(R_2(R_1{p}_A+t_1)+t_2)+t_3$

Rotation and scaling can be represented by a matrix, but translation cannot.

Translation requires vector addition. $p^w=R_b^w{p}^b+t_b^w$

Homogenous coordinates overcome this limitation by adding an extra coordinate (from (x,y,z) to (x,y,z,1) in 3D). This enables all transformations, including translation, to be represented as matrix multiplications.

Therefore, if now we have the same 3 points but represented as

$$p_B=H_1{p}_A$$ $$p_C=H_2{p}_B$$ $$p_D=H_3{p}_C$$

then $p_D=H_3{H}_2{H}_1{p}_A$