ch09_pose_estimation

第九章　位姿估计应用 §

Chapter 9   Pose Estimation Problems §

---

9.1 点云对齐 / Point-Cloud Alignment §

9.1.1 问题设置 / Problem Setup §

中文

点云对齐是三维机器人感知中最基础的问题之一：给定两组来自不同参考系的三维点集合（点云），找到最佳的旋转和平移，使两组点云尽可能重合。

形式化设置：设存在固定参考系 ${\underline{\mathcal{F}}}_i$ 和附于移动机器人的参考系 ${\underline{\mathcal{F}}}_{v_k}$。对 $M$ 个地标点 $P_j$，我们拥有：

• 在固定系中的坐标 ${\mathbf{r}}_i^{p_{j}i}$（地图中已知）
• 在车体系中的含噪测量 ${\mathbf{y}}_j={\mathbf{r}}_{v_k}^{p_j{v}_k}$

目标：找旋转矩阵 ${\mathbf{C}}_{v_{k}i}$ 和平移 ${\mathbf{r}}_i^{v_{k}i}$，最小化加权最小二乘误差

$$J=\frac{1}{2}\sum _{j=1}^M{w}_j{\left({\mathbf{y}}_j-{\mathbf{C}}_{v_{k}i}\left({\mathbf{r}}_i^{p_{j}i}-{\mathbf{r}}_i^{v_{k}i}\right)\right)}^T\left({\mathbf{y}}_j-{\mathbf{C}}_{v_{k}i}\left({\mathbf{r}}_i^{p_{j}i}-{\mathbf{r}}_i^{v_{k}i}\right)\right).$$

应用场景：

• ICP（迭代最近点）算法：每次迭代中使用本节方法对齐当前扫描与地图
• RANSAC 最小子集：从最少 3 对点快速估计初始位姿

直觉：点云对齐就像”拼拼图”——你有一张局部照片（车体系测量）和一份地图（固定系坐标），要找到相机的位置与朝向。关键约束是旋转矩阵 $\mathbf{C}\in SO(3)$，这使问题非线性。

---

English

Point-cloud alignment (also called point-set registration) is fundamental to 3D perception: given two sets of 3D points expressed in different frames, find the rotation and translation that best aligns them.

Setup. There are $M$ landmark points $P_j$ with:

• Known positions ${\mathbf{p}}_j={\mathbf{r}}_i^{p_{j}i}$ in a fixed frame ${\underline{\mathcal{F}}}_i$
• Noisy measurements ${\mathbf{y}}_j={\mathbf{r}}_{v_k}^{p_j{v}_k}$ in the vehicle frame ${\underline{\mathcal{F}}}_{v_k}$

We seek the rotation ${\mathbf{C}}_{v_{k}i}\in SO(3)$ and translation ${\mathbf{r}}_i^{v_{k}i}$ minimizing the weighted least-squares cost above with scalar weights $w_j>0$.

Key applications: Iterative Closest Point (ICP), RANSAC minimal-set pose estimation.

---

9.1.2 单位四元数解 / Unit-Length Quaternion Solution §

中文

四元数方法将点云对齐化为一个特征值问题。

化简步骤：用简化记号，关系式 ${\mathbf{y}}_j={\mathbf{C}}_{v_{k}i}({\mathbf{p}}_j-\mathbf{r})$ 用四元数重写为

$${\mathbf{y}}_j={\mathbf{q}}^{-1^{+}}({\mathbf{p}}_j-\mathbf{r}{)}^{+}\mathbf{q},$$

其中 ${\mathbf{q}}^{+}$ 和 ${\mathbf{q}}^{\oplus }$ 是四元数左/右乘矩阵（见第 7 章）。经过变换，误差线性化后，目标函数化为

$$J(\mathbf{q},\mathbf{r},\lambda )=\frac{1}{2}\sum _{j=1}^M{w}_j{\mathbf{q}}^T{\left({\mathbf{y}}_j^{\oplus }-({\mathbf{p}}_j-\mathbf{r}{)}^{+}\right)}^T\left({\mathbf{y}}_j^{\oplus }-({\mathbf{p}}_j-\mathbf{r}{)}^{+}\right)\mathbf{q}-\frac{\lambda }{2}({\mathbf{q}}^T\mathbf{q}-1).$$

求解：

1. 对平移 $\mathbf{r}$ 求导置零，得到最优平移等于两点云质心之差：

$$\hat{\mathbf{r}}=\bar{\mathbf{p}}-{\mathbf{q}}^{+}{\bar{\mathbf{y}}}^{+}{\mathbf{q}}^{-1},$$

其中 $\bar{\mathbf{p}},\bar{\mathbf{y}}$ 是加权质心。

2. 将最优平移代回后，问题化为 4×4 特征值问题：

