Interactive guide · Lie groups & Lie algebras · Act IV

Uncertainty on Lie groups

A Gaussian is defined on a vector space: a mean, and a covariance describing spread around it by addition. A pose has no addition, so what does "the robot's pose is Gaussian" mean? The answer used throughout modern estimation is to put the Gaussian in the tangent space: the mean is a group element, and the random part is a tangent vector mapped on with $\oplus$. This part shows why that model fits real uncertainty far better than a Gaussian in $(x, y, \theta)$, and how to carry it through motion, composition, inversion and the transformation of points.

The banana: why Cartesian ellipses fail

A robot driving straight with a shaky heading

🎯 Goal: see that the true uncertainty of a pose after driving is curved, that a Gaussian in the tangent space captures the curve, and that a Gaussian in $(x,y)$ cannot.

Model the pose as a concentrated Gaussian on the group:

X = \bar X\oplus\varepsilon = \bar X\operatorname{Exp}(\varepsilon), \qquad \varepsilon\sim\mathcal{N}(0, \Sigma), \qquad \Sigma\in\mathbb{R}^{3\times3}\ \text{for}\ SE(2).

A robot drives straight ahead in $K$ steps, and every step carries a little noise in its heading and distance. Each run ends somewhere different. The Monte Carlo endpoints (gray) form a curved band, a banana: a heading error made early swings the rest of the path around an arc. The blue cloud is sampled from a tangent-space Gaussian whose covariance was propagated analytically (formula in section 2). It reproduces the banana. The magenta ellipse is the best Gaussian in $(x, y)$, fitted to the very same samples. It has to cover the arc with an oval, so it puts probability where the robot never goes and centres on a point the robot rarely reaches. A filter or smoother built on it is overconfident in some directions and pessimistic in others.

Monte Carlo endpoints samples of X̄·Exp(ε), ε ~ N(0, Σ) 2σ Gaussian ellipse in (x, y)

Heading noise per step (deg)

Distance noise per step

Steps K

Propagating covariance

Motion, composition and inversion

🎯 Goal: derive the covariance update for each basic operation from the adjoint and the Jacobians, and check each against a Monte Carlo estimate.

Everything follows from moving tangent vectors to one side with the adjoint (Part 8) and linearizing Exp with the Jacobian (Part 9). With right perturbations and independent errors:

\begin{aligned} \text{motion } X_{k+1} = X_k\operatorname{Exp}(u + n): &\quad \Sigma_{k+1} = \operatorname{Ad}_{\operatorname{Exp}(u)}^{-1}\,\Sigma_k\,\operatorname{Ad}_{\operatorname{Exp}(u)}^{-\top} + J_r(u)\,Q\,J_r(u)^\top \\ \text{composition } Z = XY: &\quad \Sigma_Z = \operatorname{Ad}_{Y}^{-1}\,\Sigma_X\,\operatorname{Ad}_{Y}^{-\top} + \Sigma_Y \\ \text{inversion } Z = X^{-1}: &\quad \Sigma_Z = \operatorname{Ad}_X\,\Sigma_X\,\operatorname{Ad}_X^\top \end{aligned}

The motion rule is what generated the blue banana above: $K$ applications of it, starting from $\Sigma_0 = 0$. The first term transports the old uncertainty into the new body frame, which is where heading error turns into sideways error. The second adds the new noise. The button below draws random $SE(2)$ poses and covariances, applies each rule, and compares it with the covariance of $20{,}000$ sampled tangent errors.

Left and right covariances

One distribution, two descriptions

The same uncertain pose can be written with the noise on either side: $X = \bar X\operatorname{Exp}(\varepsilon_r)$ with $\varepsilon_r$ in the body frame, or $X = \operatorname{Exp}(\varepsilon_l)\bar X$ with $\varepsilon_l$ in the world frame. Since $\varepsilon_l = \operatorname{Ad}_{\bar X}\varepsilon_r$, the covariances are related by

\Sigma_l = \operatorname{Ad}_{\bar X}\,\Sigma_r\,\operatorname{Ad}_{\bar X}^\top.

Neither description is more correct, but each is simpler for some problems. A body-frame (right) error is natural for odometry and IMU noise, which live in the body. A world-frame (left) error is natural for GPS and for the invariant EKF, whose error dynamics become independent of the state estimate. Before combining covariances from two sources, convert them to one side, and reorder their coordinates (Part 8) if needed.

Uncertain pose, uncertain landmarks

The lever arm, and where linearization breaks

🎯 Goal: propagate pose uncertainty to the world position of observed points, see heading uncertainty grow with range, and see when the linear approximation stops being honest.

A robot sees landmarks at known body-frame positions $p_B$. Their world positions are $p_W = Tp_B$, so, with the action Jacobian from Part 9,

\Sigma_{p_W} = J\,\Sigma_T\,J^\top + R\,\Sigma_{p_B}R^\top, \qquad J = \bigl[\,R\ \ \ R\,[1]_\times p_B\,\bigr]\ \ (SE(2)),\quad [1]_\times = \begin{bmatrix}0&-1\\1&0\end{bmatrix}.

The heading column is $R[1]_\times p_B$, a vector perpendicular to the landmark's direction whose length equals its range. Heading uncertainty therefore turns into sideways position uncertainty that grows linearly with distance: $1^\circ$ of heading error is $1.7$ cm at 1 m and $17$ cm at 10 m. That is why far landmarks constrain heading so well and localize themselves so poorly. For small heading noise the linear ellipses (blue) match the Monte Carlo samples. For large noise the samples bend into arcs the ellipses cannot follow. That is the regime where one linearization is no longer enough, and iterated or sampling-based methods earn their cost.

The robot at the left with its heading uncertainty cone. Blue ellipses: first-order 2σ covariance of each landmark. Gray: Monte Carlo samples.

Heading σ (deg)

Position σ

The error-state filter in one screen

Kalman filtering on a group

An extended Kalman filter on a Lie group keeps a mean $\bar X$ on the group and a covariance $P$ of the tangent error $\varepsilon$. Prediction uses the motion rule from section 2. The update linearizes the measurement in the tangent space and retracts:

H = \frac{\partial h(\bar X\oplus\varepsilon)}{\partial\varepsilon},\quad K = PH^\top(HPH^\top + R)^{-1},\quad \bar X \leftarrow \bar X\oplus K\bigl(z - h(\bar X)\bigr),\quad P\leftarrow (I - KH)\,P.

This is the "error-state" or "multiplicative" EKF used in nearly every attitude and visual-inertial estimator. The state is never represented by Euler angles, the update never leaves the group, and $P$ always has the group's dimension. One subtlety: after the retraction, the error is expressed at the new mean, and a careful filter transports $P$ with the Jacobian of that reset. For small corrections that Jacobian is close to $I$.

Check your understanding

0/5 answered