2D Rotations and Orientation¶

The Angular State: More Than Just a Point¶

In basic geometry, a "point" is defined solely by its coordinates. However, a robot is an extended body; it doesn't just exist at a location—it faces a direction. This heading is known as orientation (or, in more formal aeronautic terms, attitude).

To fully describe a robot's state in a 2D plane, we move beyond a simple point with state $(p_1, p_2, v_1, v_2)$ and introduce a fifth state variable: the heading angle $\theta$.

Key Properties of $\theta$

• Units: While degrees are intuitive, robotics algorithms almost exclusively use radians.
• Topology: Unlike linear distance, which is infinite, orientation is periodic. It exists on a circle, meaning the state "wraps around" (e.g., $0$ and $2\pi$ represent the same physical heading).
• Measurement: We track changes in orientation using an Inertial Measurement Unit (IMU). Specifically, the gyroscope inside the IMU measures angular velocity ($\omega$), defined as the rate of change of the angle over time:

$$\omega = \frac{d\theta}{dt} = \dot{\theta}$$

This is the rotational analog of the position-velocity relationship.

Discretization of Rotation¶

To use this in a Kalman Filter, we must discretize it. Following the same logic as linear motion, we assume the measured angular velocity ($\omega_{k-1}$) is constant over the time step $\Delta t = t_k - t_{k-1}$.

Integrating the differential equation:

$$\int_{\theta_{k-1}}^{\theta_k} d\theta = \int_{t_{k-1}}^{t_k} \omega_{k-1} \, dt$$

Final Discrete Orientation Equation:

$$\theta_k = \theta_{k-1} + \omega_{k-1} \Delta t$$

A Primer: What is a 2D Rotation?¶

Before we use the rotation matrix $\mathbf{R}$, it is worth seeing where it comes from. A rotation in the plane is a linear map that turns every vector about the origin by a fixed angle while preserving its length (and the angles between vectors).

Deriving the matrix by rotating a vector. Take a vector $\mathbf{c}$ of length $r$ pointing at angle $\alpha$:

$$\mathbf{c} = \begin{bmatrix} c_1 \\ c_2 \end{bmatrix} = \begin{bmatrix} r\cos\alpha \\ r\sin\alpha \end{bmatrix}.$$

Rotating it counter-clockwise (CCW) by $\theta$ simply adds $\theta$ to its angle:

$$\mathbf{c}' = \begin{bmatrix} r\cos(\alpha+\theta) \\ r\sin(\alpha+\theta) \end{bmatrix}.$$

Apply the angle-addition identities $\cos(\alpha+\theta)=\cos\alpha\cos\theta-\sin\alpha\sin\theta$ and $\sin(\alpha+\theta)=\sin\alpha\cos\theta+\cos\alpha\sin\theta$, then substitute $c_1 = r\cos\alpha$ and $c_2 =r\sin\alpha$:

$$\mathbf{c}' = \begin{bmatrix} c_1 \cos\theta - c_2 \sin\theta \\ c_1 \sin\theta + c_2 \cos\theta \end{bmatrix} = \underbrace{\begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}}_{\mathbf{R}(\theta)} \begin{bmatrix} c_1 \\ c_2 \end{bmatrix}.$$

The same matrix, read column-by-column. A linear map is completely determined by what it does to the basis vectors. Rotating $\mathbf{e}_1=[1,0]^T$ by $\theta$ gives $[\cos\theta,\sin\theta]^T$; rotating $\mathbf{e}_2=[0,1]^T$ gives $[-\sin\theta,\cos\theta]^T$. Stacking these images as columns reproduces $\mathbf{R}(\theta)$. This is the recurring lesson: the columns of a rotation matrix are the rotated basis vectors.

Properties of the Rotation Matrix $\mathbf{R}$¶

$\mathbf{R}(\theta)$ belongs to the Special Orthogonal Group $SO(2)$ — the group of 2D rotations. Two facts define this group, and the rest follow from them:

• its columns are orthonormal, and
• its determinant is $+1$.

Why are the columns orthonormal? ($\Rightarrow \mathbf{R}^{-1}=\mathbf{R}^T$)