$$\mathbf{Wq}=\lambda \mathbf{q},\mathbf{W}=\frac{1}{w}\sum _{j=1}^M{w}_j{\left(({\mathbf{y}}_j-\bar{\mathbf{y}}{)}^{\oplus }-({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^{+}\right)}^T\left(({\mathbf{y}}_j-\bar{\mathbf{y}}{)}^{\oplus }-({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^{+}\right).$$

由于 $J=\lambda /2$，取最小特征值对应的特征向量即为最优旋转四元数。

直觉：旋转约束 $\Vert \mathbf{q}\Vert =1$ 通过 Lagrange 乘子引入；最小化能量就是选择与”期望旋转方向”最匹配的特征向量。这等同于从数据的协方差矩阵中提取主方向。

---

English

The quaternion approach reduces point-cloud alignment to a 4×4 eigenproblem.

Key steps:

1. Re-express the alignment constraint using quaternion multiplication matrices ${\mathbf{q}}^{+}$ and ${\mathbf{q}}^{\oplus }$ (Section 7.2.3).
2. The optimal translation is the difference between the weighted centroids of the two clouds (in the fixed frame):

$${\hat{\mathbf{r}}}_i^{v_{k}i}=\bar{\mathbf{p}}-{\hat{\mathbf{C}}}_{v_{k}i}^T\bar{\mathbf{y}}.$$

3. After substituting the optimal translation, the remaining problem reduces to the Davenport/Wahba eigenproblem:

$$\mathbf{Wq}=\lambda \mathbf{q}$$

where $\mathbf{W}$ is a $4\times 4$ symmetric positive-semidefinite matrix built from cross-covariances of the centroid-subtracted point clouds.

Solution: Since $J=\lambda /2$, minimizing the cost requires choosing the smallest eigenvalue and its corresponding eigenvector (the rotation quaternion $\hat{\mathbf{q}}$). The unit-length constraint uniquely determines the overall scale. This approach is originally due to Davenport (1965) and Horn (1987).

---

9.1.3 旋转矩阵解（Wahba 问题）/ Rotation Matrix Solution (Wahba’s Problem) §

中文

旋转矩阵方法将对齐问题化为一个 SVD（奇异值分解）问题，并给出完整的全局最小值分析。

化简：定义去心点对

$$\mathbf{W}=\frac{1}{w}\sum _{j=1}^M{w}_j({\mathbf{y}}_j-\bar{\mathbf{y}})({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^T\in {\mathbb{R}}^{3\times 3}.$$

最优旋转等价于最大化 $\mathrm{tr}(\mathbf{C}{\mathbf{W}}^T)$，约束 $\mathbf{C}\in SO(3)$。

SVD 解：对 $\mathbf{W}=\mathbf{UD}{\mathbf{V}}^T$（$\mathbf{D}=\mathrm{diag}(d_1,d_2,d_3)$，$d_1\ge d_2\ge d_3\ge 0$），当全局最小值唯一时，解为

$$\boxed{{\hat{\mathbf{C}}}_{v_{k}i}=\mathbf{US}{\mathbf{V}}^T,\mathbf{S}=\mathrm{diag}(1,1,\det \mathbf{U}\det \mathbf{V}).}$$

$\mathbf{S}$ 中的最后一个符号位确保 $\det \hat{\mathbf{C}}=1$（即确保是真旋转而非反射）。

各种情形：

条件	最小值情况
$\det \mathbf{W}>0$	唯一全局最小值，$\mathbf{S}=1$
$\det \mathbf{W}<0$ 且 $d_3$ 唯一（$d_2>d_3$）	唯一全局最小值，$\mathbf{S}=\mathrm{diag}(1,1,-1)$
$\mathrm{rank}\mathbf{W}=2$（$d_3=0$，$d_2>0$）	唯一解，$\mathbf{S}=\mathrm{diag}(1,1,\det \mathbf{U}\det \mathbf{V})$
点共线（rank 1）或共点（rank 0）	无穷多最小值

重要事实：对旋转矩阵参数化而言，无局部极小值——任何满足一阶条件的临界点要么是全局极小值，要么是极大值或鞍点。因此迭代方法从任意初始点出发都会收敛到全局最小值（只要存在唯一解）。

迭代方法（基于李群 $SO(3)$）：使用左扰动方案 $\mathbf{C}=\exp ({\mathbf{\psi }}^{\wedge }){\mathbf{C}}_{\mathrm{op}}$，每步更新

$${\mathbf{C}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{\psi }}^{\star \wedge }){\mathbf{C}}_{\mathrm{op}},$$

其中最优扰动

$${\mathbf{\psi }}^{\star }={\mathbf{C}}_{\mathrm{op}}{\mathbf{I}}^{-1}{\mathbf{C}}_{\mathrm{op}}^T\mathbf{b},\mathbf{I}=-\frac{1}{w}\sum _{j=1}^M{w}_j({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^{\wedge }({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^{\wedge }.$$

唯一性条件：需要至少三个非共线点（使 $\mathbf{I}$ 正定）。

---

English

The rotation matrix approach reduces to a singular-value decomposition and provides a complete global analysis.

Cross-covariance matrix:

$$\mathbf{W}=\frac{1}{w}\sum _{j=1}^M{w}_j({\mathbf{y}}_j-\bar{\mathbf{y}})({\mathbf{p}}_j-\bar{\mathbf{p}}{)}^T.$$

SVD solution. Compute $\mathbf{W}=\mathbf{UD}{\mathbf{V}}^T$. The unique global minimum (when it exists) is:

$$\hat{\mathbf{C}}=\mathbf{US}{\mathbf{V}}^T,\mathbf{S}=\mathrm{diag}(1,1,\det \mathbf{U}\det \mathbf{V}).$$

The last sign entry in $\mathbf{S}$ corrects for a possible reflection, ensuring $\det \hat{\mathbf{C}}=+1$. This is the classical solution to Wahba’s problem (Wahba, 1965; Horn, 1987; Arun et al., 1987).

No local minima: A second-order perturbation analysis shows that in all practically relevant cases (unique global minimum exists), there are no local minima — only one global minimum, one global maximum, and saddle points. This guarantees convergence of iterative methods.

Iterative approach (using $SO(3)$ kinematics): At each iteration, solve a linear system for the optimal perturbation ${\mathbf{\psi }}^{\star }$ and update ${\mathbf{C}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{\psi }}^{\star \wedge }){\mathbf{C}}_{\mathrm{op}}$. Both the “mass matrix” $\mathbf{I}$ and cross-covariance $\mathbf{W}$ are precomputed from the points, making each iteration $O(1)$ (independent of $M$).

Condition: The solution is unique as long as there are at least three non-collinear points.

---

9.1.4 变换矩阵解 / Transformation Matrix Solution §

中文

最后一种方法直接以变换矩阵 ${\mathbf{T}}_{v_{k}i}\in SE(3)$ 为变量，使用 $SE(3)$ 敏感的扰动方案进行迭代优化。

定义点的齐次表示 ${\mathbf{y}}_j=[{\mathbf{y}}_j^T,1{]}^T$ 和 ${\mathbf{p}}_j=[{\mathbf{p}}_j^T,1{]}^T$，误差为

$${\mathbf{e}}_j={\mathbf{y}}_j-\mathbf{T}{\mathbf{p}}_j.$$

使用扰动 $\mathbf{T}=\exp ({\mathbf{ϵ}}^{\wedge }){\mathbf{T}}_{\mathrm{op}}$ 和恒等式 ${\mathbf{ϵ}}^{\wedge }{\mathbf{z}}_j={\mathbf{z}}_j^{\odot }\mathbf{ϵ}$，目标函数在 $\mathbf{ϵ}$ 中恰好是二次型，最优更新：

$$\left(\frac{1}{w}\sum _{j=1}^M{w}_j{\mathbf{z}}_j^{\odot T}{\mathbf{z}}_j^{\odot }\right){\mathbf{ϵ}}^{\star }=\frac{1}{w}\sum _{j=1}^M{w}_j{\mathbf{z}}_j^{\odot T}({\mathbf{y}}_j-{\mathbf{z}}_j),$$

其中 ${\mathbf{z}}_j={\mathbf{T}}_{\mathrm{op}}{\mathbf{p}}_j$。利用恒等式 $(\mathbf{Tp}{)}^{\odot }={\mathbf{Tp}}^{\odot }{\mathcal{T}}^{-1}$，左边化为

$$\frac{1}{w}\sum _{j=1}^M{w}_j{\mathbf{z}}_j^{\odot T}{\mathbf{z}}_j^{\odot }={\mathcal{T}}_{\mathrm{op}}^{-T}\mathcal{M}{\mathcal{T}}_{\mathrm{op}}^{-1},$$

其中 $\mathcal{M}$ 是广义质量矩阵（仅依赖于固定系中的点，可预计算）。最优更新的闭式解为

$${\mathbf{ϵ}}^{\star }={\mathcal{T}}_{\mathrm{op}}{\mathcal{M}}^{-1}{\mathcal{T}}_{\mathrm{op}}^T\mathbf{a},$$

其中 $\mathbf{a}$ 是依赖于当前猜测的向量，可用 $\mathbf{W}$ 和质心计算。更新后迭代：${\mathbf{T}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{ϵ}}^{\star \wedge }){\mathbf{T}}_{\mathrm{op}}$。

---

English

Using homogeneous-coordinate points ${\mathbf{p}}_j=[{\mathbf{p}}_j^T,1{]}^T$ and the $SE(3)$-sensitive perturbation $\mathbf{T}=\exp ({\mathbf{ϵ}}^{\wedge }){\mathbf{T}}_{\mathrm{op}}$, the objective becomes exactly quadratic in $\mathbf{ϵ}$. The optimal update is:

$${\mathbf{ϵ}}^{\star }={\mathcal{T}}_{\mathrm{op}}{\mathcal{M}}^{-1}{\mathcal{T}}_{\mathrm{op}}^T\mathbf{a},$$

where the generalized mass matrix $\mathcal{M}$ (built from the fixed-frame point cloud) is precomputed and used at every iteration, and $\mathbf{a}$ is a $6\times 1$ vector that can also be computed from $\mathbf{W}$ and the centroids (without looping over individual points per iteration). The update ${\mathbf{T}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{ϵ}}^{\star \wedge }){\mathbf{T}}_{\mathrm{op}}$ keeps ${\mathbf{T}}_{\mathrm{op}}\in SE(3)$ exactly.

Summary of three approaches:

Method	Parameterization	Solution type	Notes
Quaternion	$\mathbf{q}\in \mathbb{H},\Vert \mathbf{q}\Vert =1$	Eigenproblem ($4\times 4$)	No approximation
Rotation matrix	$\mathbf{C}\in SO(3)$	SVD, or iterative	Full case analysis
Transformation	$\mathbf{T}\in SE(3)$	Iterative (GN)	Simultaneous $\mathbf{C},\mathbf{r}$

---

9.2 点云跟踪 / Point-Cloud Tracking §

中文

点云跟踪将对齐问题推广到时间序列：机器人随时间运动，在每个时刻观测若干地标点的位置，目标是估计整条位姿轨迹 $\{{\mathbf{T}}_0,{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K\}$。与对齐不同，这里需要融合运动先验和测量信息。

9.2.1 问题设置 / Problem Setup §

状态：$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K\}$，每个 ${\mathbf{T}}_k={\mathbf{T}}_{v_{k}i}$。

输入（运动先验）：初始位姿猜测 ${\check{\mathbf{T}}}_0$ 和各时刻的广义速度 ${\mathbf{\varpi }}_k=[{\mathbf{\nu }}_{v_k}^{i{v}_k};{\mathbf{\omega }}_{v_k}^{i{v}_k}]$（均在车体系中表示）。

测量：在时刻 $k$ 观测地标 $P_j$ 在车体系中的位置

$${\mathbf{y}}_{jk}={\mathbf{r}}_{v_k}^{p_j{v}_k}+{\mathbf{n}}_{jk},{\mathbf{n}}_{jk}\sim \mathcal{N}(0,{\mathbf{R}}_{jk}).$$

---

English

Point-cloud tracking extends alignment to a trajectory estimation problem. The vehicle moves over time, observing known landmarks at each step. Both a motion prior (generalized velocity inputs) and landmark measurements are fused to estimate the pose trajectory $\{{\mathbf{T}}_0,\dots ,{\mathbf{T}}_K\}$.

State: $\mathbf{x}=\{{\mathbf{T}}_k{\}}_{k=0}^K$ where each ${\mathbf{T}}_k\in SE(3)$.

Inputs: Prior pose ${\check{\mathbf{T}}}_0$ and generalized velocity ${\mathbf{\varpi }}_k$ at each step.

Measurements: Position of landmark $P_j$ in the vehicle frame: ${\mathbf{y}}_{jk}={\mathbf{D}}^T{\mathbf{T}}_k{\mathbf{p}}_j+{\mathbf{n}}_{jk}$, where ${\mathbf{D}}^T=[1_{3\times 3}{0}_{3\times 1}]$ strips the homogeneous coordinate.

---

9.2.2 运动先验 / Motion Prior §

中文

从 $SE(3)$ 运动学 $\dot{\mathbf{T}}={\mathbf{\varpi }}^{\wedge }\mathbf{T}$ 出发，假设速度输入在离散时间段内为常数，得到：

标称运动学（均值传播）：

$${\check{\mathbf{T}}}_k=\underset{{\Xi }_k}{\underbrace{\exp (\Delta t_k{\mathbf{\varpi }}_k^{\wedge })}}{\hat{\mathbf{T}}}_{k-1}.$$

扰动运动学（协方差传播）：

$$\delta {\mathbf{\xi }}_k=\underset{{\mathbf{F}}_{k-1}=\mathrm{Ad}({\Xi }_k)}{\underbrace{\exp (\Delta t_k{\mathbf{\varpi }}_k^{\wedge })}}\delta {\mathbf{\xi }}_{k-1}+{\mathbf{w}}_k,{\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_k).$$

注意：与普通向量空间的 EKF 不同，这里的线性化矩阵 ${\mathbf{F}}_{k-1}=\mathrm{Ad}({\Xi }_k)$ 只依赖于输入（广义速度），不依赖于当前状态估计——这是在李群上使用指数映射定义扰动的重要优势。

---

English

Starting from the $SE(3)$ kinematic equation $\dot{\mathbf{T}}={\mathbf{\varpi }}^{\wedge }\mathbf{T}$, assuming piecewise-constant inputs over intervals $\Delta t_k$:

Nominal kinematics (mean propagation):

$${\check{\mathbf{T}}}_k={\Xi }_k{\hat{\mathbf{T}}}_{k-1},{\Xi }_k=\exp (\Delta t_k{\mathbf{\varpi }}_k^{\wedge }).$$

Perturbation kinematics (covariance propagation):

$$\delta {\check{\mathbf{\xi }}}_k={\mathbf{F}}_{k-1}\delta {\hat{\mathbf{\xi }}}_{k-1}+{\mathbf{w}}_k,{\mathbf{F}}_{k-1}=\mathrm{Ad}({\Xi }_k)=\exp (\Delta t_k{\mathbf{\varpi }}_k^{\wedge }).$$

A key advantage: the Jacobian ${\mathbf{F}}_{k-1}$ depends only on the input ${\mathbf{\varpi }}_k$, not the current state estimate. This simplifies the EKF prediction step considerably.

---

9.2.3 测量模型 / Measurement Model §

中文

测量模型（非线性）为

$${\mathbf{y}}_{jk}={\mathbf{D}}^T{\mathbf{T}}_k{\mathbf{p}}_j+{\mathbf{n}}_{jk}.$$

使用扰动 ${\mathbf{T}}_k=\exp (\delta {\mathbf{\xi }}_k^{\wedge }){\bar{\mathbf{T}}}_k$ 线性化后（一阶近似）：

$$\delta {\mathbf{y}}_{jk}\approx \underset{{\mathbf{G}}_{jk}}{\underbrace{{\mathbf{D}}^T({\bar{\mathbf{T}}}_k{\mathbf{p}}_j{)}^{\odot }}}\delta {\mathbf{\xi }}_k+{\mathbf{n}}_{jk}.$$

其中 $(\cdot {)}^{\odot }$ 是第 8 章定义的齐次点算子，将 $4\times 1$ 齐次点映射为 $4\times 6$ 矩阵。线性化 Jacobian 为

$${\mathbf{G}}_{jk}={\mathbf{D}}^T({\check{\mathbf{T}}}_k{\mathbf{p}}_j{)}^{\odot }\in {\mathbb{R}}^{3\times 6}.$$

---

English

The linearized measurement Jacobian is:

$${\mathbf{G}}_{jk}={\mathbf{D}}^T({\check{\mathbf{T}}}_k{\mathbf{p}}_j{)}^{\odot }\in {\mathbb{R}}^{3\times 6},$$

where ${\check{\mathbf{T}}}_k$ is the predicted mean pose and $(\cdot {)}^{\odot }$ is the homogeneous-point operator from Chapter 8 satisfying ${\mathbf{ϵ}}^{\wedge }\mathbf{z}={\mathbf{z}}^{\odot }\mathbf{ϵ}$.

For $M$ simultaneous point measurements at time $k$, stack: ${\mathbf{y}}_k=[{\mathbf{y}}_{1k}^T,\dots ,{\mathbf{y}}_{Mk}^T{]}^T$, ${\mathbf{G}}_k=[{\mathbf{G}}_{1k}^T,\dots ,{\mathbf{G}}_{Mk}^T{]}^T$, ${\mathbf{R}}_k=\mathrm{diag}({\mathbf{R}}_{1k},\dots ,{\mathbf{R}}_{Mk})$.

---

9.2.4 扩展卡尔曼滤波解 / EKF Solution §

中文

将 $SE(3)$ 的对称性与第 4 章的 EKF 框架结合，得到如下”李群版 EKF“，共五个标准方程：

$$\text{协方差预测：}{\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_k,\text{\hspace{1em}(9.1)}$$ $$\text{均值预测：}{\check{\mathbf{T}}}_k={\Xi }_k{\hat{\mathbf{T}}}_{k-1},\text{\hspace{1em}(9.2)}$$ $$\text{卡尔曼增益：}{\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k\right)}^{-1},\text{\hspace{1em}(9.3)}$$ $$\text{协方差更新：}{\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{G}}_k){\check{\mathbf{P}}}_k,\text{\hspace{1em}(9.4)}$$ $$\text{均值更新：}{\hat{\mathbf{T}}}_k=\exp \left({\left({\mathbf{K}}_k({\mathbf{y}}_k-{\check{\mathbf{y}}}_k)\right)}^{\wedge }\right){\check{\mathbf{T}}}_k.\text{\hspace{1em}(9.5)}$$

其中 ${\check{\mathbf{y}}}_k=[{\mathbf{D}}^T{\check{\mathbf{T}}}_k{\mathbf{p}}_1;\dots ;{\mathbf{D}}^T{\check{\mathbf{T}}}_k{\mathbf{p}}_M]$ 是非线性测量模型在预测均值处的评估。

关键差异：

• 均值存储在李群 $SE(3)$ 中，更新使用指数映射确保 ${\hat{\mathbf{T}}}_k\in SE(3)$
• 协方差存储在李代数 $\mathfrak{se}(3)$ 中（$6\times 6$ 矩阵），以扰动 $\delta {\mathbf{\xi }}_k$ 为坐标

---

English

The Lie-group-adapted EKF (five canonical equations):

Step	Equation
Cov. predict	${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_k$
Mean predict	${\check{\mathbf{T}}}_k={\Xi }_k{\hat{\mathbf{T}}}_{k-1}$
Kalman gain	${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1}$
Cov. update	${\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{G}}_k){\check{\mathbf{P}}}_k$
Mean update	${\hat{\mathbf{T}}}_k=\exp \left(\right({\mathbf{K}}_k({\mathbf{y}}_k-{\check{\mathbf{y}}}_k){)}^{\wedge }){\check{\mathbf{T}}}_k$

Key insight: Means live in the Lie group $SE(3)$ (matrices), covariances live in the Lie algebra $\mathfrak{se}(3)$ ($6\times 6$ matrices). The exponential map in the mean update ensures the estimate always stays exactly on the manifold $SE(3)$.

Relationship to Invariant EKF: Under a certain right-perturbation choice, this EKF takes the form of the invariant EKF (Barrau and Bonnabel, 2017), which enjoys state-independent Jacobians and provably stable error dynamics.

---

9.2.5 批量最大后验估计解 / Batch MAP Solution §

中文

批量方法同时求解整条轨迹 $\mathbf{x}=\{{\mathbf{T}}_0,\dots ,{\mathbf{T}}_K\}$。

误差函数：

对运动先验（$k=0$）：${\mathbf{e}}_{v,0}(\mathbf{x})=\ln ({\check{\mathbf{T}}}_0{\mathbf{T}}_0^{-1}{)}^{\vee }$；

对后续步（$k\ge 1$）：${\mathbf{e}}_{v,k}(\mathbf{x})=\ln ({\Xi }_k{\mathbf{T}}_{k-1}{\mathbf{T}}_k^{-1}{)}^{\vee }$；

对测量（每个地标 $j$，每个时刻 $k$）：${\mathbf{e}}_{y,jk}(\mathbf{x})={\mathbf{y}}_{jk}-{\mathbf{D}}^T{\mathbf{T}}_k{\mathbf{p}}_j$。

目标函数（MAP = 最大后验）：

$$J(\mathbf{x})=\sum _{k=0}^K\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{W}}_{v,k}^{-1}{\mathbf{e}}_{v,k}+\sum _{j,k}\frac{1}{2}{\mathbf{e}}_{y,jk}^T{\mathbf{R}}_{jk}^{-1}{\mathbf{e}}_{y,jk}.$$

高斯-牛顿迭代：将所有误差关于扰动 ${\mathbf{ϵ}}_k$ 线性化后，构建块状矩阵方程

$$\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b},\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H},$$

其中 $\mathbf{H}$ 是稀疏的块双对角矩阵（含测量部分后为块结构）。求解后按 ${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k}$ 更新，迭代至收敛。

李群批量估计的本质：更新步骤在李代数 $\mathfrak{se}(3)$ 中（线性空间）求解，但均值始终存储在李群 $SE(3)$ 中（非线性流形）。这是”在正切空间中优化”的标准做法，等价于黎曼梯度下降。

---

English

The batch MAP problem estimates the entire trajectory simultaneously by minimizing:

$$J(\mathbf{x})=\sum _k\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{W}}_{v,k}^{-1}{\mathbf{e}}_{v,k}+\sum _{j,k}\frac{1}{2}{\mathbf{e}}_{y,jk}^T{\mathbf{R}}_{jk}^{-1}{\mathbf{e}}_{y,jk}.$$

Gauss-Newton update: After linearizing all error terms about the operating point ${\mathbf{x}}_{\mathrm{op}}=\{{\mathbf{T}}_{\mathrm{op},k}\}$, the quadratic approximation leads to:

$$\underset{\text{block-tridiagonal }\mathbf{A}}{\underbrace{{\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}}}\delta {\mathbf{x}}^{\star }=\underset{\mathbf{b}}{\underbrace{{\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{e}({\mathbf{x}}_{\mathrm{op}})}}.$$

The structure of $\mathbf{A}$ is block-tridiagonal (from the motion prior chain) plus low-rank updates (from measurements), enabling efficient sparse Cholesky solves. After each solve, update via ${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k}$ and iterate.

---

9.3 位姿图松弛 / Pose-Graph Relaxation §

中文

位姿图问题是 SLAM 的核心。与上节不同的是，这里没有地标点的绝对位置，只有一组相对位姿”测量”（伪测量）。这些相对测量来自里程计、视觉里程计等方法，会在闭环时出现矛盾（不一致性），需要”松弛”整个图以找到全局一致的位姿估计。

9.3.1 问题设置 / Problem Setup §

状态：$\mathbf{x}=\{{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K\}$，其中 ${\mathbf{T}}_k$ 是位姿 $k$ 相对于固定参考位姿 0 的变换矩阵（固定 ${\mathbf{T}}_0=1$）。

测量：对图中某些边 $(k,\ell )$，有相对位姿测量

$${\mathbf{T}}_{k\ell }=\exp ({\mathbf{\xi }}_{k\ell }^{\wedge }){\bar{\mathbf{T}}}_{k\ell },{\mathbf{\xi }}_{k\ell }\sim \mathcal{N}(0,{\mathbf{\Sigma }}_{k\ell }).$$

误差函数（移动系不变误差）：

$${\mathbf{e}}_{k\ell }(\mathbf{x})=\ln {\left({\bar{\mathbf{T}}}_{k\ell }{\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}\right)}^{\vee }.$$

直觉：${\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}$ 是从位姿 $k$ 到 $\ell$ 的真实相对变换，${\bar{\mathbf{T}}}_{k\ell }$ 是其测量值。误差就是两者之差（在李代数中）。如果所有测量都完全准确，绕任意闭环一圈后误差为零。

---

English

Pose-graph relaxation is the back-end of SLAM. Given a set of relative pose “measurements” $\{{\bar{\mathbf{T}}}_{k\ell },{\mathbf{\Sigma }}_{k\ell }\}$ for edges $(k,\ell )$ in a pose graph, find the globally consistent absolute poses ${\mathbf{T}}_k$ (relative to a fixed reference node 0).

Error term (moving-frame invariant):

$${\mathbf{e}}_{k\ell }(\mathbf{x})=\ln {\left({\bar{\mathbf{T}}}_{k\ell }{\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}\right)}^{\vee }\in {\mathbb{R}}^6.$$

When the graph has loops, the measurements are inconsistent (they don’t compose to identity around any loop), and we must relax the graph to minimize the total weighted squared error.

---

9.3.2 批量最大似然解 / Batch Maximum Likelihood Solution §

中文

目标函数（最大似然 = 最小化加权平方误差）：

$$J(\mathbf{x})=\frac{1}{2}\sum _{k,\ell }{\mathbf{e}}_{k\ell }(\mathbf{x}{)}^T{\mathbf{\Sigma }}_{k\ell }^{-1}{\mathbf{e}}_{k\ell }(\mathbf{x}).$$

使用左扰动方案 ${\mathbf{T}}_k=\exp ({\mathbf{ϵ}}_k^{\wedge }){\mathbf{T}}_{\mathrm{op},k}$，线性化误差为

$${\mathbf{e}}_{k\ell }(\mathbf{x})\approx {\mathbf{e}}_{k\ell }({\mathbf{x}}_{\mathrm{op}})-{\mathbf{G}}_{k\ell }\delta {\mathbf{x}}_{k\ell },$$

其中

$${\mathbf{G}}_{k\ell }=[-\mathcal{J}(-{\mathbf{e}}_{k\ell }({\mathbf{x}}_{\mathrm{op}}){)}^{-1}{\mathcal{T}}_{\mathrm{op},k}{\mathcal{T}}_{\mathrm{op},\ell }^{-1}\mathcal{J}(-{\mathbf{e}}_{k\ell }({\mathbf{x}}_{\mathrm{op}}){)}^{-1}].$$

注意：${\mathcal{J}}^{-1}$ 项在收敛后残差非零时非平凡，保留该项可提升精度。

高斯-牛顿步骤：构建并求解 $\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}$，其中 $\mathbf{A}={\sum }_{k,\ell }{\mathbf{P}}_{k\ell }^T{\mathbf{G}}_{k\ell }^T{\mathbf{\Sigma }}_{k\ell }^{-1}{\mathbf{G}}_{k\ell }{\mathbf{P}}_{k\ell }$（稀疏矩阵，对应图的拓扑结构）。然后按 ${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k}$ 更新，迭代至收敛。

初始化：找到一棵生成树，从节点 0 出发通过复合相对测量来初始化所有节点的位姿。

利用稀疏性：

• 通过识别”悬臂链”（只有一端连接到度大于 2 节点）和”约束链”（两端均连接到节点），可以先合并局部链，再求解精简图，大幅减少计算量
• 对链状图，$\mathbf{A}$ 为块三对角矩阵，可用稀疏 Cholesky 分解高效求解，代价线性于链长

---

English

Maximum likelihood objective:

$$J(\mathbf{x})=\frac{1}{2}\sum _{(k,\ell )\in \mathcal{E}}{\mathbf{e}}_{k\ell }(\mathbf{x}{)}^T{\mathbf{\Sigma }}_{k\ell }^{-1}{\mathbf{e}}_{k\ell }(\mathbf{x}).$$

Linearized error:

$${\mathbf{e}}_{k\ell }(\mathbf{x})\approx {\mathbf{e}}_{k\ell }({\mathbf{x}}_{\mathrm{op}})-{\mathbf{G}}_{k\ell }\delta {\mathbf{x}}_{k\ell },$$

where the $6\times 12$ Jacobian ${\mathbf{G}}_{k\ell }$ involves the inverse left Jacobian ${\mathcal{J}}^{-1}$ and the current adjoint estimates. Setting $\mathcal{J}\approx 1$ (first-order approximation) gives a simpler but less accurate version; keeping more terms is beneficial when residuals are large.

Gauss-Newton iteration: Solve $\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}$ (sparse linear system), update poses via ${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k}$, iterate.

Initialization: Find a spanning tree in the graph; compound relative measurements outward from node 0.

Sparsity exploitation: The matrix $\mathbf{A}$ has the same sparsity pattern as the pose-graph adjacency matrix. For chain subgraphs, $\mathbf{A}$ is block-tridiagonal and solvable in $O(K)$ via sparse Cholesky + forward-backward substitution.

---

9.4 惯性导航 / Inertial Navigation §

中文

惯性测量单元（IMU）是最常用的自运动传感器之一。它同时测量角速度（陀螺仪）和线加速度（加速度计），可以通过积分获得位置和朝向估计。然而 IMU 积分会随时间累积误差，需要与其他传感器融合。

本节展示如何将 IMU 数据嵌入到李群框架中，实现严格数学正确的状态估计。

9.4.1 传感器模型 / IMU Sensor Model §

简化 IMU 模型（传感器系与车体系重合）：

$$\underset{\text{measured}}{\underbrace{\left[\begin{array}{c}\tilde{\mathbf{a}}\\ \tilde{\mathbf{\omega }}\end{array}\right]}}=\underset{\text{true}}{\underbrace{\left[\begin{array}{c}{\mathbf{C}}_{vi}({\mathbf{a}}_i^{vi}-{\mathbf{g}}_i)\\ {\mathbf{\omega }}_v^{vi}\end{array}\right]}}+\underset{\text{bias}}{\underbrace{\left[\begin{array}{c}{\mathbf{b}}_a\\ {\mathbf{b}}_{\omega }\end{array}\right]}}+\underset{\text{noise}}{\underbrace{\left[\begin{array}{c}{\mathbf{w}}_a\\ {\mathbf{w}}_{\omega }\end{array}\right]}}.$$

其中 ${\mathbf{g}}_i$ 为重力（固定系），偏置 $\mathbf{b}$ 建模为随机游走过程。

---

English

The IMU measures angular rate and specific force (acceleration minus gravity), both corrupted by biases and noise:

$$\left[\begin{array}{c}\tilde{\mathbf{a}}\\ \tilde{\mathbf{\omega }}\end{array}\right]=\left[\begin{array}{c}{\mathbf{C}}_{vi}({\mathbf{a}}_i^{vi}-{\mathbf{g}}_i)\\ {\mathbf{\omega }}_v^{vi}\end{array}\right]+\left[\begin{array}{c}{\mathbf{b}}_a\\ {\mathbf{b}}_{\omega }\end{array}\right]+\left[\begin{array}{c}{\mathbf{w}}_a\\ {\mathbf{w}}_{\omega }\end{array}\right].$$

Biases ${\mathbf{b}}_a,{\mathbf{b}}_{\omega }$ are modeled as random walks: ${\dot{\mathbf{b}}}_a={\mathbf{w}}_{b,a}$, ${\dot{\mathbf{b}}}_{\omega }={\mathbf{w}}_{b,\omega }$.

---

9.4.2 扩展位姿：$S{E}_2(3)$ / Extended Poses: $S{E}_2(3)$ §

中文

由于 IMU 测量线加速度，状态向量中必须包含线速度 ${\mathbf{v}}_i^{vi}$。最优雅的方式是将位置、速度和旋转打包成一个扩展位姿矩阵

$${\mathbf{T}}_{iv}=\left[\begin{array}{ccc}{\mathbf{C}}_{iv}& {\mathbf{r}}_i^{vi}& {\mathbf{v}}_i^{vi}\\ 0^T& 1& 0\\ 0^T& 0& 1\end{array}\right]\in S{E}_2(3).$$

这组 $5\times 5$ 矩阵构成另一个矩阵李群 $S{E}_2(3)$，具备类似 $SE(3)$ 的代数结构：

• 李代数元素：$\mathbf{\xi }=[\mathbf{\rho };\mathbf{\nu };\mathbf{\varphi }]\in {\mathbb{R}}^9$
• 指数映射：

$$\exp ({\mathbf{\xi }}^{\wedge })=\left[\begin{array}{ccc}\exp ({\mathbf{\varphi }}^{\wedge })& \mathbf{J}(\mathbf{\varphi })\mathbf{\rho }& \mathbf{J}(\mathbf{\varphi })\mathbf{\nu }\\ 0^T& 1& 0\\ 0^T& 0& 1\end{array}\right]$$

• $9\times 9$ 伴随矩阵：

$$\mathcal{T}=\mathrm{Ad}({\mathbf{T}}_{iv})=\left[\begin{array}{ccc}{\mathbf{C}}_{iv}& 0& {\mathbf{r}}_i^{vi\wedge }{\mathbf{C}}_{iv}\\ 0& {\mathbf{C}}_{iv}& {\mathbf{v}}_i^{vi\wedge }{\mathbf{C}}_{iv}\\ 0& 0& {\mathbf{C}}_{iv}\end{array}\right]$$

直觉：$S{E}_2(3)$ 是”导航群”。它把旋转、位置和速度统一在一个矩阵中，可以用一次矩阵乘法完成坐标变换的”一站式”操作——正如 $SE(3)$ 把旋转和位置统一在一起一样。

---

English

Since the IMU measures linear acceleration, the velocity ${\mathbf{v}}_i^{vi}$ must be included in the state. We package rotation, position, and velocity into an extended pose matrix:

$${\mathbf{T}}_{iv}=\left[\begin{array}{ccc}{\mathbf{C}}_{iv}& {\mathbf{r}}_i^{vi}& {\mathbf{v}}_i^{vi}\\ 0^T& 1& 0\\ 0^T& 0& 1\end{array}\right]\in S{E}_2(3).$$

The set of all such $5\times 5$ matrices forms the matrix Lie group $S{E}_2(3)$ (also called $S{E}_2(3)$ or the “navigation group”). Its Lie algebra ${\mathfrak{se}}_2(3)$ has elements parametrized by $\mathbf{\xi }=[\mathbf{\rho };\mathbf{\nu };\mathbf{\varphi }]\in {\mathbb{R}}^9$. The exponential map, adjoint, and left Jacobian $\mathcal{J}(\mathbf{\xi })$ all take $3\times 3$ block forms analogous to $SE(3)$, with $SO(3)$’s left Jacobian $\mathbf{J}(\mathbf{\varphi })$ appearing on the diagonal.

Key identities (same as $SE(3)$, extended to $9\times 9$):

$$(\mathcal{T}\mathbf{\xi }{)}^{\wedge }=\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{-1},\exp ((\mathcal{T}\mathbf{\xi }{)}^{\wedge })=\mathbf{T}\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{T}}^{-1}.$$

