M12-T1 — Uncertainty on Manifolds: Angles and Rotations

Learning objectives

After this lesson, you should be able to:

• Compute a circular mean and spread, and explain why the arithmetic mean of angles fails.
• Use the exponential and logarithm maps to move between rotation vectors and rotation matrices.
• Represent the uncertainty of a rotation as a covariance of a tangent-space error, and compose two uncertain rotations to first order.
• Explain gimbal lock and why Euler-angle covariances are misleading near it.

The intuition

A robot's heading is measured 1 000 times while it faces almost exactly backwards (180°), with 0.2 rad of noise. About half the readings come out near +180° and half near −180°. The arithmetic mean is therefore near 0°: it points forward, the opposite direction, with a standard deviation of 2.98 rad that says nothing about the true scatter of 0.2 rad. The circular mean adds the headings as unit vectors, $(\cos\theta,\sin\theta)$, and takes the direction of the sum: 179.8°, with a spread of 0.201 rad.

In three dimensions the same problem is worse, because rotations do not even commute. The cure is to stop treating the rotation as a list of numbers. An uncertain rotation is written $R=\hat R\,\mathrm{Exp}(\boldsymbol\delta)$: an estimate $\hat R$ multiplied by a small rotation whose axis-angle vector $\boldsymbol\delta$ lies in a flat 3-D space, the tangent space at the identity. In that space an ordinary Gaussian $\mathcal{N}(0,\Sigma)$ makes sense and Σ is a 3 × 3 covariance with a physical meaning: the spread of the rotation error, in radians about each axis.

The right panel shows what goes wrong with the alternative, Euler angles (yaw, pitch, roll). With the true pitch at 0° an error of 0.05 rad in each axis appears as 0.05 rad in each angle: the numbers are what one expects. At a pitch of 60° the same error gives 0.10 in yaw and roll; at 85° 0.70; at 89° 1.55 rad. The rotation is exactly as uncertain in every case (0.05 rad, in the tangent space); only the description is breaking down, because at 90° of pitch yaw and roll become the same rotation (gimbal lock). A covariance on Euler angles near that configuration is useless; a covariance on the tangent vector is not.

Add and subtract uncertainty in the tangent space, not on the angles. The composition R = R̂ Exp(δ) replaces the addition θ = θ̂ + δ.

Definitions

Circular mean

$\bar\theta=\operatorname{atan2}\bigl(\overline{\sin\theta},\overline{\cos\theta}\bigr)$: the direction of the mean unit vector. Its length $\bar r=\lVert(\overline{\cos\theta},\overline{\sin\theta})\rVert$ (between 0 and 1) says how concentrated the angles are.

Wrapped difference

$\theta_1\ominus\theta_2=\operatorname{atan2}(\sin(\theta_1-\theta_2),\cos(\theta_1-\theta_2))$: the shortest signed angle from $\theta_2$ to $\theta_1$.

Rotation group $SO(3)$

The 3 × 3 matrices $R$ with $R^\top R=I$ and $\det R=1$.

Exponential map

$\mathrm{Exp}(\mathbf{w})$: the rotation by angle $\lVert\mathbf{w}\rVert$ about the axis $\mathbf{w}/\lVert\mathbf{w}\rVert$. The logarithm $\mathrm{Log}$ is its inverse.

Tangent-space error

$\boldsymbol\delta=\mathrm{Log}(\hat R^\top R)$: the small rotation from the estimate to the truth, expressed in the frame of the estimate.

Derivation: the circular mean, and the first-order composition

Why the circular mean

The natural mean of points on a circle is the point that minimises the sum of squared distances. Using the chord (straight-line) distance between unit vectors, $\lVert\mathbf{u}(\theta)-\mathbf{u}(\phi)\rVert^2=2-2\cos(\theta-\phi)$:

1. $$\bar\theta=\arg\min_\phi\sum_i\bigl(2-2\cos(\theta_i-\phi)\bigr)=\arg\max_\phi\sum_i\cos(\theta_i-\phi)$$

   Minimise the squared chord distances to the sample.

2. $$\sum_i\cos(\theta_i-\phi)=\cos\phi\sum\cos\theta_i+\sin\phi\sum\sin\theta_i=N\bar r\cos(\phi-\bar\theta)$$

   Expand the cosine of a difference; the sums define the mean resultant $\bar r$ and angle $\bar\theta$.

3. $$\bar\theta=\operatorname{atan2}\Bigl(\sum\sin\theta_i,\sum\cos\theta_i\Bigr)$$

   Maximised at $\phi=\bar\theta$. The result is the direction of the vector sum.

This is the maximum likelihood estimate of the centre of a von Mises distribution, the circular analogue of the Gaussian $p(\theta)\propto e^{\kappa\cos(\theta-\mu)}$, and for small spread it is close to a wrapped Gaussian. The spread is best measured by the wrapped differences $\theta_i\ominus\bar\theta$, as the example does (0.201 rad).