We saw above that the columns of $\mathbf{R}$ are the rotated basis vectors, $\mathbf{R}\mathbf{e}_1=[\cos\theta,\sin\theta]^T$ and $\mathbf{R}\mathbf{e}_2=[-\sin\theta,\cos\theta]^T$. The original basis $\mathbf{e}_1,\mathbf{e}_2$ is orthonormal (unit-length and mutually perpendicular), and a rotation preserves lengths and angles, so the rotated columns are orthonormal as well. Orthonormal columns are precisely the statement

$$\mathbf{R}^T\mathbf{R}=\mathbf{I}\quad\Longleftrightarrow\quad \mathbf{R}^{-1}=\mathbf{R}^T .$$

You can verify it directly, with $c=\cos\theta,\ s=\sin\theta$:

$$\mathbf{R}^T\mathbf{R}=\begin{bmatrix} c & s \\ -s & c \end{bmatrix}\begin{bmatrix} c & -s \\ s & c \end{bmatrix}=\begin{bmatrix} c^2+s^2 & 0 \\ 0 & c^2+s^2 \end{bmatrix}=\mathbf{I}.$$

Rigid transformation (isometry). Because $\mathbf{R}$ is orthogonal it preserves the norm of every vector and the angle between any two vectors ($\lVert\mathbf{R}\mathbf{c}\rVert=\lVert\mathbf{c}\rVert$). The robot does not "stretch" or "squish" when it turns.

No reflections ($\det\mathbf{R}=+1$). Here $\det\mathbf{R}=c^2+s^2=1$. A determinant of $+1$ (rather than $-1$) guarantees the coordinate system stays right-handed and is not mirrored — a pure rotation, never a reflection.

Group structure of $SO(2)$.

• Composition adds angles: $\mathbf{R}(\theta_1)\mathbf{R}(\theta_2)=\mathbf{R}(\theta_1+\theta_2)$.
• 2D rotations commute: $\mathbf{R}(\theta_1)\mathbf{R}(\theta_2)=\mathbf{R}(\theta_2)\mathbf{R}(\theta_1)$ — a special feature of the plane (3D rotations do not commute).
• Identity and inverse: $\mathbf{R}(0)=\mathbf{I}$ and $\mathbf{R}(\theta)^{-1}=\mathbf{R}(-\theta)$. Since $\cos$ is even and $\sin$ is odd, $\mathbf{R}(-\theta)=\mathbf{R}(\theta)^T$ — the same "inverse is the transpose" result, now read straight off the angle.

Why this matters in practice. Inverting a rotation — e.g. mapping a world-frame vector back to the body frame, $\mathbf{a}^B=\mathbf{R}^T\mathbf{a}^W$ — is therefore essentially free: a transpose just relabels entries ($O(n^2)$) and avoids the $O(n^3)$ cost of a general matrix inverse. It is also more numerically robust: using the transpose sidesteps the rounding-error accumulation of explicit inversion, keeping $\mathbf{R}$ orthonormal over many filter steps.

Active vs. Passive Rotations (Alibi vs. Alias)¶

The single matrix $\mathbf{R}(\theta)$ can play two conceptually different roles. Confusing them is the most common source of sign errors in robotics.

Active rotation (alibi — "the object moves"). The coordinate frame is held fixed and the vector itself is physically rotated by $+\theta$:

$$\mathbf{c}' = \mathbf{R}(\theta)\,\mathbf{c}.$$

Here $\mathbf{c}'$ is a genuinely different arrow than $\mathbf{c}$.

Passive rotation (alias — "same object, new coordinates"). The vector is held fixed and the coordinate axes are rotated by $+\theta$. The physical arrow does not move; only its components change. Expressing a fixed vector in axes rotated $+\theta$ CCW uses

$$\mathbf{c}_{\text{new}} = \mathbf{R}(-\theta)\,\mathbf{c}_{\text{old}} = \mathbf{R}(\theta)^T\,\mathbf{c}_{\text{old}}.$$

where