---

9.4.3 运动模型 / Motion Model §

中文

连续时间运动方程（选择在惯性系中表示平移和速度）：

$${\dot{\mathbf{C}}}_{iv}={\mathbf{C}}_{iv}(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{w}}_{\omega }{)}^{\wedge },\dot{\mathbf{r}}=\mathbf{v},\dot{\mathbf{v}}={\mathbf{C}}_{iv}(\tilde{\mathbf{a}}-{\mathbf{b}}_a-{\mathbf{w}}_a)+\mathbf{g}.$$

这里使用右扰动方案（本书唯一使用右扰动的地方），因为运动方程自然地在 ${\mathbf{T}}_{iv}$ 的右侧出现噪声：

$${\mathbf{T}}_{iv}={\bar{\mathbf{T}}}_{iv}\exp (\delta {\mathbf{\xi }}^{\wedge }),\delta \mathbf{\xi }=[\delta \mathbf{\rho };\delta \mathbf{\nu };\delta \mathbf{\varphi }].$$

分离标称分量和扰动分量后，扰动动力学（LTV 系统）为

$$\delta \dot{\mathbf{x}}=\mathbf{A}(t)\delta \mathbf{x}+\mathbf{L}\mathbf{w},$$

其中 $\delta \mathbf{x}=[\delta \mathbf{\rho };\delta \mathbf{\nu };\delta \mathbf{\varphi };\delta {\mathbf{b}}_{\omega };\delta {\mathbf{b}}_a{]}^T\in {\mathbb{R}}^{15}$，$\mathbf{A}$ 依赖于当前（标称）角速度和加速度测量值（减去偏置）。

