A Micro Lie Theory for State Estimation
+ and − on curved state spaces, so every estimator you already know keeps working — with + swapped for a curved-space equivalent. The paper strips Lie theory down to exactly that working set (“micro”), and this page walks through it.
- Why vector math breaks (S¹ example)
- Lie groups: manifold + group
- Tangent spaces and the Lie algebra
- Exp and Log: the wrap/unwrap maps
- Plus and minus: the new + and −
- The adjoint: local ↔ global
- Jacobians on manifolds
- Uncertainty and covariance
- Discrete integration (gyros)
- Estimation: ESKF and graph optimization
- The big picture & cheat sheet
1 · Why vector-space math breaks on rotations
Motivation. Before touching any theory, look at the smallest rotation group there is: the circle. If the naive operations fail here — where you can see the failure — they will fail in 3D where you can't.
A 2D orientation is a point \(z\) on the unit circle \(S^1\) (a unit complex number). Say your compass reads \(350^\circ\) and \(10^\circ\) and you want the mean heading. The arithmetic mean gives \((350+10)/2 = 180^\circ\) — pointing exactly the wrong way. The true mean, \(0^\circ\), is the midpoint of the short arc, not of the angle readings.
The failure generalizes: a 3D orientation is a \(3\times3\) matrix with 9 entries constrained by \(R^\top R = I\). What is the mean of two such matrices? The covariance of a 9-entry vector that really has 3 degrees of freedom? The derivative of \(Rp\) with respect to \(R\)? None of these questions has an answer inside linear algebra — which is exactly the gap Lie theory fills.
DEMO 1 Circular mean vs arithmetic mean (drag the sliders)
Push A and B to opposite sides (e.g. \(120^\circ\)/\(300^\circ\)): the two unit vectors cancel and even the circular mean becomes undefined — the manifold keeps you honest about what is and isn't knowable.
+, −, means, covariances and derivatives; on curved state spaces these must be re-invented — that re-invention is the whole paper.2 · Lie groups: a smooth manifold that is also a group
Motivation. We need two properties at once. The manifold property encodes “my state satisfies constraints” (unit norm, orthogonality) and gives us a flat place to do calculus. The group property encodes “states compose” (pose \(A\) followed by pose \(B\)) and gives inverses and an origin. Neither alone is enough.
A smooth manifold \(\M\) is a curved but edge- and spike-free surface embedded in a higher-dimensional space: locally it looks like a flat \(\mathbb{R}^m\) — the unit sphere, the set of orthogonal matrices. The dimension \(m\) is the number of degrees of freedom of the state. A group \((\G, \circ)\) is a set with a composition \(\circ\) satisfying, for \(X, Y, Z \in \G\):
$$ \begin{aligned} \text{Closure:} &&& X \circ Y \in \G \\ \text{Identity } E: &&& E \circ X = X \circ E = X \\ \text{Inverse } X^{-1}: &&& X^{-1} \circ X = X \circ X^{-1} = E \\ \text{Associativity:} &&& (X \circ Y) \circ Z = X \circ (Y \circ Z) \end{aligned} \eqtag{(1–4)} $$A Lie group is a set that is both. In robotics the composition law is matrix (or quaternion/complex) multiplication, and each axiom is something you already rely on: closure — composing two poses gives a pose; inverse — \(X^{-1}\) is the relative pose “from the body back to the world”; identity — the origin frame.
Each group also acts on other objects: \(X \cdot v\) rotates a vector (\(\SO(3)\)), or moves a point (\(\SE(3)\): \(X\cdot p = Rp + t\)). Estimation residuals are always built from actions, e.g. “where should the beacon appear in my camera” is \(X^{-1}\cdot b\).
| group \(\M, \circ\) | element \(X\) | constraint | dim \(m\) | action \(X\cdot v\) |
|---|---|---|---|---|
| \(\mathbb{R}^n, +\) | \(v \in \mathbb{R}^n\) | — (trivial) | \(n\) | \(v + x\) |
| \(S^1, \cdot\) | unit complex \(z\) | \(z^*z = 1\) | 1 | \(zx\) (rotate in plane) |
| \(\SO(2), \cdot\) | \(2\times2\) rotation matrix | \(R^\top R = I\) | 1 | \(Rx\) |
| \(S^3, \cdot\) | unit quaternion \(q\) | \(q^*q = 1\) | 3 | \(qxq^*\) |
| \(\SO(3), \cdot\) | \(3\times3\) rotation matrix | \(R^\top R = I\) | 3 | \(Rx\) |
| \(\SE(3), \cdot\) | \(\begin{bmatrix}R & t\\ 0 & 1\end{bmatrix}\) | \(R^\top R = I\) | 6 | \(Rp + t\) |
3 · Tangent spaces and the Lie algebra
Motivation. If the state lives on a curved surface, where do velocities, perturbations and uncertainties live? They can't live on the surface — a velocity is not an orientation. The manifold's smoothness hands us the answer: at every point \(X\) there is a flat tangent space \(\TR{X}\), a vector space of the same dimension \(m\) as the manifold, hugging the surface at \(X\).
Concretely: let \(X(t)\) be any smooth path on \(\M\) through \(X\). Its velocity \(\dot{X} = \partial X/\partial t\) lies in \(\TR{X}\) — a velocity is a direction you could leave \(X\) in, and all such directions form the tangent plane.
3.1 · The Lie algebra \(\mf = T_E\M\)
Motivation. Every point's tangent space is a different plane, but — because the group looks the same everywhere — they are all copies of one flat space. It pays to nominate one representative: the tangent space at the identity \(E\). That space is the Lie algebra, noted \(\mf\). All calculus (derivatives, covariances) will ultimately be done in \(\mf\) or \(\mathbb{R}^m\), then carried to wherever it's needed.
$$ \mf \;,=\; T_E\M \eqtag{(8)} $$Why does \(\mf\) have the structure it has? Differentiate the group constraint at the identity. For \(\SO(3)\), the constraint \(R^\top R = I\) gives \(R^\top\dot{R} + \dot{R}^\top R = 0\), i.e. \(R^\top\dot R\) equals the negative of its own transpose — it is skew-symmetric:
$$ \sk{\omega} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \in \mathfrak{so}(3), \qquad \dot{R} = R\,\sk{\omega}. $$Skew matrices are 3-dimensional, so \(m = 3\): the 9 entries of \(R\) really carry 3 DoF, as promised. Any \(\sk{\omega}\) is a linear combination \(\omega_x E_x + \omega_y E_y + \omega_z E_z\) of three generator matrices, so \(\mathfrak{so}(3)\) is secretly just \(\mathbb{R}^3\). We make this identification with two mutually inverse linear maps (the paper's Figs. 5–6):
$$ \text{hat}: \quad \mathbb{R}^3 \to \mathfrak{so}(3); \quad \omega \mapsto \omega^\wedge = \sk{\omega} \qquad\quad \text{vee}: \quad \mathfrak{so}(3) \to \mathbb{R}^3; \quad \sk{\omega} \mapsto \omega^\vee = \omega \eqtag{(10–11)} $$From here on the paper (and this page) works with the Cartesian representatives \(\tau \in \mathbb{R}^m\) rather than with \(\tau^\wedge \in \mf\): vectors stack into state vectors, and all the operators below (\(\oplus, \ominus, \Ad\), Jacobians, covariances) are ordinary matrices over \(\mathbb{R}^m\). Nothing is lost — \(\mf \cong \mathbb{R}^m\).
4 · Exp and Log: wrapping the tangent onto the manifold
Motivation. We now have flat increments \(\tau\) in the tangent space and curved states \(X\) on the manifold. We need a map between them that is exact (not a linearization), so that a small tangent increment is an honest state update. It comes free from the group's own kinematics.
Take the velocity relation \(\dot X = X\,v^\wedge\) with constant body velocity \(v\) (for \(\SO(3)\): constant angular velocity). This is a linear ODE, and linear ODEs are solved by the matrix exponential:
$$ X(t) = X(0)\,\exp(v^\wedge t) \qquad\text{with}\qquad \exp(\tau^\wedge) \;=\; E + \tau^\wedge + \tfrac{1}{2!}{\tau^\wedge}^2 + \tfrac{1}{3!}{\tau^\wedge}^3 + \cdots \eqtag{(13, 16)} $$Geometrically, \(\exp\) wraps the straight tangent vector around the manifold along the geodesic — like winding a string around a ball (the paper's Fig. 1). Its inverse \(\log\) unwraps. Because the map is exact, every group element has a unique-ish tangent representation: \(X = \exp(\Log(X)^\wedge)\).
4.1 · Worked derivation: Rodrigues' formula for \(\SO(3)\)
Motivation. The infinite series becomes useful only when it collapses to a closed form. For \(\SO(3)\) the collapse is a beautiful one-liner worth seeing once.
Write \(\tau = \theta u\) with unit axis \(u\) and angle \(\theta\). The powers of \(\sk{u}\) cycle, using \(\sk{u}\,\sk{u} = uu^\top - I\) (expand and use \(u^\top u = 1\)):
$$ \sk{u}^0 = I,\quad \sk{u}^1 = \sk{u},\quad \sk{u}^2 = uu^\top - I,\quad \sk{u}^3 = -\sk{u},\quad \sk{u}^4 = -\sk{u}^2,\ \ldots $$so the series splits into even terms (multiples of \(I\), forming the cosine series) and odd terms (multiples of \(\sk u\), forming the sine series):
$$ \boxed{\;R = \Exp(\theta u) = I + \sin\theta\,\sk{u} + (1-\cos\theta)\,\sk{u}^2\;} \eqtag{\text{Rodrigues}} $$The same trick (with a factor 2 from the double cover, since \(q\) and \(-q\) are the same rotation) gives the unit quaternion \(q = \cos(\theta/2) + u\sin(\theta/2)\). For \(\SE(n)\) a translation block appears, handled by a matrix \(V(\theta)\) — see the cheat sheet in §11.
4.2 · Capitalized \(\Exp\) / \(\Log\): straight to \(\mathbb{R}^m\)
Motivation. We decided to live in \(\mathbb{R}^m\), so compose hat with exp into one shortcut (paper eqs. 21–24):
$$ \Exp : \mathbb{R}^m \to \M; \quad \tau \mapsto X = \Exp(\tau) \,=\, \exp(\tau^\wedge) \qquad\qquad \Log : \M \to \mathbb{R}^m; \quad X \mapsto \tau = \Log(X) \,=\, \log(X)^\vee $$Three properties of \(\exp\) do heavy lifting later. The first two say the exponential is a homomorphism along a line; the third is the conjugation rule — verifiable by expanding the series and watching all the \(X^{-1}X\) pairs cancel to 1:
$$ \exp((t+s)\tau^\wedge) = \exp(t\tau^\wedge)\exp(s\tau^\wedge), \qquad \exp(-\tau^\wedge) = \exp(\tau^\wedge)^{-1}, \qquad \exp\!\left(X\tau^\wedge X^{-1}\right) = X\exp(\tau^\wedge)X^{-1} \eqtag{(17, 19, 20)} $$5 · Plus and minus: the replacement for + and −
Motivation. With \(\Exp\), \(\Log\) and \(\circ\) we can finally define the operators the whole estimation machinery runs on. The design goal: make \(X \oplus \tau\) behave like \(x + \delta\) does in vector spaces — go from a state to a nearby state, and back.
$$ \text{right-}\oplus: \;\; Y = X \oplus {}^{X}\tau \;,=\; X \circ \Exp({}^{X}\tau) \qquad\qquad \text{right-}\ominus: \;\; {}^{X}\tau = Y \ominus X \;,=\; \Log\!\left(X^{-1} \circ Y\right) \eqtag{(25, 26)} $$Read \(\ominus\) as “the increment that takes \(X\) to \(Y\)”, expressed in the tangent space at \(X\). Because composition is not commutative, there are also left versions, \({}^{E}\tau \oplus X = \Exp({}^{E}\tau)\circ X\) and \({}^{E}\tau = Y \ominus X = \Log(Y \circ X^{-1})\), where the increment lives at the identity instead. The superscript keeps track of the reference frame: \({}^{X}\tau\) is a local increment (expressed in \(X\)'s own frame), \({}^{E}\tau\) is a global increment (expressed in the origin frame). The paper's default — and ours — is right/local.
6 · The adjoint: carrying increments between frames
Motivation. Sensors and filters disagree about where “here” is: a gyro reports increments in the body frame (local), a SLAM optimizer may want them in the world frame (global). Since both tangent spaces are copies of \(\mathbb{R}^m\), the transfer must be a linear map — the adjoint.
Composing the same two increments in the two orders must land on the same point: \(\Exp({}^{E}\tau)\circ X = X \circ \Exp({}^{X}\tau)\). Apply the conjugation rule (20) on the left and match exponents:
$$ \exp({}^{E}\tau^\wedge) = X\exp({}^{X}\tau^\wedge)X^{-1} = \exp\!\left(X\,{}^{X}\tau^\wedge X^{-1}\right) \;\;\Longrightarrow\;\; {}^{E}\tau^\wedge = X\,{}^{X}\tau^\wedge X^{-1} $$Definition and matrix form (vee of both sides), for \(\tau \in \mathbb{R}^m\):
$$ \Ad_X : \mf \to \mf; \quad \Ad_X(\tau^\wedge) = X\tau^\wedge X^{-1} \qquad\Longrightarrow\qquad {}^{E}\tau = \Ad_X\,{}^{X}\tau, \qquad \Ad_X\,\tau = \left(X\,\tau^\wedge X^{-1}\right)^\vee \eqtag{(29–31)} $$Two cases you will use constantly:
$$ \SO(3):\quad \Ad_R = R \qquad\qquad \SE(3):\quad \Ad_{M} = \begin{bmatrix} R & \sk{t}R \\ 0 & R \end{bmatrix} \eqtag{(139, 175)} $$The \(\SO(3)\) case is a one-liner: \(R\sk{\theta}R^{-1} = \sk{R\theta}\) (rotating a skew matrix rotates its axis), and vee-ing gives \(R\theta\). For \(\SE(3)\), expanding \(M\tau^\wedge M^{-1}\) and using \(\sk{a}\,\sk{b} - \sk{b}\,\sk{a} = \sk{a \times b}\) produces the coupling block \(\sk t R\): in pose increments, translation and rotation leak into each other when you change frames — exactly the cross-term you'd expect. Three properties to remember (paper eqs. 32–34):
$$ X \oplus \tau = (\Ad_X\,\tau) \oplus X, \qquad \Ad_{X^{-1}} = \Ad_X^{-1}, \qquad \Ad_{X\circ Y} = \Ad_X\,\Ad_Y \eqtag{(32–34)} $$The first is the local-to-global dictionary as a formula; the last says adjoints compose like the states do.
7 · Jacobians on manifolds
Motivation. An EKF needs \(\partial(\text{measurement})/\partial(\text{state})\); Gauss–Newton needs \(\partial(\text{residual})/\partial(\text{state})\); covariance propagation needs \(\partial(\text{next state})/\partial(\text{current state})\). The catch: “\(\partial f/\partial X\)” is not defined when \(X\) is a rotation matrix — you can't subtract two rotations. The fix is to write the derivative exactly as in vector spaces, but with all variations expressed in tangent spaces via \(\oplus\) and \(\ominus\).
7.1 · The right Jacobian of a function
Replace \(+\) by \(\oplus\) and \(−\) by \(\ominus\) in the standard limit definition. For \(f: \M \to \mathcal{N}\):
$$ \frac{{}^{X}\D f(X)}{\D X} \;,=\; \lim_{\tau \to 0} \frac{f\left(X \oplus {}^{X}\tau\right) \ominus f(X)}{\tau} \;=\; \lim_{\tau \to 0} \frac{\Log\!\left(f(X)^{-1}\circ f(X\circ\Exp(\tau))\right)}{\tau} \;\in\; \mathbb{R}^{n\times m} \eqtag{(41)} $$Unpack it once, slowly. The numerator perturbs the state along tangent direction \(\tau\), runs \(f\), and measures the result's change as a tangent vector at \(f(X)\) — both \(\ominus\) operations are exact. Dividing the vector \(\Log(\cdot) \in \mathbb{R}^n\) by the scalar magnitude of \(\tau\) and taking the limit is the ordinary directional derivative. What you get is a plain \(n \times m\) matrix \(J\) mapping local tangents at \(X\) to local tangents at \(f(X)\), satisfying the familiar linearized prediction:
$$ f(X \oplus {}^{X}\tau) \;\longrightarrow\; f(X) \oplus \frac{{}^{X}\D f(X)}{\D X}\,\tau \qquad (\tau \to 0) \eqtag{(43)} $$And because the definition mirrors the vector-space one, the chain rule reads exactly as always — for \(Z = g(f(X))\):
$$ \frac{{}^{X}\D Z}{\D X} = \frac{{}^{Y}\D Z}{\D Y}\;\frac{{}^{X}\D Y}{\D X} \eqtag{(58)} $$7.2 · The elementary blocks (build everything else from these)
Motivation. Any estimator function is a composition of a handful of primitives: inverse, composition, \(\Exp\), and the group action. Compute each primitive's Jacobian once, chain them forever after. Each one falls out of the limit definition (41); here is one worked in full, the rotation action on a point — because it's the residual of half the registration problems in robotics:
$$ \frac{{}^{R}\D(R\,p)}{\D R} = \lim_{\theta \to 0} \frac{(R \oplus \theta)\,p \ominus R\,p}{\theta} = \lim_{\theta \to 0} \frac{R\Exp(\theta)p - Rp}{\theta} = \lim_{\theta \to 0} \frac{R\sk{\theta}p}{\theta} = -R\sk{p} \eqtag{(150)} $$using \(\Exp(\theta) \approx I + \sk\theta\) for small \(\theta\) and \(\sk{\theta}p = -\sk p\,\theta\). The full block library:
| function \(f\) | Jacobian | formula | paper |
|---|---|---|---|
| inverse \(X^{-1}\) | \(\dfrac{{}^{X}\D X^{-1}}{\D X}\) | \(-\Ad_X\) | (62) |
| composition in \(X\): \(X \circ Y\) | \(\dfrac{{}^{X}\D(X\circ Y)}{\D X}\) | \(\Ad_{Y^{-1}}\) | (65) |
| composition in \(Y\): \(X \circ Y\) | \(\dfrac{{}^{Y}\D(X\circ Y)}{\D Y}\) | \(I\) | (66) |
| exponential \(\Exp(\tau)\) | \(\Jr(\tau) = \dfrac{\D\Exp(\tau)}{\D\tau}\) | closed form below | (67) |
| Log map \(\Log(X)\) | \(\dfrac{\D\Log(X)}{\D X}\) | \(\Jr^{-1}(\tau),\ \tau=\Log X\) | (79) |
| action on points: \(R\,p\) | \(\dfrac{{}^{R}\D(Rp)}{\D R}\ /\ \dfrac{\D(Rp)}{\D p}\) | \(-R\sk{p}\ \ /\ \ R\) | (150–151) |
The only nontrivial block is the Jacobian of the exponential map itself, \(\Jr(\tau)\) — “how does the wrapped point move when I bend the tangent vector?”. For \(\SO(3)\) it has the closed form (with \(\Jl = \Jr^{\top}\), and both \(\to I\) as \(\tau \to 0\)):
$$ \Jr(\theta) = I - \frac{1-\cos\theta}{\theta^2}\sk{\theta} + \frac{\theta - \sin\theta}{\theta^3}\sk{\theta}^2 \eqtag{(143)} $$ $$ \Jr^{-1}(\theta) = I + \frac{1}{2}\sk{\theta} + \left(\frac{1}{\theta^2} - \frac{1}{2\theta}\cot\frac{\theta}{2}\right)\sk{\theta}^2 \eqtag{(144)} $$It is used through three approximation rules — worth memorizing, they appear in every derivation (EKF prediction, IMU preintegration, covariances):
$$ \Exp(\tau + \delta\tau) \approx \Exp(\tau)\Exp\!\left(\Jr(\tau)\,\delta\tau\right), \qquad \Log\!\left(\Exp(\tau)\Exp(\delta\tau)\right) \approx \tau + \Jr^{-1}(\tau)\,\delta\tau \eqtag{(68, 70)} $$ $$ \Ad_{\Exp(\tau)} = \Jl(\tau)\,\Jr^{-1}(\tau) \eqtag{(75)} $$
8 · Uncertainty: Gaussians that live in the tangent space
Motivation. A Kalman filter is a mean and a covariance. On a manifold, the naive definitions collapse: \(X - \bar X\) doesn't exist, and \(E[(X-\bar X)(X-\bar X)^\top]\) would be a \(9\times 9\) (for \(\SO(3)\)) matrix for a 3-DoF object — always singular, always ill-defined (the paper's footnote 5). The fix uses exactly the pieces we've built: keep the mean on the manifold, and the uncertainty in the tangent space.
Define the local perturbation and its covariance (paper eqs. 51–52):
$$ X = \bar{X} \oplus {}^{\bar X}\tau, \qquad \Sigma_X \;,=\; E\!\left[{}^{\bar X}\tau\,{}^{\bar X}\tau^{\top}\right] \in \mathbb{R}^{m \times m}, \qquad X \sim \mathcal{N}_{\M}\!\left(\bar{X}, \Sigma_X\right) $$The mean \(\bar X\) is a genuine manifold element; \(\Sigma_X\) is an honest \(m\times m\) PSD matrix. Because \(\Sigma\) describes a tangent vector, its frame matters: a global (at-\(E\)) covariance converts to local by the adjoint — the same \(\Ad\) as in §6, now acting on covariances:
$$ {}^{E}\Sigma_X = \Ad_X\; {}^{X}\Sigma_X\; \Ad_X^{\top} \eqtag{(54)} $$The paper's example: “3D orientation known up to rotations in the horizontal plane” is a statement about a global covariance \({}^{E}\Sigma = \mathrm{diag}(\sigma_\phi^2, \sigma_\theta^2, \infty)\) — expressible only at \(E\), then pulled to wherever the state is.
How does a tangent Gaussian look on the manifold? Push the distribution through \(\oplus\): sample \(\tau \sim \mathcal N(0,\Sigma)\) in the tangent plane and wrap with \(\Exp\).
Propagating uncertainty through \(Y = f(X)\) needs only the linearized prediction (43): push the perturbation through \(J\), then take expectations:
$$ \Sigma_Y \;\approx\; \frac{{}^{X}\D f}{\D X}\;\Sigma_X\;\left(\frac{{}^{X}\D f}{\D X}\right)^{\!\top} \eqtag{(55)} $$— the familiar \(J\Sigma J^\top\), with \(J\) from §7. The mean of samples is found the same way a vector mean is, by iteration: start from any sample, compute the average correction \(\bar\tau = \frac{1}{N}\sum_i (X_i \ominus \bar X)\), apply \(\bar X \leftarrow \bar X \oplus \bar\tau\), repeat. It converges in a few steps because the corrections live in one flat tangent space.
9 · Discrete integration: where your gyro code comes from
Motivation. Continuous motion \(X(t) = X_0 \circ \Exp(vt)\) assumes constant velocity. Real IMUs deliver rates in chunks — integration must be discrete, and the \(\oplus\) formalism makes the discretization exact per-step rather than an approximation of some grand addition.
Segment the motion into piecewise-constant velocities \(v_k\) over \(\delta t_k\); each chunk is a tangent increment \(\tau_k = v_k\,\delta t_k\) expressed in the frame where the rate is measured (the body, for a gyro). Then compose (paper eq. 56):
$$ X_k = X_{k-1} \oplus \tau_k = X_{k-1} \circ \Exp(\tau_k) \qquad\Longrightarrow\qquad R_k = R_{k-1}\Exp(\omega_k\,\delta t) $$— which is precisely the quaternion or matrix update line in every IMU pipeline, now derived rather than ad-hoc. Because each \(\Exp\) is exact, the only errors are sensor errors (bias, noise) — not discretization of the group structure. This is also the backbone of on-manifold IMU preintegration (Forster et al.), where many chunks are composed into one delta with its combined covariance.
10 · Estimation on manifolds: ESKF and graph optimization
Motivation. Everything so far assembles into the two workhorses of robotic state estimation. The punchline of this section is how little changes: the algorithms keep their vector-space shape; only + becomes \(\oplus\), and every Jacobian comes from the §7 block table.
10.1 · Error-state Kalman filter (paper §V-A)
Setup: a planar robot with pose \(X \in \SE(2) = \begin{bmatrix} R & t \\ 0 & 1\end{bmatrix}\), beacons \(b_k \in \mathbb R^2\), noisy control \(u \in \mathbb R^3 \cong \mathfrak{se}(2)\) (linear + angular velocity over \(\delta t\)), and Cartesian beacon measurements \(y = X^{-1}\cdot b + n\). The error state and covariance are defined in the tangent space at the estimate:
$$ \delta x \;,=\; X \ominus \hat{X} \in \mathbb R^3, \qquad P = E[\delta x\, \delta x^\top] \in \mathbb R^{3\times3} \eqtag{(97–98)} $$Predict (control \(u_j\) arrives): apply \(\oplus\), then propagate \(P\) with the §7 blocks — the state-transition matrix is the composition Jacobian \(F = \Ad_{\Exp(u)}^{-1}\) and the noise enters through \(G = \Jr(u)\):
$$ \hat{X}_j = \hat{X}_i \oplus u_j, \qquad P_j = F\,P_i\,F^\top + G\,W\,G^\top \eqtag{(99–100)} $$Correct (measurement \(y_k\) arrives): standard EKF algebra verbatim; the only Lie-theory object is the measurement Jacobian \(H = {}^{X}\D(X^{-1}\cdot b)/\D X = -\left[\,I \;\; R J_2 (b - t)\,\right]\) (with \(J_2 = \begin{bmatrix}0&-1\\1&0\end{bmatrix}\)), itself the action Jacobian (150) chained with the inverse Jacobian (62):
$$ z = y_k - \hat{X}^{-1}\!\cdot b, \qquad Z = HPH^\top + N, \qquad K = PH^\top Z^{-1} \eqtag{(101)} $$ $$ \delta x = Kz, \qquad \hat{X} \leftarrow \hat{X} \oplus \delta x, \qquad P \leftarrow P - KZK^\top \eqtag{(102)} $$Notice what did not change: the Kalman gain, the innovation covariance, the covariance update — all identical to the textbook filter. The paper's summary is exact: “the only changes with respect to a regular EKF are in (99) and (101), where regular \(+\) are substituted by \(\oplus\)”.
Worked correction step — all numbers
Prior: \(\hat X = \big(\theta{=}30^\circ,\ t{=}(1,2)^\top\big)\), \(P = \mathrm{diag}(0.09,\,0.04,\,0.04)\) (i.e. \(\sigma_x{=}0.3\) m, \(\sigma_y{=}0.2\) m, \(\sigma_\theta{=}0.2\) rad). Beacon \(b = (4,3)^\top\). Measurement \(y = (3.198,\ -0.684)^\top\), \(N = \mathrm{diag}(0.01, 0.01)\).
| innovation | \(z = y - \hat X^{-1}\cdot b = (0.100,\ -0.050)^\top\) |
| meas. Jacobian | \(H = -\left[I \;\ R J_2(b-t)\right] = \begin{bmatrix}-1 & 0 & 2.366\\ 0 & -1 & -2.098\end{bmatrix}\) |
| Kalman gain | \(K = PH^\top(HPH^\top + N)^{-1} = \begin{bmatrix}-0.602 & -0.529\\ -0.235 & -0.383\\ 0.140 & -0.248\end{bmatrix}\) |
| optimal correction | \(\delta x = Kz = (-0.0338,\ -0.0043,\ 0.0264)^\top\ \ (\text{last entry in rad})\) |
| update | \(\hat X \leftarrow \hat X \oplus \delta x:\ \ \theta = 31.51^\circ,\ \ t = (0.966,\ 1.995)^\top\) |
| covariance | \(P \leftarrow P - KZK^\top = \begin{bmatrix}0.036 & -0.021 & 0.013\\ -0.021 & 0.025 & -0.010\\ 0.013 & -0.010 & 0.006\end{bmatrix}\) |
One range-free bearing-only sighting of one beacon pulls the heading estimate by \(+1.5^\circ\) and cuts \(\sigma_\theta\) from 0.20 to 0.077 rad. Nothing in this table needed an angle-wrap special case — \(\oplus\) handled the geometry.
10.2 · Graph-based smoothing and mapping (paper §V-B)
Motivation. Filters are causal and local; smoothers re-estimate the whole trajectory from all measurements. The state is now a heterogeneous stack — poses and landmark positions — and this is where the theory's second payoff appears: a bundle of manifolds is itself a manifold, block by block.
The composite manifold \(\M = \langle \M_1, \ldots, \M_M\rangle\) simply concatenates everything: identity, inverse, composition and \(\Exp^{\langle\rangle}\) act block-wise, so \(\oplus\) acts block-wise and Jacobians are block matrices of the §7 blocks (paper eqs. 85–89). The total squared error is a sum of residual squared norms, each whitened by an information matrix \(\Omega\):
$$ r_{ij}(X) = \Omega_{ij}^{1/2}\left(u_{ij} \ominus (\hat X_j \ominus \hat X_i)\right), \qquad r_{ik}(X) = \Omega_{ik}^{1/2}\left(y_{ik} \ominus (\hat X_i^{-1}\cdot \hat b_k)\right) \eqtag{(104–105)} $$Gauss–Newton on a manifold is then word for word the vector-space algorithm, with the linearization from (90):
$$ r_p(X \oplus \delta x) \approx r_p(X) + J_p\,\delta x \quad\Longrightarrow\quad \delta x^\star = -\left(J^\top J\right)^{-1} J^\top r, \qquad X \leftarrow X \oplus \delta x^\star \eqtag{(108–110)} $$Iterate to convergence. Sparsity of \(J\) comes free from the block structure (each factor touches only its own two nodes — the block pattern of the paper's eq. 107). Adding self-calibration (an unknown control bias \(c\)) means appending one more block \(c \in \mathbb R^2\) to the composite and one more column of Jacobian blocks — nothing else in the solver moves (paper §V-C). And going 2D → 3D is \(\SE(2) \to \SE(3)\), \(m{:}\ 3 \to 6\): every equation above is unchanged, which is the entire point of the abstraction.
DEMO 2 On-manifold Gauss–Newton: Wahba point-cloud alignment
Estimate the rotation \(R\) minimizing \(\sum_i \|q_i - R p_i\|^2\) — the residual of §7.2's action Jacobian in the loop (eq. 150). Six points \(p_i\) (blue in your mind's eye) were rotated by a hidden truth \(R^\star=\Exp(0.9,-0.7,0.5)\) (a \(70.9^\circ\) turn) and jittered by noise. Start far away: \(\hat R_0 = \Exp(2.8,\ 1.9,\ -2.4)\). Step the solver and watch the Jacobian block do the work — then uncheck it and watch the solver stall, exactly as in Fig. 8.
cost per iteration (log scale). With the correct \(J\): quadratic convergence, ~5 steps to the noise floor. With \(J = I\): the direction is wrong and the cost stalls around 10–15.
+ becomes \(\oplus\), Jacobians come from the block table.11 · The big picture, and the cheat sheet
What the “micro” leaves out, and why that's OK. Full Lie theory continues with the Lie bracket (a product on \(\mf\)), the Baker–Campbell–Hausdorff formula (exact composition of exponentials), and the deep fact that the algebra nearly determines the group. The paper deliberately relegates all of it: for estimation you compose, differentiate and propagate — and every one of those operations is carried out on \(\mathbb R^m \cong \mf\) with the \(\oplus/\ominus\) operators. What you give up (closed-form composition of large rotations, i.e. BCH) you rarely need: you iterate instead. When you do need the bigger canvas — continuous-time trajectory estimation on \(\mathrm{SE}_k(3)\), Lie groups of higher symmetry — Barfoot's State Estimation for Robotics and Chirikjian's volume II pick up exactly where this paper stops.
Where you've already met this paper. The operators here are the vocabulary of modern SLAM stacks: GTSAM's Pose3::Expmap and its \(\oplus\)-retraction optimizers, on-manifold IMU preintegration (Forster et al.), and the companion C++ library manif, which implements every formula on this page with analytic Jacobians. The quaternion tutorial of Solà (2017) is the deep-dive companion for the \(S^3\) column.
11.1 · Notation
| \(X, Y \in \M\) | group elements (poses, orientations); \(E\): identity |
| \(\TR{X},\ \mf = T_E\M\) | tangent space at \(X\); Lie algebra (tangent at identity) |
| \(\tau \in \mathbb R^m\) | tangent increment in Cartesian form; \(\tau^\wedge \in \mf\) its hat image |
| \(\Exp, \Log\) | \(\mathbb R^m \leftrightarrow \M\) wrap/unwrap (geodesic) maps |
| \(X \oplus \tau,\ \tau = Y \ominus X\) | right (local) plus/minus; left versions use \(\Exp(\tau)\circ X\) |
| \(\Ad_X\) | adjoint matrix: local ↔ global tangent change of frame |
| \(\Jr, \Jl\) | right / left Jacobians of \(\Exp\); \(\Jl = \Jr^\top\), \(\Jl(\tau) = \Ad_{\Exp(\tau)}\Jr(\tau)\) |
| \(\Sigma,\ P,\ \Omega\) | covariance (tangent space), filter covariance, information matrix |
11.2 · \(\SO(3)\) quick reference
| \(\Exp(\theta)\) | \(I + \frac{\sin\theta}{\theta}\sk\theta + \frac{1-\cos\theta}{\theta^2}\sk\theta^2\) (Rodrigues) |
| \(\Log(R)\) | \(\theta = \frac{1}{2\sin\theta}\left(R - R^\top\right)^\vee\), \(\ \theta = \cos^{-1}\frac{\mathrm{tr}R - 1}{2}\) (handle \(\theta \to 0, \pi\)) |
| \(\Ad_R\) | \(R\) |
| \(\Jr(\theta)\) | \(I - \frac{1-\cos\theta}{\theta^2}\sk\theta + \frac{\theta-\sin\theta}{\theta^3}\sk\theta^2\); \(\ \Jl = \Jr^\top\) |
| \(J\) of \(Rp\) | \(\frac{{}^{R}\D(Rp)}{\D R} = -R\sk p\), \(\quad \frac{\D(Rp)}{\D p} = R\) |
| \(J\) of \(R^{-1}\), \(R_1 R_2\) | \(-R\), \(\quad \Ad_{R_2^{-1}} = R_2^\top\) |
11.3 · \(\SE(3)\) quick reference (\(\tau = (\rho, \theta) \in \mathbb R^6\))
| \(\Exp(\tau)\) | \(\begin{bmatrix} \Exp(\theta) & V(\theta)\rho \\ 0 & 1\end{bmatrix}\), \(\quad V(\theta) = I + \frac{1-\cos\theta}{\theta^2}\sk\theta + \frac{\theta - \sin\theta}{\theta^3}\sk\theta^2\) |
| \(\Log(M)\) | \(\big(V^{-1}(\theta)\,t,\ \Log(R)\big)\) |
| \(\Ad_M\) | \(\begin{bmatrix} R & \sk t R \\ 0 & R \end{bmatrix}\) |
| \(\Jl(\rho,\theta)\) | block matrix built on \(\Jl(\theta)\) and \(Q(\rho,\theta)\) — paper eqs. (179–180); \(\Jr(\rho,\theta) = \Jl(-\rho,-\theta)\) |
| \(J\) of \(M\cdot p\) | \(\frac{{}^{M}\D(Mp)}{\D M} = \left[\,R \;\; -R\sk p\,\right]\), \(\quad \frac{\D(Mp)}{\D p} = R\) |
| \(J\) of \(M^{-1}\), \(M_1M_2\) | \(-\Ad_M\), \(\quad \Ad_{M_2^{-1}}\) |
The paper on one page
States live on Lie groups: curved (manifold: constraints, local flatness) and composable (group: \(\circ\), inverse, \(E\)). Calculus happens in tangent spaces — all copies of \(\mf \cong \mathbb R^m\). \(\Exp\) wraps tangent increments exactly onto the manifold along geodesics; \(\Log\) unwraps. From these: \(\oplus\) / \(\ominus\) replace \(+\)/\(−\) (local by default, global via the adjoint \(\Ad_X\)). Jacobians are the ordinary derivatives with tangent-space variations (41), built from blocks \(-\Ad_X\), \(\Ad_{Y^{-1}}\), \(\Jr\), \(-R\sk p\) and chained as usual. Uncertainty is a tangent-space Gaussian around a manifold mean, propagated by \(J\Sigma J^\top\) and re-framed by \(\Ad \,\Sigma\, \Ad^\top\). Integration is a chain of \(\oplus\). ESKF and graph optimizers keep their textbook form with \(+ \to \oplus\) — worked out in \(\SE(2)\) and identical in \(\SE(3)\).