Composition of rotation errors

Take $R=R_1R_2$ with $R_i=\hat R_i\mathrm{Exp}(\boldsymbol\delta_i)$. We want the tangent error of the product relative to $\hat R=\hat R_1\hat R_2$.

1. $$\hat R^\top R=\hat R_2^\top\,\mathrm{Exp}(\boldsymbol\delta_1)\,\hat R_2;\mathrm{Exp}(\boldsymbol\delta_2)$$

   Substitute and cancel $\hat R_1^\top\hat R_1=I$.

2. $$\hat R_2^\top\mathrm{Exp}(\boldsymbol\delta_1)\hat R_2=\mathrm{Exp}(\hat R_2^\top\boldsymbol\delta_1)$$

   Conjugating a rotation by another rotates its axis: the identity $R\,\mathrm{Exp}(\mathbf{w})R^\top=\mathrm{Exp}(R\mathbf{w})$, here with $R=\hat R_2^\top$.

3. $$\mathrm{Exp}(\mathbf{a})\mathrm{Exp}(\mathbf{b})\approx\mathrm{Exp}(\mathbf{a}+\mathbf{b})\quad(\text{small }\mathbf{a},\mathbf{b})$$

   The Baker–Campbell–Hausdorff formula to first order: the correction is of order $\tfrac12\mathbf{a}\times\mathbf{b}$.

4. $$\boldsymbol\delta\approx\hat R_2^\top\boldsymbol\delta_1+\boldsymbol\delta_2;\Rightarrow;\Sigma_R=\hat R_2^\top\Sigma_1\hat R_2+\Sigma_2$$

   For independent errors the covariances add, with the first one rotated into the new frame.

The neglected term $\tfrac12\mathbf{a}\times\mathbf{b}$ is second order and explains why the approximation degrades when the errors are large: in the example it is accurate to 1% for errors of 0.05 and to 2% for 0.2, and 13% off at 0.5 rad.

Gimbal lock

For the ZYX Euler angles, $R_{31}=-\sin(\text{pitch})$ and yaw and roll are recovered from $\operatorname{atan2}$ of entries proportional to $\cos(\text{pitch})$. As the pitch tends to 90° those entries tend to zero, and a small perturbation of the matrix changes the angle by a large amount: the Jacobian from tangent errors to yaw and roll grows like $1/\cos(\text{pitch})$. At 85° it is 11.5, and at 89° 57, and the measured standard deviations (0.70 and 1.55) are of that order (0.05 × 11.5 = 0.57 at 85°, against 0.70 measured), saturating when the angle spreads over a large part of its range.

Do the algebra of uncertainty in the tangent space. Use matrices or quaternions to store the rotation and a three-vector to describe its error: no singularities, and covariances keep their meaning.

A worked example

Three programs of pure Python (a few lines each for $\mathrm{Exp}$ and $\mathrm{Log}$).

1. Circular statistics. 1 000 headings around 180° with a true spread of 0.2 rad. The arithmetic mean is 0.1° with a standard deviation of 2.98 rad; the circular mean is 179.8° with a spread of 0.201 rad of wrapped differences.
2. Composition. Two rotations $R_1$ and $R_2$ (rotation vectors (0.3, −0.5, 0.8) and (−0.6, 0.2, 0.4)), the first with per-axis standard deviations (s, 3s, s/2), the second (2s, s, s). 20 000 Monte Carlo trials compared with $\hat R_2^\top\Sigma_1\hat R_2+\Sigma_2$. For s = 0.05 the standard deviations per axis are 0.118, 0.126, 0.103 against 0.119, 0.126, 0.103, and the xy covariance 0.0042 against 0.0043. For s = 0.2: 0.467, 0.501, 0.416 against 0.477, 0.503, 0.412. For s = 0.5: 1.043, 1.114, 0.998 against 1.192, 1.258, 1.029, and the xy covariance 0.267 against 0.433: the approximation is breaking down.
3. Euler angles. A 0.05 rad error per axis in the tangent space, read as yaw/pitch/roll, for rotations of pitch 0°, 60°, 85° and 89°: the standard deviations of yaw are 0.051, 0.101, 0.704 and 1.551 rad (roll is the same); the pitch stays at 0.05, 0.05, 0.045 and 0.034; the tangent error is 0.05 throughout.

import math
from robotprob import Rng, mean, matmul, transpose, matvec, wrap_angle