---

English

Continuous-time nominal motion model (in $S{E}_2(3)$):

$${\dot{\bar{\mathbf{T}}}}_{iv}={\bar{\mathbf{T}}}_{iv}\Omega +\Gamma ,\Omega =\left[\begin{array}{ccc}{\bar{\mathbf{\omega }}}^{\wedge }& 0& \bar{\mathbf{a}}\\ 0^T& 0& 0\\ 0^T& 1& 0\end{array}\right],$$

where $\bar{\mathbf{\omega }}=\tilde{\mathbf{\omega }}-{\bar{\mathbf{b}}}_{\omega }$ and $\bar{\mathbf{a}}=\tilde{\mathbf{a}}-{\bar{\mathbf{b}}}_a$, and $\Gamma$ contains the gravity vector.

The perturbation model (LTV system on the $15$-dimensional state $\delta \mathbf{x}=[\delta \mathbf{\rho };\delta \mathbf{\nu };\delta \mathbf{\varphi };\delta {\mathbf{b}}_{\omega };\delta {\mathbf{b}}_a{]}^T$):

$$\delta \dot{\mathbf{x}}=\left[\begin{array}{ccccc}-{\bar{\mathbf{\omega }}}^{\wedge }& 1& 0& 0& 0\\ 0& -{\bar{\mathbf{\omega }}}^{\wedge }& -{\bar{\mathbf{a}}}^{\wedge }& 0& -1\\ 0& 0& -{\bar{\mathbf{\omega }}}^{\wedge }& -1& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\delta \mathbf{x}+\mathbf{Lw}.$$

This right-perturbation scheme (the only place in the book) is natural because the noise enters on the “vehicle side” of ${\mathbf{T}}_{iv}$.

---

9.4.4 均值传播 / Propagating the Mean §

中文

假设每个 IMU 采样间隔 $({\tau }_{j-1},{\tau }_j)$ 内测量值恒定，标称扩展位姿的更新为（封闭式）

$$\bar{\mathbf{C}}({\tau }_j)=\bar{\mathbf{C}}({\tau }_{j-1})\exp (\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j^{\wedge }),$$ $$\bar{\mathbf{r}}({\tau }_j)=\bar{\mathbf{r}}({\tau }_{j-1})+\Delta {\tau }_j\bar{\mathbf{v}}({\tau }_{j-1})+\frac{1}{2}\Delta {\tau }_j^2\left(\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{N}(\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j){\bar{\mathbf{a}}}_j+\mathbf{g}\right),$$ $$\bar{\mathbf{v}}({\tau }_j)=\bar{\mathbf{v}}({\tau }_{j-1})+\Delta {\tau }_j\left(\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{J}(\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j){\bar{\mathbf{a}}}_j+\mathbf{g}\right),$$

其中 $\mathbf{J}(\cdot )$ 是 $SO(3)$ 的左 Jacobian，$\mathbf{N}(\mathbf{\varphi })$ 是新的矩阵函数

$$\mathbf{N}(\varphi \mathbf{a})=\frac{2(1-\cos \varphi )}{{\varphi }^2}1+\left(1-\frac{2(1-\cos \varphi )}{{\varphi }^2}\right)\mathbf{a}{\mathbf{a}}^T+\frac{2(\varphi -\sin \varphi )}{{\varphi }^2}{\mathbf{a}}^{\wedge }.$$

这些方程比简单欧拉积分更精确，特别是在角速度较大时。

---

English

Under piecewise-constant IMU measurements over $[{\tau }_{j-1},{\tau }_j]$, the nominal mean update is:

$$\bar{\mathbf{C}}({\tau }_j)=\bar{\mathbf{C}}({\tau }_{j-1})\exp (\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j^{\wedge }),$$ $$\bar{\mathbf{r}}({\tau }_j)=\bar{\mathbf{r}}({\tau }_{j-1})+\Delta {\tau }_j\bar{\mathbf{v}}({\tau }_{j-1})+\frac{1}{2}\Delta {\tau }_j^2\left({\mathbf{\alpha }}_r({\tau }_j)+\mathbf{g}\right),$$ $$\bar{\mathbf{v}}({\tau }_j)=\bar{\mathbf{v}}({\tau }_{j-1})+\Delta {\tau }_j\left({\mathbf{\alpha }}_v({\tau }_j)+\mathbf{g}\right),$$

where:

$${\mathbf{\alpha }}_r({\tau }_j)=\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{N}(\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j){\bar{\mathbf{a}}}_j,{\mathbf{\alpha }}_v({\tau }_j)=\bar{\mathbf{C}}({\tau }_{j-1})\mathbf{J}(\Delta {\tau }_j{\bar{\mathbf{\omega }}}_j){\bar{\mathbf{a}}}_j.$$

The $\mathbf{N}$ function accounts for the rotation experienced during the interval when integrating acceleration into position. For small $\varphi$, $\mathbf{N}\to 1$ and ${\mathbf{\alpha }}_r\approx {\mathbf{\alpha }}_v$ (Euler approximation).

---

9.4.5 协方差传播 / Propagating the Covariance §

中文

扰动动力学（LTV 系统）对应的离散时间协方差传播为

$$\check{\mathbf{P}}({\tau }_j,{\tau }_j)=\mathbf{\Phi }({\tau }_j,{\tau }_{j-1})\check{\mathbf{P}}({\tau }_{j-1},{\tau }_{j-1})\mathbf{\Phi }({\tau }_j,{\tau }_{j-1}{)}^T+\mathbf{Q}({\tau }_j,{\tau }_{j-1}),$$

其中转移矩阵 $\mathbf{\Phi }({\tau }_j,{\tau }_{j-1})=\exp ({\mathbf{A}}_j\Delta {\tau }_j)$，离散过程噪声协方差

$$\mathbf{Q}({\tau }_j,{\tau }_{j-1})={\int }_{{\tau }_{j-1}}^{{\tau }_j}\mathbf{\Phi }({\tau }_j,t)\mathbf{L}{\mathbf{Q}}_c{\mathbf{L}}^T\mathbf{\Phi }({\tau }_j,t{)}^{T}dt,$$

可用 $\Delta {\tau }_j$ 的三阶近似高效计算（见正文公式）。

---

English

The discrete-time covariance propagation uses:

$$\check{\mathbf{P}}({\tau }_j)={\mathbf{\Phi }}_j\check{\mathbf{P}}({\tau }_{j-1}){\mathbf{\Phi }}_j^T+{\mathbf{Q}}_j,$$

where ${\mathbf{\Phi }}_j=\exp ({\mathbf{A}}_j\Delta {\tau }_j)$ is the transition matrix (computable via matrix exponential or truncated series), and ${\mathbf{Q}}_j$ is the integrated process noise covariance. The third-order approximation for ${\mathbf{Q}}_j$ balances accuracy and efficiency:

$${\mathbf{Q}}_j\approx \Delta {\tau }_j\mathbf{L}{\mathbf{Q}}_c{\mathbf{L}}^T+\frac{1}{2}\Delta {\tau }_j^2({\mathbf{A}}_j\mathbf{L}{\mathbf{Q}}_c{\mathbf{L}}^T+\text{transpose})+\frac{1}{6}\Delta {\tau }_j^3(\cdots ).$$

---

9.4.6 滤波 / Filtering §

中文

将 IMU 运动模型嵌入 EKF 框架。状态为

$$\mathbf{x}=\left\{{\mathbf{T}}_{iv}=\left[\begin{array}{ccc}{\mathbf{C}}_{iv}& {\mathbf{r}}_i^{vi}& {\mathbf{v}}_i^{vi}\\ 0^T& 1& 0\\ 0^T& 0& 1\end{array}\right],\mathbf{b}=\left[\begin{array}{c}{\mathbf{b}}_{\omega }\\ {\mathbf{b}}_a\end{array}\right]\right\}.$$

状态维度为 $15$（$9$ 维扩展位姿扰动 + $6$ 维偏置），协方差矩阵 $15\times 15$。

预测步骤（循环遍历所有 IMU 数据 $j=0,1,\dots$ 直至下一外部测量时刻 $k$）：

$${\hat{\mathbf{P}}}_{j=0}={\hat{\mathbf{P}}}_{k-1},{\hat{\mathbf{T}}}_{j=0}={\hat{\mathbf{T}}}_{k-1},$$

对每个 IMU 数据：用 §9.4.4 传播均值，用 §9.4.5 传播协方差。

更新步骤（同 §9.2.4 的格式，但状态是 $S{E}_2(3)$ 扩展位姿）：当有外部测量（如 GPS、相机）时，

$${\check{\mathbf{y}}}_k=\mathbf{h}({\check{\mathbf{T}}}_k),{\mathbf{G}}_k=\frac{\partial \mathbf{h}}{\partial \mathbf{\xi }}{|}_{{\check{\mathbf{T}}}_k},$$ $${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1},$$ $${\hat{\mathbf{P}}}_k=(1-{\mathbf{K}}_k{\mathbf{G}}_k){\check{\mathbf{P}}}_k,$$ $$\delta {\mathbf{\xi }}_k={\mathbf{K}}_k({\mathbf{y}}_k-{\check{\mathbf{y}}}_k),{\hat{\mathbf{T}}}_k={\check{\mathbf{T}}}_k\exp (\delta {\mathbf{\xi }}_k^{\wedge })(\text{右更新，因为是 }S{E}_2(3)).$$

注意这里均值更新用右乘（因为扰动定义在右侧），偏置用加法更新：${\hat{\mathbf{b}}}_k={\check{\mathbf{b}}}_k+\delta {\mathbf{b}}_k$。

---

English

The IMU-based EKF operates on the $15$-dimensional state comprising the $S{E}_2(3)$ extended pose plus biases. During prediction, the state is propagated forward through each IMU sample using the mean/covariance equations of Sections 9.4.4–9.4.5. When an external measurement (e.g., GPS position, camera bearing) arrives at time $k$, the standard EKF correction is applied:

$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k{)}^{-1},$$ $${\hat{\mathbf{T}}}_k={\check{\mathbf{T}}}_k\exp (\delta {\mathbf{\xi }}_k^{\wedge }),\delta {\mathbf{\xi }}_k=[\delta \mathbf{\rho };\delta \mathbf{\nu };\delta \mathbf{\varphi }{]}_k\text{ from Kalman gain}.$$

