Coordinate Transforms

Coordinate system transformations for robotics.

Homogeneous Coordinates 

Points in 3D are represented as 4D vectors:

\[\begin{split}\mathbf{p} = \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}\end{split}\]

A 4×4 transformation matrix combines rotation and translation:

\[\begin{split}T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}\end{split}\]

Where \(R\) is a 3×3 rotation matrix and \(t\) is translation.

Rotation about Z-axis (yaw):

\[\begin{split}R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}\end{split}\]

Convert WGS84 to UTM:

from pyproj import Transformer

transformer = Transformer.from_crs("EPSG:4326", "EPSG:32611")
x, y = transformer.transform(lat, lon)

Transform world coordinates to vehicle frame:

\[\mathbf{p}_{vehicle} = R(-\theta)(\mathbf{p}_{world} - \mathbf{p}_{vehicle\_origin})\]

Where \(\theta\) is the vehicle heading.