• $\mathbf{c}_{\text{old}}$ is the vector in the old coordinate system.
• $\mathbf{c}_{\text{new}}$ is the same vector in the new coordinate system.
• The angle $\theta$ is the CCW rotation of the old to the new coordinate system.

Note:

• Rotating a vector by $+\theta$ produces the same numbers as keeping the vector still and rotating the axes by $-\theta$.
• If the angle $\theta$ is the CCW rotation of the new to the old coordinate system the sign changes: $$\mathbf{c}_{\text{new}} = \mathbf{R}(\theta)\,\mathbf{c}_{\text{old}} $$

Defining Orientation ($\theta$): The Bridge Between Frames¶

Crucially, the orientation angle $\theta_k$ is defined as the angle of the Body Frame relative to the World Frame. By standard convention, this is the counter-clockwise angle measured from the fixed World X-axis ($X^W$) to the robot's forward-pointing Body X-axis ($X^B$). This definition is what allows our rotation matrix $\mathbf{R}_k$ to function as a Body-to-World transformer, taking accelerations measured relative to the robot's nose ($\mathbf{a}^B$) and projecting them onto the global map axes ($\mathbf{a}^W$). Conversely, the transpose $\mathbf{R}_k^T$ represents the inverse World-to-Body transformation.

In the alias (passive) language above, this is the new$\to$old case with the World as the new frame and the Body as the old: $\theta$ is the angle from the World axes to the Body axes, so $\mathbf{a}^W = \mathbf{R}(\theta)\,\mathbf{a}^B$ (no sign flip).

Body Frame vs. Global Frame¶

In our model, we track the state vector $(p_1, p_2, v_1, v_2, \theta)$ in the World Frame ($W$, fixed to the map; we drop the explicit $W$ superscript on the state), while the sensors provide their inputs $(a_1^B, a_2^B, \omega)$ in the Body Frame ($B$):

• $a_1^B, a_2^B$: linear accelerations measured by the IMU relative to the robot's local orientation (its "nose").
• $\omega$: the angular velocity (yaw rate) from the gyroscope, measured about its vertical (out-of-plane) axis. Unlike the linear accelerations, $\omega$ does not need to be rotated between frames: in 2D the rotation axis (the out-of-plane $Z$-axis) is shared by the Body and World frames for every heading, so $\omega^B = \omega^W$ and it can be integrated directly into the heading (see Discretization of Rotation above). This frame-invariance is special to planar rotation; in full 3D the body-frame gyro rates must be transformed before integration.

Because the accelerations live in the Body frame but the state lives in the World frame, the filter must rotate $(a_1^B, a_2^B)$ into the World frame before using them. Using the orientation $\theta$ defined above (the World$\to$Body angle), $\mathbf{R}(\theta)$ is exactly the Body-to-World map:

$$\begin{bmatrix} a_1^W \\ a_2^W \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} a_1^B \\ a_2^B \end{bmatrix} = \begin{bmatrix} a_1^B \cos\theta - a_2^B \sin\theta \\ a_1^B \sin\theta + a_2^B \cos\theta \end{bmatrix},$$

or, compactly, with time indices ($\theta_k$):

$$\mathbf{a}_k^W = \mathbf{R}_k\,\mathbf{a}_k^B.$$

Example: Projecting a local vector to the world frame¶

Suppose the robot is oriented at $\theta = 45^\circ$ and its sensors measure an acceleration vector $\mathbf{a}^B = [1, 1]^T$ (meaning the robot is accelerating equally "forward" and "to its left"). To find this acceleration in the world frame, we apply the rotation matrix:

$$\mathbf{a}^W = \mathbf{R} \mathbf{a}^B = \begin{bmatrix} \cos 45^\circ & -\sin 45^\circ \\ \sin 45^\circ & \cos 45^\circ \end{bmatrix} \begin{bmatrix} 1 \\ 1 \end{bmatrix} = \begin{bmatrix} 0.707 & -0.707 \\ 0.707 & 0.707 \end{bmatrix} \begin{bmatrix} 1 \\ 1 \end{bmatrix} = \begin{bmatrix} 0 \\ 1.414 \end{bmatrix}$$

Result: The vector $[0, 1.414]^T$ points exactly along the World Y-axis.