The right-hand perturbation update (unique to IMU/inertial nav) is consistent with the right-perturbation scheme used for $S{E}_2(3)$.

This EKF structure is the core of practically every inertial navigation system used in aerospace and robotics, from the Apollo Lunar Module (where a Kalman filter fused IMU + radar data) to modern visual-inertial odometry.

---

9.5 本章小结 / Chapter Summary §

中文

本章将第一、二部分的工具整合，解决了三个核心的三维位姿估计应用：

问题	核心方法	关键结果
点云对齐	四元数特征值 / SVD / $SE(3)$ 迭代 GN	无局部极小值；唯一解等价于 Wahba 问题
点云跟踪	李群版 EKF / 批量 MAP (GN)	均值在 $SE(3)$；协方差在 $\mathfrak{se}(3)$
位姿图松弛	批量 ML (GN)，稀疏求解	图稀疏性对应 $\mathbf{A}$ 矩阵稀疏性
惯性导航	$S{E}_2(3)$ 扩展位姿 + 右扰动 EKF	预积分；偏置估计；与外部测量融合

贯穿全章的核心思想：

1. 扰动在李代数中（线性空间），均值在李群中（非线性流形）
2. 左扰动（$\mathbf{T}=\exp ({\mathbf{ϵ}}^{\wedge }){\mathbf{T}}_{\mathrm{op}}$）在大多数情况适用；惯性导航使用右扰动
3. 任何估计问题都可通过定义合适的误差函数，然后用高斯-牛顿迭代求解