# 1. angles are not numbers on a line: the mean of headings that straddle +-pi
rng = Rng(3)
heads = [wrap_angle(math.pi + rng.gauss(0.0, 0.2)) for _ in range(1000)]          # true heading 180 degrees, noise 0.2 rad
naive = mean(heads)
naive_sd = math.sqrt(mean([(h - naive) ** 2 for h in heads]))
circ = math.atan2(mean([math.sin(h) for h in heads]), mean([math.cos(h) for h in heads]))
circ_sd = math.sqrt(mean([wrap_angle(h - circ) ** 2 for h in heads]))
print('1000 headings around 180 degrees with a true spread of 0.2 rad')
print('  naive mean', round(math.degrees(naive), 1), 'deg, naive std', round(naive_sd, 2), 'rad')
print('  circular mean', round(math.degrees(circ) % 360, 1), 'deg, std of wrapped differences', round(circ_sd, 3), 'rad')

# 2. rotations in 3D: the exponential map, the logarithm, and uncertainty in the tangent space
def hat(w): return [[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]]
def Exp(w):
    th = math.sqrt(sum(a * a for a in w))
    K = hat(w)
    if th < 1e-9: return [[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]
    K2 = matmul(K, K)
    a, b = math.sin(th) / th, (1 - math.cos(th)) / th ** 2
    return [[(1.0 if i == j else 0.0) + a * K[i][j] + b * K2[i][j] for j in range(3)] for i in range(3)]
def Log(R):
    c = max(-1.0, min(1.0, (R[0][0] + R[1][1] + R[2][2] - 1) / 2)); th = math.acos(c)
    if th < 1e-9: return [0.0, 0.0, 0.0]
    k = th / (2 * math.sin(th))
    return [k * (R[2][1] - R[1][2]), k * (R[0][2] - R[2][0]), k * (R[1][0] - R[0][1])]
def cov3(vs):
    return [[mean([v[i] * v[j] for v in vs]) for j in range(3)] for i in range(3)]

R1, R2 = Exp([0.3, -0.5, 0.8]), Exp([-0.6, 0.2, 0.4])
print('composing two uncertain rotations R = R1 R2, each with an error R_hat Exp(delta); delta1 has std (s, 3s, s/2) per axis and delta2 std (2s, s, s)')
print('  s     std of the error of R per axis: Monte Carlo        first order R2^T S1 R2 + S2      covariance xy: MC, first order')
for s in (0.05, 0.2, 0.5):
    r = Rng(11)
    sg1, sg2 = [s, 3 * s, s / 2], [2 * s, s, s]
    errs = []
    for _ in range(20000):
        d1 = [r.gauss(0.0, a) for a in sg1]; d2 = [r.gauss(0.0, a) for a in sg2]
        Rt = matmul(matmul(R1, Exp(d1)), matmul(R2, Exp(d2)))                    # the true rotation
        Rh = matmul(R1, R2)                                                      # the estimate
        errs.append(Log(matmul(transpose(Rh), Rt)))                              # the error as a tangent vector
    C = cov3(errs)
    S1 = [[sg1[i] ** 2 if i == j else 0.0 for j in range(3)] for i in range(3)]
    S2 = [[sg2[i] ** 2 if i == j else 0.0 for j in range(3)] for i in range(3)]
    P = matmul(matmul(transpose(R2), S1), R2)
    first = [[P[i][j] + S2[i][j] for j in range(3)] for i in range(3)]
    print(str(s).rjust(5), '  ', [round(math.sqrt(C[i][i]), 3) for i in range(3)], '   ', [round(math.sqrt(first[i][i]), 3) for i in range(3)], '   ', round(C[0][1], 4), round(first[0][1], 4))

# 3. Euler angles near gimbal lock: the same small error looks huge in roll and yaw
def euler(R):                                                                    # ZYX: yaw, pitch, roll
    pitch = -math.asin(max(-1.0, min(1.0, R[2][0])))
    return math.atan2(R[1][0], R[0][0]), pitch, math.atan2(R[2][1], R[2][2])
print('the same rotation error of 0.05 rad (tangent space) read as Euler angles, 5000 samples')
print('pitch of the true rotation   std yaw   std pitch   std roll   std of the tangent error')
for pitch_deg in (0, 60, 85, 89):
    p = math.radians(pitch_deg)
    Rt0 = [[math.cos(p), 0.0, -math.sin(p)], [0.0, 1.0, 0.0], [math.sin(p), 0.0, math.cos(p)]]   # a pure pitch rotation
    r = Rng(5)
    ys, ps, rs, ts = [], [], [], []
    for _ in range(5000):
        d = [r.gauss(0.0, 0.05) for _ in range(3)]
        Rn = matmul(Rt0, Exp(d))
        y, pi_, ro = euler(Rn)
        ys.append(y); ps.append(pi_ - p); rs.append(ro)
        ts.append(sum(a * a for a in d) / 3)
    sd = lambda v: math.sqrt(mean([(a - mean(v)) ** 2 for a in v]))
    print(str(pitch_deg).rjust(14), 'deg', str(round(sd(ys), 3)).rjust(12), str(round(sd(ps), 3)).rjust(10), str(round(sd(rs), 3)).rjust(10), str(round(math.sqrt(mean(ts)), 3)).rjust(14))

1000 headings around 180 degrees with a true spread of 0.2 rad
  naive mean 0.1 deg, naive std 2.98 rad
  circular mean 179.8 deg, std of wrapped differences 0.201 rad
composing two uncertain rotations R = R1 R2, each with an error R_hat Exp(delta); delta1 has std (s, 3s, s/2) per axis and delta2 std (2s, s, s)
  s     std of the error of R per axis: Monte Carlo        first order R2^T S1 R2 + S2      covariance xy: MC, first order
 0.05    [0.118, 0.126, 0.103]     [0.119, 0.126, 0.103]     0.0042 0.0043
  0.2    [0.467, 0.501, 0.416]     [0.477, 0.503, 0.412]     0.0671 0.0692
  0.5    [1.043, 1.114, 0.998]     [1.192, 1.258, 1.029]     0.267 0.4326
the same rotation error of 0.05 rad (tangent space) read as Euler angles, 5000 samples
pitch of the true rotation   std yaw   std pitch   std roll   std of the tangent error
             0 deg        0.051       0.05       0.05           0.05
            60 deg        0.101       0.05      0.101           0.05
            85 deg        0.704      0.045      0.704           0.05
            89 deg        1.551      0.034       1.55           0.05

In robotics

Where this matters in a filter

• Headings in the EKF and UKF (Module 8): wrap every angle residual, average angular sigma points with sines and cosines.
• Orientation in a 3-D filter: store the rotation as a quaternion or matrix and keep a 3-D tangent-space error in the covariance: the error-state filter of the next lesson.
• Pose graphs in 3-D (Module 11): the Gauss–Newton update of a pose is $R\leftarrow R\,\mathrm{Exp}(\Delta)$, never $R+\Delta$.
• Sampling rotation noise for a particle filter: draw $\boldsymbol\delta\sim\mathcal{N}(0,\Sigma)$ and set $R\leftarrow R\,\mathrm{Exp}(\boldsymbol\delta)$.

Left or right perturbation

The error can be defined in the body frame ($R=\hat R\mathrm{Exp}(\boldsymbol\delta)$, “right”) or in the world frame ($R=\mathrm{Exp}(\boldsymbol\delta)\hat R$, “left”). Both work; they differ in the Jacobians and in the frame in which Σ is expressed (related by the rotation $\hat R$). Choose one and use it consistently; mixing them is a classic source of bugs.

Representations

Directional statistics beyond the mean

For a 2-D heading the von Mises distribution plays the role of the Gaussian; for a direction on a sphere the von Mises–Fisher distribution does; for rotations with heavy uncertainty (a kidnapped orientation) the matrix Fisher or Bingham distributions on quaternions are used. For small errors, all are well approximated by the tangent-space Gaussian.

Common misconceptions

Key takeaways

1. Angles live on a circle: average unit vectors (circular mean) and measure spread with wrapped differences. The arithmetic mean near ±180° is wrong by 180°.
2. An uncertain rotation is R̂ Exp(δ) with δ ~ N(0, Σ) in the 3-D tangent space; Σ is a covariance in rad², with no singularities.
3. Compose to first order with Σ_R = R̂₂ᵀ Σ₁ R̂₂ + Σ₂: 1–2% accurate for errors up to 0.2 rad in the example, 13% off at 0.5 rad.
4. Euler angles blow up near gimbal lock: the same 0.05 rad error appeared as 1.55 rad in yaw and roll at 89° of pitch.
5. Update rotations by R ← R Exp(Δ), store them as matrices or quaternions, and keep covariances in the tangent space.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• J. Solà, J. Deray and D. Atchuthan, “A micro Lie theory for state estimation in robotics”, arXiv:1812.01537, 2018 , the standard compact introduction to exactly this material. Free to read; the arXiv listing’s licence was not checked.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , chapter 7 (matrix Lie groups) and the sections on uncertainty of rotations. Free to read as the author’s PDF; the book is copyrighted.
• K. V. Mardia and P. E. Jupp, Directional Statistics, Wiley, 2000, chapters 2 and 3 (circular mean, von Mises distribution). Paid book.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, sections 5.4 and 7.5 for the handling of angles in motion and measurement models. Paid book.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

With the tangent-space error in hand, the next lesson builds the filter that every aircraft and phone uses: the error-state Kalman filter, which keeps a nominal state on the manifold and estimates a small error around it.

Related content

Nothing below is required to finish this lesson.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T6: Error-State Kalman Filter
• UAV M6-T5: PX4 state estimation and EKF2
• Physical AI: Belief under uncertainty