---

English

This chapter unified the Lie-group machinery of Part II with the estimation framework of Part I to tackle three fundamental 3D estimation problems:

1. Point-cloud alignment — three equivalent approaches (quaternion eigenproblem, SVD/Wahba, $SE(3)$-iterative GN), all provably free of local minima when a unique global solution exists.

2. Point-cloud tracking — a Lie-group-adapted EKF and batch MAP where means live in $SE(3)$ and covariances live in $\mathfrak{se}(3)$; the motion-model Jacobian ${\mathbf{F}}_{k-1}=\mathrm{Ad}({\Xi }_k)$ depends only on inputs, not the state.

3. Pose-graph relaxation — batch ML on a sparse graph, exploiting the sparsity of $\mathbf{A}$ for efficient computation; the key is defining invariant error terms of the form ${\mathbf{e}}_{k\ell }=\ln ({\bar{\mathbf{T}}}_{k\ell }{\mathbf{T}}_{\ell }{\mathbf{T}}_k^{-1}{)}^{\vee }$.

4. Inertial navigation — the $S{E}_2(3)$ “navigation group” packages rotation, position, and velocity into a $5\times 5$ matrix; a right-perturbation EKF on a 15-DOF state (extended pose + biases) serves as the algorithmic backbone of modern inertial navigation.

Unifying principle: Updates are computed in the Lie algebra (a flat vector space), but means are stored and propagated in the Lie group (a curved manifold). The exponential map bridges these two worlds and guarantees geometric consistency at every step.

---

下一章将解决更难的问题：位置和地图同时未知的位姿-点联合估计（束调整与 SLAM）。

The next chapter tackles the harder problem of estimating both the vehicle’s pose and the map of landmarks simultaneously — the bundle adjustment and SLAM problem.