30_流形滤波族

博士前数学路线图·第五批·子专题 A3：流形上的滤波族——ESKF、MEKF、IEKF、InEKF 与 UKF-M

底线陈述（BLUF）：本文把"流形上的 Kalman 滤波"从 1982 年的 MEKF 到 2023 年的 EqVIO 串成一条清晰的理论—工程主线。核心结论：只要状态在李群（或齐性空间）上演化，就应抛弃"把旋转当欧氏向量、事后归一化"的朴素做法，转而采用**乘法/指数形式的误差**（ESKF/MEKF→InEKF→EqF）或**免雅可比的 sigma 点**（UKF-M）。其中 **Barrau-Bonnabel 2017 的 group-affine + log-linear 定理**是分水岭：它把"误差动力学与状态估计无关"从启发式（FEJ）升格为数学恒等式，一举解决了 EKF-SLAM/VIO 20 年来的一致性难题。本文在严格推导（§A3.3–§A3.10）、工程对齐（§A3.11–§A3.14）、陷阱汇总（§A3.15）之间平衡，面向博士入学（档位 3，约 50 h）及博士毕业（档位 4，再加 20 h）的双目标读者。与专题 3（李群）、专题 5（李群上不确定性）、子专题 5-A2（经典非线性滤波）存在有意的公式重叠（标注"详见专题 5 §X"），以保持本单元自足可读。

---

§A3.1 为什么需要"流形上的滤波"——从三个失败案例切入 ⭐

把姿态当作 \(\mathbb{R}^3\) 欧拉角或带约束的四元数塞进标准 EKF，会遭遇三类**结构性**失败：

失败 1：过参数化协方差奇异。 四元数 \(q\in\mathbb{H}\) 有 4 维但单位范数约束让切空间只有 3 维。若用 \(4\times 4\) 协方差，沿 \(q\) 方向的协方差必然为 0 → 矩阵奇异 → Cholesky 崩溃。Lefferts-Markley-Shuster 1982（JGCD 5(5):417–429）即为此提出 MEKF。

失败 2：参数化奇点。 欧拉角在 Gimbal Lock、轴角在 \(\pi\) 附近、MRP 在 \(2\pi\) 附近均有不可微点。滤波器一旦穿越奇点，雅可比病态、协方差爆炸。

失败 3：不可观方向"幽灵观测"。 重力对齐 VIO 沿真实轨迹通常有 4 维不可观子空间（绝对位置 3 + 绕重力轴 yaw 1）；纯 3D SLAM 的全局 gauge 是 6 维刚体变换，2D SLAM 则是 3 维。以 VIO 为例，若 Jacobian 在 \(\hat X\)（非真值）处求值，线性化后的可观性 Gramian 可能**丢掉** yaw 方向——滤波器"错觉"自己看到了 yaw → 协方差过度自信 → 长期 yaw drift。Huang-Mourikis-Roumeliotis（2008–2010）的 FEJ-EKF 与 OC-EKF 都是为修补此问题而生。

本文导引：ESKF/MEKF 解决前两类问题；InEKF 从第一性原理消除第三类问题；UKF-M/EqF 把这些思想扩展到**免雅可比**和**齐性空间**。

---

§A3.2 前置回顾（与专题 3/专题 5 的接口） ⭐

A3.2.1 \(SO(3)\)、\(SE(3)\)、\(SE_2(3)\) 速览（详见专题 3 §3.3–3.5）

• \(SO(3)\)：\(R^\top R=I,\ \det R=+1\)，李代数 \(\mathfrak{so}(3)\) 同构于 \((\mathbb{R}^3,\times)\)，\(\wedge\) 算子 \(\omega^\wedge = [\omega]_\times\)（反对称矩阵）。
• \(SE(3)\)：\(\{(R,t)\}\) 的半直积，\(4\times 4\) 矩阵表示。
• \(SE_2(3)\)（"双重直接等距"群，Barrau）：把 IMU 状态 \((R,v,p)\) 统一为 \(5\times 5\) 矩阵：

\[X=\begin{bmatrix}R & v & p\\ 0_{1\times 3} & 1 & 0\\ 0_{1\times 3} & 0 & 1\end{bmatrix}\in SE_2(3)\subset GL(5,\mathbb{R})\]

群乘法即矩阵乘法，给出 \((R_1R_2,\ R_1 v_2 + v_1,\ R_1 p_2 + p_1)\)。李代数 \(\mathfrak{se}_2(3)\) 为 9 维，\(\wedge\) 算子按 旋转(3)/速度(3)/位置(3) 排列（Barrau-Bonnabel/Hartley 约定，与 manif 的 \([\rho,\theta,\nu]\) 存在排列置换，见 §A3.14）。

指数映射闭式：

\[\exp_{SE_2(3)}(\xi) = \begin{bmatrix}\mathrm{Exp}(\xi^R) & J_l(\xi^R)\xi^v & J_l(\xi^R)\xi^p\\ 0 & 1 & 0\\ 0 & 0 & 1\end{bmatrix}\]

其中 \(J_l\) 为 \(SO(3)\) 左 Jacobian（位置、速度两列共享）。\(9 \times 9\) Adjoint（以下采用 Barrau-Bonnabel TAC 2017 约定，李代数排序为 \([\omega(3),\, \nu_v(3),\, \nu_p(3)]\)，即"旋转-速度-位置"；若使用 Hartley IJRR 2020 的"位置-速度-旋转"排序，块位置需对应调换）：

\[\mathrm{Ad}_X = \begin{bmatrix}R & 0 & 0\\ (v)_\times R & R & 0\\ (p)_\times R & 0 & R\end{bmatrix}\]

A3.2.2 左/右摄动与 Concentrated Gaussian（详见专题 5 §5.3）

"集中高斯"的两种标准形式：

\[X = \bar X \exp(\xi^\wedge),\quad \xi\sim\mathcal{N}(0,P)\qquad\text{(右摄动)}$$ $$X = \exp(\xi^\wedge) \bar X,\quad \xi\sim\mathcal{N}(0,P)\qquad\text{(左摄动)}\]

两者协方差通过 Adjoint 换算：\(P_\text{left} = \mathrm{Ad}_{\bar X}\, P_\text{right}\, \mathrm{Ad}_{\bar X}^\top\)。约定一旦选定必须贯穿 predict/update/reset/ inject。manif 与 Sophus 用 \([\rho,\theta]\) 右摄动；GTSAM 用 \([\omega,\rho]\)；OpenVINS 用 JPL 四元数的左乘扰动。详尽对照见 §A3.14。

---

§A3.3 ESKF 三态分解——为什么把一个状态拆成三个 ⭐⭐

源头：Solà, Quaternion kinematics for the error-state Kalman filter, arXiv:1711.02508（下文简称 Solà-ESKF）§2.2、Table 2。

三态定义： - True state \(\chi_t\)：物理真值（不可知，只用于推导）。 - Nominal state \(\chi\)：大信号，用 IMU 做高频非线性积分，不含噪声/偏置的一阶误差。 - Error state \(\delta\chi\)：小信号，线性化，由 KF 估计，最终注入到 nominal。

真态—名义—误差的复合关系（Table 2）：

\[\mathbf{p}_t=\mathbf{p}+\delta\mathbf{p},\ \mathbf{v}_t=\mathbf{v}+\delta\mathbf{v},\ \mathbf{q}_t=\mathbf{q}\otimes\delta\mathbf{q},\ \mathbf{a}_{bt}=\mathbf{a}_b+\delta\mathbf{a}_b,\ \boldsymbol{\omega}_{bt}=\boldsymbol{\omega}_b+\delta\boldsymbol{\omega}_b,\ \mathbf{g}_t=\mathbf{g}+\delta\mathbf{g}\]

注意**姿态用乘法** \(\mathbf{q}_t=\mathbf{q}\otimes\delta\mathbf{q}\)，而位置/速度/偏置/重力用**加法**——这正是 ESKF 对 MEKF 的自然推广（MEKF 只做 \(\mathbf{q}_t=\mathbf{q}\otimes\delta\mathbf{q}\)，不处理平移）。小角近似 \(\delta\mathbf{q}\approx[1,\tfrac{1}{2}\delta\boldsymbol{\theta}^\top]^\top\)，\(\delta\boldsymbol{\theta}\in\mathbb{R}^3\) 是 KF 估计的最小维度姿态误差。

为什么用 ESKF（Solà §2.1 的四条"asset"）： 1. 最小维度：\(\delta\boldsymbol{\theta}\in\mathbb{R}^3\) 与旋转自由度匹配，\(P\) 天然正定。 2. 零附近线性化：误差态一直在 0 附近，远离所有参数化奇点（gimbal lock、\(\pi\)-wrap），线性化始终有效。 3. 二阶项可忽略：\(\|\delta\chi\|\ll 1\)，Jacobian 计算极简，部分块为常量。 4. 时间解耦：大信号由 nominal 吸收，误差态"slow"，KF 校正可以以远低于 IMU 预测率的频率运行（GPS/视觉 10–30 Hz vs IMU 200–1000 Hz）。

几何直觉：nominal 沿非线性轨迹飞行，error state 是"轨迹切丛上的一束小扰动"。KF 的任务是在**切空间**（一个欧氏向量空间）中估计这束扰动；每次更新后把扰动"注入"回流形，并把协方差按新坐标重新变换（Reset step，见 §A3.5）。

---

§A3.4 ESKF 预测与更新——完整公式链 ⭐⭐

A3.4.1 Nominal 动力学（Solà §2.3.2, Eq. 126；§2.4.1, Eq. 150）

连续：

\[\dot{\mathbf{p}}=\mathbf{v},\quad \dot{\mathbf{v}}=\mathbf{R}(\mathbf{a}_m-\mathbf{a}_b)+\mathbf{g},\quad \dot{\mathbf{q}}=\tfrac{1}{2}\mathbf{q}\otimes(\boldsymbol{\omega}_m-\boldsymbol{\omega}_b)\]

偏置与重力建模为随机游走或常量：\(\dot{\mathbf{a}}_b=\mathbf{0},\ \dot{\boldsymbol{\omega}}_b=\mathbf{0},\ \dot{\mathbf{g}}=\mathbf{0}\)（驱动项放到 error-state 动力学）。

离散（零阶中点保号积分四元数）：

\[\mathbf{q}\leftarrow\mathbf{q}\otimes\mathbf{q}\{(\boldsymbol{\omega}_m-\boldsymbol{\omega}_b)\Delta t\},\quad \mathbf{q}\{\boldsymbol{\phi}\}=\begin{bmatrix}\cos(\|\boldsymbol{\phi}\|/2)\\ \tfrac{\boldsymbol{\phi}}{\|\boldsymbol{\phi}\|}\sin(\|\boldsymbol{\phi}\|/2)\end{bmatrix}\]

A3.4.2 Error-state 连续动力学（Solà §2.3.3, Eq. 127，局部/右摄动）

\[\begin{aligned} \dot{\delta\mathbf{p}} &= \delta\mathbf{v}\\ \dot{\delta\mathbf{v}} &= -\mathbf{R}[\mathbf{a}_m-\mathbf{a}_b]_\times\,\delta\boldsymbol{\theta}-\mathbf{R}\,\delta\mathbf{a}_b+\delta\mathbf{g}-\mathbf{R}\,\mathbf{a}_n\\ \dot{\delta\boldsymbol{\theta}} &= -[\boldsymbol{\omega}_m-\boldsymbol{\omega}_b]_\times\,\delta\boldsymbol{\theta}-\delta\boldsymbol{\omega}_b-\boldsymbol{\omega}_n\\ \dot{\delta\mathbf{a}}_b &= \mathbf{a}_w,\quad \dot{\delta\boldsymbol{\omega}}_b = \boldsymbol{\omega}_w,\quad \dot{\delta\mathbf{g}} = \mathbf{0} \end{aligned}\]

\(\mathbf{a}_n,\boldsymbol{\omega}_n\) 为加速度计/陀螺白噪声，\(\mathbf{a}_w,\boldsymbol{\omega}_w\) 为偏置随机游走噪声。

A3.4.3 离散传递矩阵 \(\mathbf{F}_\mathbf{x}\)（Solà §2.4.3, Eq. 160）

按 \(\delta\mathbf{x}=[\delta\mathbf{p},\delta\mathbf{v},\delta\boldsymbol{\theta},\delta\mathbf{a}_b,\delta\boldsymbol{\omega}_b,\delta\mathbf{g}]\) 排列：

\[\mathbf{F}_\mathbf{x}=\begin{bmatrix}\mathbf{I} & \mathbf{I}\Delta t & 0 & 0 & 0 & 0\\ 0 & \mathbf{I} & -\mathbf{R}[\mathbf{a}_m-\mathbf{a}_b]_\times\Delta t & -\mathbf{R}\Delta t & 0 & \mathbf{I}\Delta t\\ 0 & 0 & \mathbf{R}^\top\{(\boldsymbol{\omega}_m-\boldsymbol{\omega}_b)\Delta t\} & 0 & -\mathbf{I}\Delta t & 0\\ 0 & 0 & 0 & \mathbf{I} & 0 & 0\\ 0 & 0 & 0 & 0 & \mathbf{I} & 0\\ 0 & 0 & 0 & 0 & 0 & \mathbf{I}\end{bmatrix}\]

工程要点：\((3,3)\) 块采用**闭式 Rodrigues** \(\mathbf{R}^\top\{\boldsymbol{\omega}\Delta t\}\)（即 \(\exp(-[\boldsymbol{\omega}]_\times\Delta t)\)，附录 B.1 Eq. 241-242），而不是一阶近似 \(\mathbf{I}-[\boldsymbol{\omega}]_\times\Delta t\)——后者在高角速度（>几十 °/s）+ 大 \(\Delta t\) 时引入显著漂移。这也是 Solà 对传统文献的关键改写之一。

A3.4.4 噪声映射与冲量离散化（Solà §2.4.3, Eq. 161；附录 E）

按同一误差顺序，冲量噪声向量取

\[\mathbf{i}=[\mathbf{v}_i,\boldsymbol{\theta}_i,\mathbf{a}_i,\boldsymbol{\omega}_i]^\top,\]

其中 \(\mathbf{v}_i\) 是加速度白噪声形成的速度冲量，\(\boldsymbol{\theta}_i\) 是陀螺白噪声形成的小角度冲量，\(\mathbf{a}_i,\boldsymbol{\omega}_i\) 是两个 bias 随机游走冲量。常见协方差传播可写为

\[ \mathbf{F}_i= \begin{bmatrix} 0 & 0 & 0 & 0\\ \mathbf{R} & 0 & 0 & 0\\ 0 & \mathbf{I} & 0 & 0\\ 0 & 0 & \mathbf{I} & 0\\ 0 & 0 & 0 & \mathbf{I}\\ 0 & 0 & 0 & 0 \end{bmatrix}_{18\times 12}, \quad \mathbf{Q}_i=\mathrm{diag}(\mathbf{V}_i,\boldsymbol{\Theta}_i,\mathbf{A}_i,\boldsymbol{\Omega}_i). \]

具体实现的正负号要与误差定义保持一致；若 \(\mathbf{Q}_i\) 是块对角，单个块的符号不改变该块注入的方差，但会影响与其他噪声源存在相关性时的交叉项。

冲量方差（Eq. 152–155）：

\[\mathbf{V}_i=\sigma_{\tilde a_n}^2\Delta t^2\mathbf{I},\ \boldsymbol{\Theta}_i=\sigma_{\tilde\omega_n}^2\Delta t^2\mathbf{I},\ \mathbf{A}_i=\sigma_{a_w}^2\Delta t\,\mathbf{I},\ \boldsymbol{\Omega}_i=\sigma_{\omega_w}^2\Delta t\,\mathbf{I}\]

注意 \(\Delta t\) 次幂：这里的 \(\sigma_{\tilde a_n},\sigma_{\tilde\omega_n}\) 按 Solà 记号理解为**离散采样噪声标准差**，所以速度/角度冲量方差含 \(\Delta t^2\)；若你拿到的是连续白噪声密度或 PSD，则离散协方差应先按 \(Q_d\approx Q_c\Delta t\) 换算，再进入误差传播。随机游走项按连续噪声密度离散化时常见 \(\sigma^2\Delta t\)。把“离散噪声标准差”和“连续噪声密度”混用，是 ESKF 最常见的调参陷阱（高翔 SLAM-in-AD Issue #132 即为此讨论）。

A3.4.5 预测 + 更新伪代码

# PREDICT (每个 IMU tick)
a = a_m - b_a;  ω = ω_m - b_g
p += v*Δt + 0.5*(R*a + g)*Δt²
v += (R*a + g)*Δt
q  = q ⊗ Exp(ω*Δt)
R  = mat(q)
F_x = (如上，闭式 Rodrigues)
P   = F_x * P * F_x^T + F_i * Q_i * F_i^T

# UPDATE (传感器事件)
z_pred = h(x)
H      = ∂h/∂x_t * X_{δx}       # X_{δx} 含 Q_{δθ}, Eq.171c
S = H*P*H^T + V
K = P*H^T * S^{-1}
δx_hat = K * (z - z_pred)
P = (I - K*H) * P * (I - K*H)^T + K*V*K^T   # Joseph form
x = inject_nominal(x, δx_hat)                # 把误差态注入 nominal
G_reset = reset_jacobian(δx_hat)              # 坐标原点移动后的 reset Jacobian
P = G_reset * P * G_reset^T
δx_hat = 0

观测雅可比链式法则（§3.1.1, Eq. 168）：\(\mathbf{H} = \mathbf{H}_\mathbf{x}\cdot\mathbf{X}_{\delta\mathbf{x}}\)，其中 \(\mathbf{X}_{\delta\mathbf{x}}=\mathrm{blkdiag}(\mathbf{I}_6,\mathbf{Q}_{\delta\boldsymbol{\theta}},\mathbf{I}_9)\)，\(\mathbf{Q}_{\delta\boldsymbol{\theta}}\) 是 \(\partial\mathbf{q}/\partial\delta\boldsymbol{\theta}\)（Eq. 171c）。

---

§A3.5 Reset Step——许多工程代码里缺失的那一环 ⭐⭐

A3.5.1 动机

误差注入后 δx_hat ← 0，但**新的误差是相对于新 nominal 态局部定义**——因此旧协方差 \(\mathbf{P}\) 描述的"旧误差坐标系"不再成立，必须做一次**坐标重参数化**。这一步在 Solà-ESKF §3.3 被命名为 ESKF reset。忽略它会让长时间运行后协方差系统性偏移，使 \(\chi^2\) 检验误伤或漏报。

A3.5.2 推导（Solà §3.3.1, Eq. 179–185）

以姿态为例。真态不变：\(\mathbf{q}^+\otimes\delta\mathbf{q}^+ = \mathbf{q}\otimes\delta\mathbf{q}\)。注入：\(\mathbf{q}^+=\mathbf{q}\otimes\hat{\delta\mathbf{q}}\)。代入得

\[\delta\mathbf{q}^+ = \hat{\delta\mathbf{q}}^*\otimes\delta\mathbf{q}\]

小角线性化（\(\hat{\delta\mathbf{q}}^*\approx[1,-\tfrac{1}{2}\hat{\delta\boldsymbol{\theta}}^\top]^\top\)）：

\[\boxed{\delta\boldsymbol{\theta}^+ = -\hat{\delta\boldsymbol{\theta}} + \left(\mathbf{I}-\bigl[\tfrac{1}{2}\hat{\delta\boldsymbol{\theta}}\bigr]_\times\right)\delta\boldsymbol{\theta}}\]

A3.5.3 协方差变换（Eq. 176–178，完整公式）

\[\hat{\delta\mathbf{x}}\leftarrow\mathbf{0},\qquad \mathbf{P}\leftarrow\mathbf{G}\,\mathbf{P}\,\mathbf{G}^\top\]

\[\mathbf{G}=\begin{bmatrix}\mathbf{I}_6 & 0 & 0\\ 0 & \mathbf{I}-\bigl[\tfrac{1}{2}\hat{\delta\boldsymbol{\theta}}\bigr]_\times & 0\\ 0 & 0 & \mathbf{I}_9\end{bmatrix}\]

全局角误差（左摄动） 情形，符号翻转为 \(\mathbf{G}_{\theta\theta}=\mathbf{I}+[\tfrac{1}{2}\hat{\delta\boldsymbol{\theta}}]_\times\)（§4.3.3, Eq. 209, 216；注入变 \(\mathbf{q}\leftarrow\mathbf{q}\{\hat{\delta\boldsymbol{\theta}}\}\otimes\mathbf{q}\)）。

A3.5.4 何时不可省略

\(\|\hat{\delta\boldsymbol{\theta}}\|<10^{-3}\) rad 时 \(\mathbf{G}\approx\mathbf{I}\)，许多开源实现直接省略，短期运行看不出差别。但在大机动场景（航拍无人机急转、Cassie 奔跑、LiDAR 剧烈抖动）下单步 \(\|\hat{\delta\boldsymbol{\theta}}\|\) 可达 1°–5°，累计的协方差偏移会污染 \(\chi^2\) 门限，诱发 outlier 错判。Markley–Crassidis–Reynolds, JGCD 46(10), 2023 专门讨论了 MEKF 中此项的必要性。

---

§A3.6 MEKF——ESKF 在 \(SO(3)\) 上的历史原型 ⭐⭐

A3.6.1 Lefferts–Markley–Shuster 1982

文献：Lefferts E.J., Markley F.L., Shuster M.D., "Kalman Filtering for Spacecraft Attitude Estimation", JGCD 5(5):417–429, 1982（AIAA Paper 82-0070 先行版）。

核心思想：将真姿态写为 \(\bar{\mathbf{q}}\otimes\delta\mathbf{q}(\delta\boldsymbol{\alpha})\)，参考四元数 \(\bar{\mathbf{q}}\) 作为"nominal"，3 维小角误差 \(\delta\boldsymbol{\alpha}\) 作为 KF 状态。KF 只操作 3 维状态 + 陀螺偏置（共 6 维），彻底避开四元数过参数化奇异问题。航天领域标准选 JPL 约定（global-to-local，左手代数 \(ji=k\)）；Sommer et al. 2018 (arXiv:1801.07478) 批判了 Hamilton 与 JPL 的混用历史。

A3.6.2 Markley 2003 的误差表示对比

文献：Markley F.L., "Attitude Error Representations for Kalman Filtering", JGCD 26(2):311–317, 2003, DOI 10.2514/2.5048。

系统比较 Rotation vector / Gibbs vector / MRP / \(2\delta\mathbf{q}_v\) 四种三轴姿态误差。一阶下等价；二阶偏置不同；MRP 与 rotation vector 数值性能最佳、范围最广。同文引入的 norm-preserving quaternion reset 正是 Solà §3.3 的理论源头。

A3.6.3 MEKF = ESKF 的 \(SO(3)\) 特例

方面	MEKF	ESKF
状态	仅姿态 + 陀螺偏置（6 维）	姿态+位置+速度+双偏置+重力（15–18 维）
位置/速度	无	加性误差（与 KF 传统一致）
姿态误差	乘性 \(\delta\mathbf{q}\)	乘性 \(\delta\mathbf{q}\)（继承自 MEKF）
应用	航天器 AOCS、手机 AHRS	VIO、惯性导航、LiDAR-IO
约定	常用 JPL	Hamilton/JPL 均可

一句话：ESKF = MEKF 的 SE(3)/IMU-driven 推广。两者共享"乘性误差 + 最小维度 + 零点线性化"三大内核；现代视角（Solà micro Lie theory，arXiv:1812.01537）用 \(\boxplus/\boxminus\) 把二者统一。

---

§A3.7 Group-Affine 与 Log-Linear——InEKF 的数学基石 ⭐⭐⭐

A3.7.1 不变误差的两种定义（Barrau–Bonnabel TAC 2017, Eq. 5–6）

设状态 \(\chi_t\in G\)（矩阵李群），估计 \(\hat\chi_t\in G\)：

• 左不变误差 \(\eta^L_t = \chi_t^{-1}\hat\chi_t\)——对整体左乘 \(g\cdot\) 不变；
• 右不变误差 \(\eta^R_t = \hat\chi_t\chi_t^{-1}\)——对整体右乘 \(\cdot g\) 不变。

警告：文献约定不统一。Barrau–Bonnabel 原文按上式；Hartley 2020 写作 \(\eta^R=\hat X X^{-1}\)（同义，交换 \(X\leftrightarrow\hat X\) 就互为逆）。manif 的 rminus/lminus 是另一套命名，二者都要贯穿自己选的约定。

A3.7.2 Group-Affine 条件（Thm 1, Eq. 7）

与 5-D 的分工说明：本节（A3）从**算法流程与工程实现**角度给出 group-affine 定理的陈述与 \(SE_2(3)\) 验证；5-D（InEKF 论文精读）则给出 Theorem 1-4 的**逐步完整证明**（含 BCH 推导与 Lyapunov 稳定性论证）。两文档使用的符号约定一致（Barrau-Bonnabel TAC 2017），但 A3 侧重工程映射，D 侧重数学细节。

定义（Group-affine）：动力学 \(\dot\chi=f_u(\chi)\) 称为 group-affine，当且仅当对所有 \(t\) 和 \(a,b\in G\)：

\[\boxed{f_u(ab) = f_u(a)\,b + a\,f_u(b) - a\,f_u(e)\,b}\]

\(e\) 为单位元。等价性：下列三陈述等价（Thm 1）： 1. 左不变误差 \(\eta^L\) 的动力学**与状态轨迹无关**（state-trajectory-independent）； 2. 右不变误差 \(\eta^R\) 的动力学与状态轨迹无关； 3. 上述 group-affine 恒等式成立。

直觉：在 \((G,+)\) 即向量空间时，group-affine 恒等式退化为 \(f(a+b)=f(a)+f(b)-f(0)\)，就是中学意义的"仿射函数"。把它搬到李群上，就是 group-affine。例子：纯左不变动力学 \(f(\chi)=\chi\omega\)、纯右不变动力学 \(f(\chi)=v\chi\)、组合 \(f_{v,\omega}(\chi)=v\chi+\chi\omega\) 均满足（Remark 1）。

A3.7.3 IMU 动力学是 group-affine（\(SE_2(3)\) 上的证明）

IMU 连续模型 \(\dot R=R\omega^\wedge,\ \dot v=Ra+g,\ \dot p=v\)，在 \(SE_2(3)\) 上写成 \(5\times 5\) 矩阵形式：

\[f_u(X) = \begin{bmatrix}R\omega_\times & Ra+g & v\\ 0 & 0 & 0\\ 0 & 0 & 0\end{bmatrix}\]

关键观察：\(f_u(X)\) 只涉及 \(R,v\) 的线性组合（\(p\) 根本不出现在右端），重力 \(g\) 作常量输入。将 \(X_1 X_2\) 代入展开后的 \(f_u(X_1X_2)\) 与右端 \(f_u(X_1)X_2 + X_1 f_u(X_2) - X_1 f_u(\mathrm{Id})X_2\) 分别对应相等（逐块验证：旋转块用 \(R_2\omega_\times=(R_2\omega)_\times R_2\) 恒等式消项；速度块 \(g\) 对等偏移；位置块 \(v\) 自然匹配）。这就是 Barrau 选择 \(SE_2(3)\) 而非 \(SO(3)\times\mathbb{R}^3\times\mathbb{R}^3\) 的根本原因——后者上同样的动力学**不**满足 group-affine。

A3.7.4 Log-Linear 定理（Thm 2）

定义 \(\xi_t\in\mathbb{R}^{\dim\mathfrak{g}}\) 为 \(\eta_t=\exp(\xi_t^\wedge)\) 的李代数坐标。若无噪声系统 group-affine，则存在**仅依赖 \(u_t\)**（不依赖 \(\chi_t\)、不依赖 \(\hat\chi_t\)）的矩阵 \(A_t\) 使得

\[\boxed{\dot\xi_t = A_t\,\xi_t}\]

精确（不是近似！）描述无噪声非线性误差动力学：若 \(\xi_t\) 由此线性 ODE 演化，则 \(\eta_t=\exp(\xi_t^\wedge)\) 对所有 \(t\) 精确成立（Proposition 2）。工程滤波里的过程噪声再作为一阶随机扰动项 \(B_tn_t\) 叠加进协方差传播，不能把“带噪声精确线性”误读为定理本身。对 IMU，

\[A = \begin{bmatrix}0 & 0 & 0\\ (g)_\times & 0 & 0\\ 0 & I & 0\end{bmatrix}_{9\times 9},\text{ time-invariant, nilpotent of degree 3}\]

离散化闭式：

\[\Phi = \exp(A\Delta t) = \begin{bmatrix}I & 0 & 0\\ (g)_\times\Delta t & I & 0\\ \tfrac{1}{2}(g)_\times\Delta t^2 & I\Delta t & I\end{bmatrix}\]

A3.7.5 稳定性定理（Thm 4）

Theorem 4 (IEKF 稳定观测器)：考虑 group-affine 系统配左（右）不变观测。若 Deyst–Price 1968 条件（一致可控/可观 + 噪声协方差有界）对真实轨迹的线性化系统成立，则 InEKF 估计 \(\hat\chi_t\) 为 \(\chi_t\) 的渐近稳定观测器，收敛半径 \(\varepsilon>0\) 与初始化时间 \(t_0\) 无关。

证明要点（附录 C）： 1. 比较对数误差 \(\xi_t\) 与其线性化 \(\xi_t^{\rm lin}\)； 2. 由于 \(A_t\) 独立于 \(\hat\chi_t\)，高阶项 \(\mathcal{O}(\|\xi\|^2)\) 可被一致 bound（EKF 做不到这一点）； 3. 用 Deyst–Price 的 Lyapunov 构造 \(V=\xi^\top P^{-1}\xi\) 推出指数衰减。

---

§A3.8 InEKF 完整算法（Right-Invariant 版） ⭐⭐⭐

A3.8.1 状态与观测分类

状态：\(\hat X_k\in SE_2(3)\)，\(\hat X=\begin{bmatrix}\hat R & \hat v & \hat p\\ 0&1&0\\0&0&1\end{bmatrix}\)，协方差 \(P_k\in\mathbb{R}^{9\times 9}\)。

观测类型选择原则（与 5-D 的 Barrau-Bonnabel 分类一致）： - \(Y=Xd+V\) 型观测为左不变观测，常见例子是 GPS/绝对位置这类世界量被状态直接映射到观测空间，常配 LI-EKF。 - \(Y=X^{-1}d+V\) 型观测为右不变观测，常见例子是世界 landmark、磁力计参考向量、接触足端相对位置等被表达到机体系或局部坐标，常配 RI-EKF。 - Contact foot kinematics 在 Hartley 2020 里写成 \(R^\top(d-p) = h_p(\alpha)\)，属于右不变观测（见 §A3.9）。

A3.8.2 预测步

状态传播（数值积分 RK4 或 Euler）：

\[\hat R_{k+1|k} = \hat R_k\exp((\omega_m\Delta t)^\wedge),\ \hat v_{k+1|k} = \hat v_k + (\hat R_k a_m + g)\Delta t,\ \hat p_{k+1|k} = \hat p_k + \hat v_k\Delta t + \tfrac{1}{2}(\hat R_k a_m + g)\Delta t^2\]

协方差传播：

\[\Phi_k = \exp(A\Delta t),\quad \bar Q_k = \int_0^{\Delta t}\exp(A(\Delta t-\tau))\,G_kQ_cG_k^\top\,\exp(A(\Delta t-\tau))^\top\,d\tau$$ $$P_{k+1|k} = \Phi_k\,P_k\,\Phi_k^\top + \bar Q_k\]

关键点：\(A\) 为常数（只依赖 \(g\)），与 \(\hat X_k\) 无关——这是 Log-Linear 定理的直接结果。\(\bar Q_k\) 可用 Van Loan 方法精确离散化；若工程上用 \(\Phi_kG_kQ_cG_k^\top\Phi_k^\top\Delta t\)，它只是小步长端点近似。\(G_k\) 是把 6 维 IMU 白噪声映射到 9 维右不变误差切空间的噪声 Jacobian；若用 Adjoint 写法，也必须先明确从 6 维噪声到 9 维误差的嵌入矩阵，不能直接把 \(9\times 9\) 的 \(\mathrm{Ad}_{\hat X}\) 与 \(6\times 6\) 的噪声协方差相乘。

A3.8.3 更新步（右不变观测）

Innovation（乘法形式）：设观测 \(Y_k = X_k^{-1}d + V_k\)（已知参考向量 \(d\) 固定在 world frame），则

\[z_k = \hat X_{k|k-1}\,Y_k - d \;\approx\; H\,\xi + \tilde V_k\]

对单个 landmark \(\ell_i\)（world 坐标），在 \(\eta^R=\hat X X^{-1}\approx \exp(\xi^\wedge)\) 且 \(\xi=[\delta\theta,\delta v,\delta p]\) 的约定下，\(\eta^R[\ell_i;0;1]-[\ell_i;0;1]\) 给出观测雅可比 \(H_i = [-(\ell_i)_\times,\ 0_{3\times 3},\ I_3]\)（不依赖 \(\hat X\)）。

Kalman 增益：

\[S_k = H\,P_{k|k-1}\,H^\top + \hat N_k$$ $$K_k = P_{k|k-1}\,H^\top\,S_k^{-1}\]

其中 \(\hat N_k\) 是把观测噪声 \(N_k\) 通过当前线性化点映射到 innovation 坐标后的协方差。不同传感器的 \(N_k\) 原始坐标不同，不应机械套用同一个 \(R^\top N R\) 公式。

状态更新（乘法/指数更新——核心！）：

\[\delta\xi_k^{\mathrm{corr}} = -K_k z_k \in\mathbb{R}^9,\qquad \boxed{\hat X_{k|k} = \exp((\delta\xi_k^{\mathrm{corr}})^\wedge)\,\hat X_{k|k-1}}\]

这里的负号来自当前约定：\(z_k\approx H\xi\) 估计的是 \(\eta^R=\hat X X^{-1}\) 的误差方向，修正估计时要施加反向误差。若改用 \(d-\hat X Y\) 作为 innovation，则可把负号吸收到 residual 定义中。与之对照：Left-Invariant 版同样必须让 residual、误差定义和乘法方向三者配套。

协方差更新（Joseph form 推荐）：

\[P_{k|k} = (I - K_k H)P_{k|k-1}(I-K_kH)^\top + K_k\hat N_k K_k^\top\]

A3.8.4 伪代码

Right-Invariant InEKF (SE_2(3))
Input: IMU (ω_m, a_m) @ Δt; landmark obs {Y_i}
Init:  X̂_0, P_0
for each step k:
    # ---- PREDICT ----
    X̂_{k+1|k} = 群指数积分(X̂_k, ω_m, a_m, Δt)
    A = [[0,0,0],[g^,0,0],[0,I,0]]           # 常数 9x9
    Φ = expm(A Δt)
    G_k = 噪声嵌入与坐标变换 Jacobian              # 9x6
    Q̄ = ∫ exp(A(Δt-τ)) G_k diag(Q_ω,Q_a) G_k^T exp(A(Δt-τ))^T dτ
    P_{k+1|k} = Φ P_k Φ^T + Q̄
    # ---- UPDATE (对每个 landmark i) ----
    z = X̂ · Y_i - d_i                         # Y_i = X^{-1} d_i + V_i
    H = [-(ℓ_i)^, 0, I]
    N̂ = 将观测噪声 N_i 映射到 innovation 坐标
    S = H P H^T + N̂
    K = P H^T S^{-1}
    δξ_corr = -K z
    X̂_{k|k} = expm(δξ_corr^) · X̂_{k|k-1}    # ★ residual 定义决定修正符号
    P_{k|k}  = (I-KH) P (I-KH)^T + K N̂ K^T

---

§A3.9 \(SE_2(3)\) 与 Contact-Aided InEKF（Cassie 实战） ⭐⭐⭐

A3.9.1 为什么腿式机器人是 InEKF 最干净的应用

足部接触 + IMU 的传感器组合产生极大的初始姿态/速度不确定性（脚刚落地、摔倒恢复、奔跑）；标准 EKF 在 \(\pm 30^\circ\) 初始姿态误差下可能发散，而 InEKF 因 log-linear 结构在该范围内仍收敛。文献：Hartley, Ghaffari, Eustice, Grizzle, "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation", IJRR 2020, arXiv:1904.09251（扩展自 RSS 2018, arXiv:1805.10410）。

A3.9.2 状态流形 \(SE_{N+2}(3)\)

\(N\) 个接触点时，把接触点位置 \(d_i\) 加入矩阵群：

\[X_t = \begin{bmatrix}R & v & p & d_1 & \cdots & d_N\\ 0 & 1 & 0 & 0 & \cdots & 0\\ 0 & 0 & 1 & 0 & \cdots & 0\\ 0 & 0 & 0 & 1 & \cdots & 0\\ \vdots & & & & \ddots & \\ 0 & 0 & 0 & 0 & \cdots & 1\end{bmatrix}\in SE_{N+2}(3)\]

维度 \(\dim\mathfrak{g} = 3(N+3)\)（\(N\) 接触 → \(9+3N\)）。

A3.9.3 Forward Kinematics 作为 Right-Invariant 观测

脚相对机体位置（编码器读数）\(h_p(\tilde\alpha)=R^\top(d-p)+J_v w^\alpha\) 可以写为 \(Y_t = X_t^{-1}b + V_t\) 形式，观测矩阵

\[H = [0_{3\times 3},\ 0,\ -I,\ I]\]

（对 \(\xi^R,\xi^v,\xi^p,\xi^d\) 列），与 \(\hat X\) 无关——这正是选 RI-EKF 的原因。

A3.9.4 接触事件处理

• 添加接触（脚落地）：\(\hat d=\hat p + \hat R\,h_p(\tilde\alpha)\)；协方差扩增 \(P^\text{new}=F_a P F_a^\top + G_t\Sigma^\alpha G_t^\top\)。
• 移除接触（脚抬起）：\(F_r\) 裁剪 \(d\) 的行列，\(P^\text{new}=F_r P F_r^\top\)。
• 二元接触检测：Cassie 用弹簧挠度编码器 + 阈值。

A3.9.5 Bias 增广——Imperfect InEKF

负面事实（Barrau PhD 2015）："There is no Lie group that includes the bias terms while also having the dynamics satisfy the group-affine property"。bias 的加性作用经 \(R\) 旋转后与姿态耦合，无论怎样设计群乘法都无法同时保持加性与 group-affine。

折衷方案（Hartley 2020）：状态流形 \(\mathcal{M} = SE_{N+2}(3)\times\mathbb{R}^6\)，bias \(\theta=(b_g,b_a)\) 作**欧氏附加状态**，pose 部分仍用右不变误差，bias 用加性误差；放弃严格 log-linear 性质但保留大部分优势。

Jacobian（连续，右不变）出现 bias-to-pose 耦合：

\[F = \begin{bmatrix}0 & 0 & 0 & -\hat R & 0\\ (g)_\times & 0 & 0 & -(\hat v)_\times\hat R & -\hat R\\ 0 & I & 0 & -(\hat p)_\times\hat R & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\end{bmatrix}\]

Pose 主块（左上 \(9\times 9\)）仍与 \(\hat X\) 无关；只有 bias 耦合项依赖。Cassie 实验（100 次 Monte-Carlo，初始 Euler \(\pm 30^\circ\)、速度 \(\pm 1\) m/s）显示 Imperfect InEKF 仍显著优于 quaternion-based EKF。

A3.9.6 代码映射

仓库	语言	关键文件/类
RossHartley/invariant-ekf	C++/BSD-3	include/InEKF.h, RobotState.h, Kinematics.h, Landmark.h；src/examples/kinematics.cpp
RossHartley/invariant-ekf-ros	C++/ROS	ROS 封装
UMich-BipedLab/Contact-Aided-Invariant-EKF	MATLAB	RSS 2018 参考实现

---

§A3.10 IKFoM 与 FAST-LIO2——iterated vs invariant 的名字冲突 ⭐⭐

A3.10.1 名字辨析

• IKFoM：**I**terated **K**alman **F**ilter **o**n **M**anifolds（He-Xu-Zhang ICRA 2021，arXiv:2102.03804）。"I" = iterated（Gauss-Newton 多次迭代），不是 invariant。
• InEKF：In**variant **EKF（Barrau-Bonnabel TAC 2017）。
• IEKF（Bell-Cathey TAC 1993）："I**terated **EKF"，证明等价 Gauss-Newton——这是 iterated 源头，与 invariant 无关。

阅读文献的生存法则：看到 "IEKF" 先确认是 iterated 还是 invariant；两者完全不同。

A3.10.2 IKFoM 核心思想

对混合流形 \(\mathcal{S} = \mathbb{R}^m\times SO(3)\times\cdots\times SO(3)\times S^2\times\cdots\)，用户只需定义每个子流形的 \(\boxplus/\boxminus\)（Hertzberg-Wagner-Frese-Schröder），系统自动合成 iterated ESKF。规范形式：

\[x_{k+1} = x_k\boxplus\bigl(\Delta t\,f(x_k,u_k,w_k)\bigr)\]

对 \(SO(3)\) 的 \(\boxplus\) 默认**右乘**：\(R\boxplus\phi=R\cdot\mathrm{Exp}(\phi)\)。Jacobian 中出现 \(A(\cdot)^{-1}\) 即 \(SO(3)\) 的右 Jacobian 逆 \(J_r^{-1}\)。

与 InEKF 的关键区别：IKFoM 不要求 group-affine，可处理任意混合流形；但也因此**不继承** log-linearity。FAST-LIO2 的状态 \(SO(3)\times\mathbb{R}^{15}\times SO(3)\times\mathbb{R}^3\) 是"分离式"表示，与 \(SE_2(3)\) 矩阵群**不等价**——所以 FAST-LIO2 在理论上是一般 EKF-on-manifold 的局部收敛保证，不是 InEKF 的 trajectory-independent 吸引域。但 LiDAR 信息丰富，这个理论差距在实际应用中不明显。

A3.10.3 FAST-LIO2 状态与残差（Xu et al. T-RO 2022, arXiv:2107.06829）

状态（存储 24 维，切空间 23 维）：

\[\mathcal{M} = SO(3)\times\mathbb{R}^{12}\times S^2\times SO(3)\times\mathbb{R}^3,\quad x=[{}^GR_I,{}^Gp_I,{}^Gv_I,b_\omega,b_a,{}^Gg,{}^IR_L,{}^Ip_L]\]

重力 \(g\) 用 \(S^2\)（2 维切空间）而非 \(\mathbb{R}^3\)，保证 \(\|g\|=9.81\) 始终成立。

LiDAR residual（点-面，无特征提取）：

\[0 = z_j^\kappa = u_j^\top\bigl({}^GT_{I_k}^\kappa\,{}^IT_{L_k}^\kappa\,{}^Lp_j - {}^Gq_j\bigr)\]

\(u_j,q_j\) 由 ikd-Tree 检索 5 近邻拟合的平面法向与质心。

快 Kalman 增益公式（关键效率点）：

\[K = (H^\top R^{-1} H + P^{-1})^{-1} H^\top R^{-1}\]

维度为**状态**（\(\sim 24\)）而非测量（\(\sim\) 数千），避免 \(m\times m\) 求逆。

A3.10.4 IKFoM 代码结构（hku-mars/IKFoM）

唯一必需 include：#include <esekfom/esekfom.hpp>。核心宏：

MTK_BUILD_MANIFOLD(state,
   ((vect3, pos))((vect3, vel))((SO3, rot))
   ((vect3, bg))((vect3, ba))((S2, grav))
   ((SO3, offset_R_L_I))((vect3, offset_T_L_I)));

MTK_BUILD_MANIFOLD 用 Boost.Preprocessor 把 ((type,name)) 列表展开为 compound manifold，自动合成 \(\boxplus,\boxminus\)、切空间维度、Jacobian。基元类型：MTK::SO3<double>、MTK::vect<3,double>、MTK::S2<double,98,10,1>（长度 98/10=9.8，原点向量选 \(e_1\)）。

滤波器模板：esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>，成员 predict(dt,Q,in)、update_iterated(z,R)；"share" 版 init_dyn_share(f,df_dx,df_dw,h_dyn_share,...) 一次性填入以节省重复计算（FAST-LIO2 用此）。

A3.10.5 FAST-LIO vs FAST-LIO2 对照

	FAST-LIO (RA-L 2021)	FAST-LIO2 (T-RO 2022)
地图	静态 k-d tree + FoV 截取	ikd-Tree（增量、box-delete、on-tree downsample、并行 rebuild）
残差	LOAM-style 边/面特征	直接点-面，无特征提取
外参	无	在线估计 \({}^IT_L\)
工具箱	手写 ESKF	IKFoM
规模	短程	km 级，支持 ARM

---

§A3.11 UKF-M——免雅可比的流形 UKF ⭐⭐⭐

A3.11.1 动机与核心思想

文献：Brossard, Barrau, Bonnabel, "A Code for Unscented Kalman Filtering on Manifolds (UKF-M)", ICRA 2020, arXiv:2002.00878。

在李代数（切空间）生成 sigma 点 → 通过 retraction \(\varphi\) 映射到流形 → 传播/观测 → 通过 \(\varphi^{-1}\) 映射回切空间计算均值/协方差。好处：免解析 Jacobian；代价：每步需 \(2(d+q)+1\) 次 \(f/h\) 求值。

更进一步，UKF-M 把 Lie 群推广到**可平行化流形**（parallelizable manifold）——存在全局光滑向量场基底。所有 Lie 群可平行化；\(S^2\) 不可平行化（毛球定理），需通过 lifting 到 \(SO(3)\) 处理。

不确定性表示：\(\chi = \varphi(\hat\chi,\xi),\ \xi\sim\mathcal{N}(0,P)\)，\(\varphi\) 满足 \(\varphi(\hat\chi,0)=\hat\chi\) 且 \(\partial_\xi\varphi(\hat\chi,0)=I\)。

A3.11.2 三种 retraction 变体（Lie 群情形）

变体	\(\varphi(\hat\chi,\xi)\)	\(\varphi^{-1}_{\hat\chi}(\chi)\)	等价误差
右扰动（Eq. 17）	\(\hat\chi\exp(\xi^\wedge)\)	\(\log(\hat\chi^{-1}\chi)\)	左不变误差
左扰动（Eq. 18）	\(\exp(\xi^\wedge)\hat\chi\)	\(\log(\chi\hat\chi^{-1})\)	右不变误差
欧氏混合（Eq. 19）	\((\exp(\xi_1^\wedge)\hat\chi_1,\hat\chi_2+\xi_2)\)	\((\log(\chi_1\hat\chi_1^{-1}),\chi_2-\hat\chi_2)\)	\(G\times\mathbb{R}^N\)（含 bias）

在惯性导航实验中（论文 Fig. 2），\(SE_2(3)\)-UKF（右扰动）在 45° 初始航向误差下显著优于 \(SO(3)\times\mathbb{R}^6\)-UKF 与原版 EKF。

A3.11.3 完整算法（Algorithm 2）

Sigma 点权重：\(\lambda=(\alpha^2-1)d\)，\(w_j=1/(2(d+\lambda))\)，\(w_m=\lambda/(\lambda+d)\)，\(w_0=w_m + 3 - \alpha^2\)。UKF-M 把标准 UKF 的三参数 \((\alpha,\beta,\kappa)\) 吸收为单 \(\alpha\)。

# PROPAGATION
χ̂_n = f(χ̂_{n-1}, ω_n, 0)
ξ_j = ±col(√((λ+d)P_{n-1}))_j,  j=1..2d
χ_n^j = f(φ(χ̂_{n-1}, ξ_j), ω_n, 0)
ξ_j^+ = φ^{-1}_{χ̂_n}(χ_n^j)
Σ_n = Σ w_j ξ_j^+ (ξ_j^+)^T
# 噪声 sigma
w_j = ±col(√((λ+q)Q_n))_j
χ̃^j = f(χ̂_{n-1}, ω_n, w_j)
Q_{ξ,n} = Σ w_j φ^{-1}(χ̃^j) φ^{-1}(χ̃^j)^T
P_n = Σ_n + Q_{ξ,n}

# UPDATE
ξ_j = ±col(√((λ+d)P_n))_j
y_j = h(φ(χ̂_n, ξ_j));  ȳ = Σ w_j y_j
P_yy = Σ w_j (y_j-ȳ)(y_j-ȳ)^T + R_n
P_ξy = Σ w_j ξ_j (y_j-ȳ)^T
K = P_ξy P_yy^{-1}
χ̂_n^+ = φ(χ̂_n, K(y_n-ȳ))
P_n^+ = P_n - K P_yy K^T

工程简化：均值不通过 weighted-mean-on-manifold（Brossard-Bonnabel-Condomines IROS 2017 需迭代），而直接用 \(f(\hat\chi,\omega,0)\)——这是 UKF-M 相较前人的关键简化。

A3.11.4 ukfm Python 库（CAOR-MINES-ParisTech/ukfm）

核心类 ukfm.UKF：

ukf = ukfm.UKF(f, h, phi, phi_inv, Q, R, alpha, state0, P0)
ukf.propagation(omega, dt)
ukf.update(y)

用户必须提供 phi/phi_inv（即 \(\boxplus/\boxminus\)）。下面是常见的 \(SO(3)\times\mathbb{R}^6\) product retraction 写法，许多工程代码会用它作为简化模型：

def phi(state, xi):
    return STATE(Rot=state.Rot.dot(SO3.exp(xi[0:3])),
                 v  =state.v + xi[3:6],
                 p  =state.p + xi[6:9])
def phi_inv(state, hat_state):
    return np.hstack([SO3.log(hat_state.Rot.T.dot(state.Rot)),
                      state.v - hat_state.v,
                      state.p - hat_state.p])

若要实现严格的 \(SE_2(3)\) 群 retraction，速度和位置列不是简单欧氏加法，而会随旋转扰动一起通过 \(R J_l(\phi)\) 等项耦合。教学代码先写 product retraction，是为了让 \(\boxplus/\boxminus\) 接口清楚；论文复现实验必须核对所用库的真实群运算。

examples/ 包含 2D 机器人定位、3D 姿态估计、惯性导航、2D SLAM、IMU-GNSS KITTI 融合、球面钟摆六个标准 benchmark，每个均附 Monte-Carlo 对比 EKF 与多种 \(\varphi\)。

A3.11.5 UKF-M 完整数值示例：SE(2) 上的 2D 机器人定位 ⭐⭐⭐

前面 §A3.11.3 给出了 UKF-M 的算法骨架，但伪代码中"Exp/Log 映射"、"流形均值"、"切空间协方差"到底如何落成数字？本节用一个足够简单的例子——SE(2) 上的 2D 移动机器人——把每一步都展开成可手算的具体数值，让读者亲身体验 UKF-M 与标准 UKF 的本质差异。

问题设定

状态：\(\chi=(R_\theta,\, t_x,\, t_y)\in SE(2)\)，其中 \(R_\theta\in SO(2)\) 是朝向角 \(\theta\) 对应的旋转矩阵，\((t_x,t_y)\) 是平面位置。用 \(3\times 3\) 齐次矩阵表示：

\[ \chi=\begin{bmatrix}\cos\theta & -\sin\theta & t_x\\ \sin\theta & \cos\theta & t_y\\ 0 & 0 & 1\end{bmatrix}\in SE(2). \]

李代数 \(\mathfrak{se}(2)\) 是 3 维的，切向量 \(\xi=(\delta\theta,\,\delta x,\,\delta y)^\top\)。

过程模型：匀速+匀角速度模型。给定控制输入 \(u=(v,\omega)\)（线速度和角速度），一步传播为

\[ \chi_{k+1}=\chi_k \cdot \operatorname{Exp}_{SE(2)}\!\bigl((u+w)\Delta t\bigr), \]

其中 \(w\sim\mathcal{N}(0,Q)\) 是过程噪声，\(\Delta t\) 是时间步长。这里采用右乘约定（body-frame 速度）。

观测模型：对一个已知 landmark \(\ell=(l_x,l_y)^\top\) 的距离-方位角观测：

\[ y=h(\chi)=\begin{bmatrix}\sqrt{(l_x-t_x)^2+(l_y-t_y)^2}\\ \operatorname{atan2}(R_\theta^\top(\ell-t))_2,\,(R_\theta^\top(\ell-t))_1)\end{bmatrix}+n,\quad n\sim\mathcal{N}(0,R). \]

距离-方位角观测对朝向角高度非线性——这正是 UKF-M 相对于标准 UKF 的优势场景。

数值参数

取以下具体数值（单位：SI）：

参数	值
\(\Delta t\)	1.0 s
初始状态 \(\hat\chi_0\)	\(\theta_0=\tfrac{\pi}{3}\)（60°），\(t_0=(1,\,2)\)
初始切空间协方差 \(P_0\)	\(\text{diag}(0.1^2,\, 0.5^2,\, 0.5^2)\)
控制输入 \(u\)	\(v=1.0\) m/s, \(\omega=0.3\) rad/s
过程噪声 \(Q\)	\(\text{diag}(0.05^2,\, 0.1^2,\, 0.1^2)\)
Landmark \(\ell\)	\((5,\, 5)\)
观测噪声 \(R\)	\(\text{diag}(0.3^2,\, 0.05^2)\)
UKF-M \(\alpha\)	\(\sqrt{3}\)（标准推荐值）

状态维度 \(d=3\)，故 sigma 点数 \(2d+1=7\)。

Step 1：SE(2) 上的 Exp 与 Log

\(SE(2)\) 的指数映射把切向量 \(\xi=(\delta\theta,\delta x,\delta y)\) 变成群元素：

\[ \operatorname{Exp}(\xi)=\begin{bmatrix}\cos\delta\theta & -\sin\delta\theta & V_1\delta x-V_2\delta y\\ \sin\delta\theta & \cos\delta\theta & V_2\delta x+V_1\delta y\\ 0 & 0 & 1\end{bmatrix}, \]

其中

\[ V_1=\frac{\sin\delta\theta}{\delta\theta},\quad V_2=\frac{1-\cos\delta\theta}{\delta\theta}. \]

当 \(\delta\theta\to 0\) 时 \(V_1\to 1,V_2\to 0\)，退化为欧氏平移。对数映射是其逆运算：

\[ \operatorname{Log}(\chi)=\begin{bmatrix}\theta\\ V_1^{-1}(V_1 t_x+V_2 t_y)\\ V_1^{-1}(-V_2 t_x+V_1 t_y)\end{bmatrix}, \]

其中 \(\theta=\operatorname{atan2}(\sin\theta,\cos\theta)\)，\(V_1,V_2\) 用 \(\theta\) 计算。

为什么要用 Exp/Log 而不是直接加减？因为 \(\theta\) 是角度，两个朝向 \(\theta_1=170°\) 和 \(\theta_2=-170°\) 的"距离"应该是 \(20°\) 而不是 \(340°\)。Exp/Log 天然在李群上做减法，自动处理角度缠绕（wrapping）。

Step 2：生成 sigma 点（在切空间）

UKF-M 的核心：sigma 点在切空间生成，然后通过 Exp 映射到流形。

计算 \(\lambda=(\alpha^2-1)d=(\sqrt{3}^2-1)\cdot 3=(3-1)\cdot 3=6\)。令 \(c=\sqrt{d+\lambda}=\sqrt{3+6}=3\)。

对 \(P_0=\text{diag}(0.01,\, 0.25,\, 0.25)\) 做 Cholesky 分解：\(L=\text{diag}(0.1,\, 0.5,\, 0.5)\)。

7 个切空间 sigma 点为：

\[ \xi_0 = (0,0,0)^\top,\quad \xi_{j}=+c\cdot L_{:,j},\quad \xi_{j+3}=-c\cdot L_{:,j},\quad j=1,2,3. \]

具体数值：

编号	\(\xi_j=(\delta\theta,\delta x,\delta y)\)
\(\xi_0\)	\((0,\, 0,\, 0)\)
\(\xi_1\)	\((+0.3,\, 0,\, 0)\)
\(\xi_2\)	\((0,\, +1.5,\, 0)\)
\(\xi_3\)	\((0,\, 0,\, +1.5)\)
\(\xi_4\)	\((-0.3,\, 0,\, 0)\)
\(\xi_5\)	\((0,\, -1.5,\, 0)\)
\(\xi_6\)	\((0,\, 0,\, -1.5)\)

Step 3：映射到流形并传播

把每个 \(\xi_j\) 通过 retraction 映射到流形上的 sigma 点：

\[ \mathcal{X}_j=\varphi(\hat\chi_0,\xi_j)=\hat\chi_0\cdot\operatorname{Exp}(\xi_j). \]

以 \(\xi_1=(0.3,0,0)\) 为例：\(\operatorname{Exp}(\xi_1)\) 只旋转 0.3 rad，不平移：

\[ \operatorname{Exp}(0.3,0,0)=\begin{bmatrix}\cos0.3 & -\sin0.3 & 0\\ \sin0.3 & \cos0.3 & 0\\ 0 & 0 & 1\end{bmatrix}\approx\begin{bmatrix}0.9553 & -0.2955 & 0\\ 0.2955 & 0.9553 & 0\\ 0 & 0 & 1\end{bmatrix}. \]

然后 \(\mathcal{X}_1=\hat\chi_0\cdot\operatorname{Exp}(\xi_1)\) 得到朝向角 \(\theta_0+0.3\approx 1.347+0.3=1.647\) rad 的群元素（位置不变，因为这是 body-frame 右乘纯旋转）。

关键对比：标准 UKF 会直接对 \((\theta, t_x, t_y)\) 生成 sigma 点 \(\theta_0\pm c\sigma_\theta\)，这在 \(\theta\) 接近 \(\pm\pi\) 时产生缠绕灾难。UKF-M 的 sigma 点通过 Exp 映射，天然在群上移动，永远不会跨越角度奇点。

接下来对每个 \(\mathcal{X}_j\) 执行过程模型传播：

\[ \mathcal{X}_j^+=f(\mathcal{X}_j,\, u,\, 0)=\mathcal{X}_j\cdot\operatorname{Exp}\bigl((0.3,\,1.0,\,0)\cdot 1.0\bigr). \]

这里控制输入 \((\omega,v,0)=(0.3,1.0,0)\) 乘以 \(\Delta t=1\)，得到一步的 body-frame 位移向量 \(\xi_u=(0.3,1.0,0)\)。

Step 4：恢复传播均值（Fréchet 均值的简化）

标准做法是对流形上的 7 个传播后 sigma 点 \(\{\mathcal{X}_j^+\}\) 求 Fréchet 均值——需要迭代优化 \(\min_{\bar\chi}\sum w_j d(\bar\chi,\mathcal{X}_j^+)^2\)。这在高维李群上计算量大。

UKF-M 的关键简化（Brossard-Barrau-Bonnabel ICRA 2020）：直接用无噪声传播作为新均值：

\[ \hat\chi_1^-=f(\hat\chi_0,u,0)=\hat\chi_0\cdot\operatorname{Exp}(\xi_u). \]

这避免了迭代 Fréchet 均值，是 UKF-M 的核心工程贡献。

本质洞察：UKF-M 的 sigma 点从一开始就"住在"流形上——它们不是被裁剪到流形的欧氏点，而是通过 Exp 映射自然生成的群元素。这保证了每个 sigma 点都是合法的刚体位姿，不存在"非单位旋转矩阵"或"非归一化四元数"的问题。标准 UKF 在环境空间（如四元数的 \(\mathbb{R}^4\)）中生成 sigma 点，必须事后归一化，而归一化本身破坏了 sigma 点间的统计对称性。

Step 5：切空间协方差恢复

把每个传播后的 sigma 点拉回到以 \(\hat\chi_1^-\) 为原点的切空间：

\[ \xi_j^+=\varphi^{-1}_{\hat\chi_1^-}(\mathcal{X}_j^+)=\operatorname{Log}\bigl((\hat\chi_1^-)^{-1}\cdot\mathcal{X}_j^+\bigr). \]

然后计算协方差：

\[ P_1^-=\sum_{j=0}^{6}w_j\,\xi_j^+(\xi_j^+)^\top+Q_\xi, \]

其中 \(Q_\xi\) 是噪声 sigma 点传播的贡献（同 §A3.11.3 算法中的噪声 sigma 部分）。

角度缠绕的自动处理：因为 \(\xi_j^+\) 是通过 Log 映射计算的，它自动给出 \((-\pi,\pi]\) 内的最短弧度差。如果两个朝向分别是 \(175°\) 和 \(-175°\)，Log 给出的差是 \(10°\) 而不是 \(350°\)——这正是流形方法的核心优势。

Step 6：观测更新

在 \(\hat\chi_1^-\) 处重新生成 sigma 点（用新的 \(P_1^-\)），过观测函数 \(h\)：

\[ y_j=h(\varphi(\hat\chi_1^-,\xi_j')),\quad \bar y=\sum w_j y_j, \]

\[ P_{yy}=\sum w_j(y_j-\bar y)(y_j-\bar y)^\top+R,\quad P_{\xi y}=\sum w_j\xi_j'(y_j-\bar y)^\top. \]

Kalman 增益 \(K=P_{\xi y}P_{yy}^{-1}\)，创新（innovation）\(\nu=y_{\text{obs}}-\bar y\)。

更新后的状态（通过 Exp 映射注入流形）：

\[ \hat\chi_1^+=\varphi(\hat\chi_1^-,\, K\nu)=\hat\chi_1^-\cdot\operatorname{Exp}(K\nu). \]

\[ P_1^+=P_1^--KP_{yy}K^\top. \]

注意更新的增量 \(K\nu\) 是一个 3 维切向量，通过 Exp 映射"推"回流形。这与标准 UKF 的加法更新 \(\hat x^+=\hat x^-+K\nu\) 形式相同，但 \(+\) 被替换为 \(\varphi\)——这就是 UKF-M 的全部修改。

与标准 UKF 的定量对比

为了让对比更直观，考虑 \(\theta_0=170°\) 的极端情况。标准 UKF 在 \((\theta,t_x,t_y)\) 空间生成 sigma 点 \(\theta\pm c\sigma_\theta\)。当 \(c\sigma_\theta>10°\) 时，一个 sigma 点落在 \(>180°\)，另一个在 \(<180°\)。标准加权平均

\[\bar\theta=\sum w_j\theta_j\]

会给出接近 \(0°\) 的结果（因为 \(180°\) 和 \(-180°\) 的算术平均是 \(0°\)），而真实均值应该在 \(170°\) 附近。

UKF-M 不会遇到这个问题，因为：

1. Sigma 点通过 Exp 映射生成，\(\xi_j=(+c\sigma_\theta,0,0)\) 映射为"从当前朝向右转 \(c\sigma_\theta\)"——结果仍然在 170° 附近。
2. 均值直接用无噪声传播而非加权平均。
3. 协方差在切空间计算，Log 映射自动取最短弧度差。

场景	标准 UKF 均值误差	UKF-M 均值误差
\(\theta_0=60°\), \(\sigma_\theta=5°\)	\(<0.01°\)	\(<0.01°\)
\(\theta_0=170°\), \(\sigma_\theta=15°\)	\(\sim 160°\)（灾难性）	\(<0.1°\)
\(\theta_0=90°\), \(\sigma_\theta=30°\)	\(\sim 5°\)	\(<0.5°\)

⚠️ 陷阱：用欧氏平均代替 Fréchet 均值

错误做法：对 sigma 点的旋转部分直接算 \(\bar R=\sum w_j R_j\)（这不是合法旋转矩阵！）或对角度直接算术平均（会出现上述 wrapping 灾难）。

现象：当 sigma 点跨越 \(\pm\pi\) 时，均值突然跳到 \(0\)；协方差矩阵特征值爆炸。

根本原因：\(SO(2)\)（乃至 \(SO(3)\)）不是向量空间，加法平均没有几何意义。Fréchet 均值是唯一满足"距离平方和最小"的群元素。

正确做法：UKF-M 用无噪声传播取代迭代 Fréchet 均值；若需精确均值，对 sigma 点做迭代：\(\bar\chi\leftarrow\bar\chi\cdot\operatorname{Exp}\bigl(\sum w_j\operatorname{Log}(\bar\chi^{-1}\mathcal{X}_j)\bigr)\)，迭代 3-5 次收敛。

练习

练习 1（⭐⭐⭐）：把上述示例推广到 \(SO(3)\) 姿态估计。状态 \(R\in SO(3)\)，过程模型 \(R_{k+1}=R_k\operatorname{Exp}(\omega\Delta t+w)\)，观测为磁力计 \(y=R^\top m_{\text{ref}}+n\)（\(m_{\text{ref}}\) 是参考磁场方向）。写出完整的 UKF-M 算法（sigma 点数 \(2\times 3+1=7\)），并验证在大初始误差（\(>90°\)）下标准 UKF（用欧拉角）发散而 UKF-M 收敛。

练习 2（⭐⭐⭐⭐）：对上述 SE(2) 例子，运行 100 次 Monte Carlo 仿真（50 个时间步），分别用标准 UKF 和 UKF-M。计算两种方法的 NEES（Normalized Estimation Error Squared，见 A1 §一致性诊断）：\(\text{NEES}_k=\xi_{e,k}^\top P_{k|k}^{-1}\xi_{e,k}\)，其中 \(\xi_{e,k}=\operatorname{Log}(\hat\chi_k^{-1}\chi_{\text{true},k})\)。绘制 NEES 随时间的均值曲线。理论上 \(\overline{\text{NEES}}\approx d=3\)，验证 UKF-M 的 NEES 更接近 \(\chi^2(3)\) 分布。

---

§A3.12 Equivariant Filter——InEKF 的严格推广（档位 4 进阶）

A3.12.1 等变性定义（Mahony-Hamel-Trumpf, arXiv:2006.08276）

状态空间 \(\mathcal{M}\)（可以不是 Lie 群）、输入空间 \(\mathbb{V}\)、对称群 \(G\)、右作用 \(\phi:G\times\mathcal{M}\to\mathcal{M}\)、输入作用 \(\psi:G\times\mathbb{V}\to\mathbb{V}\)。动力学 \(\dot\xi=f_u(\xi)\) **等变**当且仅当

\[\boxed{d\phi_X\cdot f_u(\xi) = f_{\psi_X(u)}\bigl(\phi_X(\xi)\bigr)\quad \forall X\in G,\xi\in\mathcal{M},u\in\mathbb{V}}\]

输出等变性：\(h(\phi_X(\xi)) = \rho_X(h(\xi))\)。

A3.12.2 EqF 如何推广 InEKF

• InEKF：要求 \(\chi\in G\)（状态就是李群元素），且动力学 group-affine。
• EqF：状态在**齐性空间** \(\mathcal{M}\)（李群作用传递）即可；若 \(\mathcal{M}=G\) 且 lifted system 恰好 group-affine，EqF 退化为 InEKF。EqF 能处理 InEKF 做不到的情形：SLAM manifold、VIO manifold、\(S^2\)-only 单轴姿态。
• 观测者状态：\(\hat X\in G\)（非 \(\mathcal{M}\)！），估计 \(\hat\xi=\phi(\hat X,\xi^\circ)\)；lift \(\Lambda:\mathcal{M}\times\mathbb{V}\to\mathfrak{g}\) 满足 \(dE|_{id}\phi_{\xi}(E)\Lambda(\xi,u)=f_u(\xi)\)。
• 全局等变误差 \(e = \phi(\hat X^{-1},\xi)\in\mathcal{M}\)，在**固定原点** \(\xi^\circ\) 线性化（而非沿轨迹）——这就是 EqF 相较 EKF 输出线性化精度提升的来源。进一步利用输出等变性可得 三阶线性化误差（vs EKF 的二阶，称为 EqF⋆）。

Gada-van Goor-Banavar-Mahony 2022（arXiv:2210.13728）：不同原点/局部坐标的 EqF，若噪声模型等价，则性能相同——"Equivariant Filters are Equivariant"。

EqF 与 InEKF 的本质区别：InEKF 要求状态本身是李群元素（\(X\in G\)），且动力学满足 group-affine。EqF 放宽了这两个要求：(1) 状态可以生活在齐性空间（李群作用传递但不一定是群本身），(2) 只需要系统的对称性（等变性），不需要 group-affine。代价是需要构造一个 lift 映射 \(\Lambda:\mathcal{M}\times\mathbb{V}\to\mathfrak{g}\)，把系统动力学"抬升"到对称群上。

直觉类比：InEKF 要求"状态空间本身是一个群"——就像要求地球表面是一个平面。EqF 只要求"有一个群在状态空间上作用"——就像承认地球是球面，但利用旋转群在球面上的作用来简化问题。球面不是群（两个点没有天然的"乘法"），但旋转群可以把任何点移到任何其他点（传递性），这就够了。

EqF 线性化精度的提升。标准 EKF 在当前估计 \(\hat x\) 处线性化观测函数 \(h\)；InEKF 利用不变误差在固定点线性化；EqF 进一步利用输出等变性 \(h(\phi_X(\xi))=\rho_X(h(\xi))\)，把输出误差 \(\delta y = h(\xi)-h(\xi^\circ)\) 分解为等变部分（精确）和非等变部分（高阶小量）。van Goor-Mahony (T-RO 2023, §IV-C) 证明这给出了**三阶线性化误差**，而标准 EKF 只有二阶。在 Monte Carlo NEES 测试中，这个理论优势直接反映为 EqF 的协方差**与真实分布的统计距离更小**。

A3.12.3 EqVIO 状态结构（van Goor–Mahony T-RO 2023, arXiv:2205.01980）

VI-SLAM 完全可观的齐性空间：

\[M^{VI}_n(3) = \mathcal{T}^{VI}_n(3)/\alpha,\quad \mathcal{T}^{VI}_n(3) = SE(3)\times\mathbb{R}^3\times(\mathbb{R}^3)^n - \mathcal{E}\]

\(\alpha\) 是绕重力方向的 \(SE_{e_3}(3) = S^1\ltimes\mathbb{R}^3\)（位置+yaw 4 维不可观子群）作用。

VI-SLAM Lie 群：

\[\mathbf{SLAM}^{VI}_n(3) = SE_2(3)\times \mathbf{SOT}(3)^n\]

其中 \(\mathbf{SOT}(3)=SO(3)\times MR(1)\)（Scaled Orthogonal Transformations），每个 landmark 配一个 \(\mathbf{SOT}(3)\)——几何角度解释了**为什么 inverse-depth 参数化对 SLAM 有效**。

A3.12.4 EuRoC 定量对比

ICRA21 workshop 版（monocular EqF，V 序列，RMSE m）：

序列	R-UKF-LG	SVO-MSF	MSCKF	ROVIO	EqF
V1_01	0.55	0.40	0.34	0.10	0.07
V1_02	0.40	0.63	0.20	0.10	0.11
V2_01	0.37	0.20	0.10	0.12	0.08
V2_02	0.47	0.37	0.16	0.14	0.13

T-RO 2023 全 EuRoC：EqVIO 在 5/11 序列**获最佳 RMSE，均值与 OpenVINS 并列第一，处理速度比第二快算法快 **2.14×（整帧 5.4 ms，filter 仅 1.2 ms）；UZH-FPV 4/10 序列最佳，速度优势 5.3×。1000 次 Monte-Carlo 仿真中，EqVIO 的 full-state NEES 中位数与理论 \(\chi^2_d\) 吻合，而标准 EKF-VIO NEES 随时间超过理论上界（overconfident）。

---

A3.12.5 误差定义的统一视角：同一件事的四种局部坐标 ⭐⭐⭐

前面分别介绍了 ESKF、MEKF、InEKF、UKF-M，容易让人产生一种误解：这些方法像四个互不相干的滤波器。更准确的理解是，它们都在回答同一个问题：

本质洞察：滤波器真正估计的不是"状态本身"，而是"真状态相对当前参考状态的小扰动"。 状态在流形上时，小扰动必须活在切空间；参考状态负责承载大运动，切空间扰动负责承载不确定性。

这个观点与线性 KF 的区别很小，却足以改变整个工程实现。在线性 KF 中，状态空间是 \(\mathbb{R}^n\)，所以真值、估计值和误差自然满足

\[x=\hat x+\delta x.\]

这条式子背后有一个隐含假设：任意两个状态可以相减，差仍然是同一个线性空间中的向量。位置、速度、温度满足这个假设；旋转不满足。两个旋转矩阵 \(R_1,R_2\in SO(3)\) 的差 \(R_1-R_2\) 一般不是旋转，甚至没有清晰的物理意义。

因此在李群上要把"减法"替换成"相对变换"：

\[\delta R=\hat R^\top R,\qquad \delta R\in SO(3).\]

再用对数映射把相对旋转压回切空间：

\[\delta\theta=\log(\hat R^\top R)^\vee,\qquad \delta\theta\in\mathbb{R}^3.\]

这就是 \(\boxminus\) 的本质：它不是普通减法，而是"先求相对位姿，再取对数坐标"。

四种常见滤波器可以用同一张表组织：

方法	大状态在哪里传播	小误差如何定义	KF 操作对象	关键收益
MEKF	四元数或 \(SO(3)\) 姿态	\(q=\hat q\otimes\delta q\)	\(\delta\theta,b_g\)	姿态最小维度
ESKF	\(SE(3)\times\mathbb{R}^k\) 名义状态	姿态乘性，其余加性	\(\delta p,\delta v,\delta\theta,\delta b\)	IMU 工程可用
InEKF	矩阵李群 \(G\)	\(\eta=\hat X X^{-1}\) 或 \(X^{-1}\hat X\)	\(\xi=\log(\eta)\)	误差动力学结构稳定
UKF-M	流形 \(\mathcal M\)	retraction \(\varphi,\varphi^{-1}\)	sigma 点切空间坐标	免解析 Jacobian

如果不做这种拆分会怎样？标准 EKF 会把旋转当作欧氏向量，预测后再归一化四元数。这种做法短时间内可能能跑，但它的协方差并不知道"归一化"改变了状态坐标。协方差仍认为自己生活在四维空间，而真实姿态只有三维自由度。长时间运行后，协方差会在约束方向上产生不可解释的数值退化。

从工程角度看，这和机器人局部地图的维护很像：全局地图可能很大，但 scan-to-map 每次只在当前位姿附近求一个小增量。大地图负责承载全局结构，小增量负责快速优化。ESKF/MEKF/InEKF 也是同一思想：名义状态承载大运动，误差状态承载小校正。

A3.12.6 从 MEKF 推到 ESKF：姿态乘性误差的逐步推导 ⭐⭐⭐

先看最小问题：只估计姿态和陀螺偏置。真实角速度测量模型为

\[\omega_m=\omega+b_g+n_g,\]

其中 \(\omega\) 是真实机体系角速度，\(b_g\) 是陀螺偏置，\(n_g\) 是白噪声。真实姿态满足

\[\dot R=R(\omega)^\wedge=R(\omega_m-b_g-n_g)^\wedge.\]

名义姿态不含瞬时噪声，只用估计偏置积分：

\[\dot{\hat R}=\hat R(\omega_m-\hat b_g)^\wedge.\]

现在定义右乘误差

\[R=\hat R\exp(\delta\theta^\wedge).\]

这一步的动机是：\(\hat R\) 表示大姿态，\(\exp(\delta\theta^\wedge)\) 表示从名义姿态到真实姿态的小转动。小角误差 \(\delta\theta\) 才是 KF 要估计的变量。

Step 1：对复合关系求导。

\[\dot R=\dot{\hat R}\exp(\delta\theta^\wedge)+\hat R\frac{d}{dt}\exp(\delta\theta^\wedge).\]

当 \(\delta\theta\) 很小时，使用一阶近似

\[\exp(\delta\theta^\wedge)\approx I+\delta\theta^\wedge,\qquad \frac{d}{dt}\exp(\delta\theta^\wedge)\approx \dot{\delta\theta}^\wedge.\]

代入得

\[\dot R\approx \hat R(\omega_m-\hat b_g)^\wedge(I+\delta\theta^\wedge)+\hat R\dot{\delta\theta}^\wedge.\]

Step 2：真实动力学也写到名义坐标下。

真实动力学为

\[\dot R=R(\omega_m-b_g-n_g)^\wedge =\hat R(I+\delta\theta^\wedge)(\omega_m-b_g-n_g)^\wedge.\]

令 \(\hat\omega=\omega_m-\hat b_g\)，\(\delta b_g=b_g-\hat b_g\)，则

\[\omega_m-b_g-n_g=\hat\omega-\delta b_g-n_g.\]

忽略二阶小量 \(\delta\theta\,\delta b_g\) 与 \(\delta\theta\,n_g\)，右端近似为

\[\hat R\left[(I+\delta\theta^\wedge)\hat\omega^\wedge-(\delta b_g+n_g)^\wedge\right].\]

Step 3：两边左乘 \(\hat R^\top\) 并比较一阶项。

左边名义展开为

\[\hat\omega^\wedge+\hat\omega^\wedge\delta\theta^\wedge+\dot{\delta\theta}^\wedge.\]

右边真实展开为

\[\hat\omega^\wedge+\delta\theta^\wedge\hat\omega^\wedge-(\delta b_g+n_g)^\wedge.\]

消去共同的 \(\hat\omega^\wedge\)：

\[\dot{\delta\theta}^\wedge =\delta\theta^\wedge\hat\omega^\wedge-\hat\omega^\wedge\delta\theta^\wedge-(\delta b_g+n_g)^\wedge.\]

利用李括号恒等式

\[a^\wedge b^\wedge-b^\wedge a^\wedge=(a\times b)^\wedge,\]

得到

\[\delta\theta^\wedge\hat\omega^\wedge-\hat\omega^\wedge\delta\theta^\wedge =(\delta\theta\times\hat\omega)^\wedge =(-\hat\omega\times\delta\theta)^\wedge =(-\hat\omega^\wedge\delta\theta)^\wedge.\]

因此

\[\boxed{\dot{\delta\theta}=-\hat\omega^\wedge\delta\theta-\delta b_g-n_g.}\]

这就是 MEKF 姿态误差方程，也是 ESKF 姿态误差块的来源。

ESKF 只是把同一思想扩展到惯性导航状态

\[x=(p,v,R,b_a,b_g,g).\]

真值与名义值的复合关系写成

\[p=\hat p+\delta p,\quad v=\hat v+\delta v,\quad R=\hat R\exp(\delta\theta^\wedge),\quad b_a=\hat b_a+\delta b_a,\quad b_g=\hat b_g+\delta b_g.\]

继续从 IMU 模型

\[\dot v=g+R(a_m-b_a-n_a)\]

出发，一阶展开

\[R(a_m-b_a-n_a) \approx \hat R(I+\delta\theta^\wedge)(\hat a-\delta b_a-n_a),\qquad \hat a=a_m-\hat b_a.\]

保留一阶项：

\[R(a_m-b_a-n_a) \approx \hat R\hat a+\hat R\delta\theta^\wedge\hat a-\hat R\delta b_a-\hat R n_a.\]

由于

\[\delta\theta^\wedge\hat a=\delta\theta\times\hat a=-\hat a^\wedge\delta\theta,\]

速度误差方程为

\[\boxed{\dot{\delta v}= -\hat R\hat a^\wedge\delta\theta-\hat R\delta b_a+\delta g-\hat R n_a.}\]

位置误差则由 \(\dot p=v\) 直接得到

\[\boxed{\dot{\delta p}=\delta v.}\]

这样，Solà-ESKF 的 \(F\) 矩阵不是凭空背出来的，而是由三个复合关系逐块推导出来：

误差块	来源	关键近似	结果
\(\dot{\delta p}\)	\(\dot p=v\)	无	\(\delta v\)
\(\dot{\delta v}\)	\(\dot v=g+Ra\)	\(\exp(\delta\theta^\wedge)\approx I+\delta\theta^\wedge\)	\(-\hat R\hat a^\wedge\delta\theta-\hat R\delta b_a+\delta g-\hat R n_a\)
\(\dot{\delta\theta}\)	\(\dot R=R\omega^\wedge\)	李括号一阶化	\(-\hat\omega^\wedge\delta\theta-\delta b_g-n_g\)

A3.12.7 IEKF 与 InEKF 的名字差异：一个强调迭代，一个强调不变 ⭐⭐

IEKF 这个缩写有两个历史含义，阅读论文时必须分清：

缩写	常见全称	核心操作	代表场景
IEKF	Iterated EKF	同一观测更新内反复线性化，等价单步 Gauss-Newton	FAST-LIO2、强非线性测量
InEKF / invariant EKF	Invariant EKF	用左/右不变误差构造滤波器	IMU 导航、腿式接触、InEKF-SLAM

二者可以叠加：可以做"iterated invariant EKF"，即用不变误差定义状态，再对一次观测更新做多次 Gauss-Newton 迭代。A4 会从全景角度解释这个组合。

为什么会有这个命名冲突？因为两个研究传统关注的痛点不同。优化/SLAM 传统关心"测量模型非线性太强，一次线性化不够"，于是引入 iterated EKF。几何控制/不变观测器传统关心"误差坐标不对导致稳定性和可观性被破坏"，于是引入 invariant EKF。

反事实地看，如果只做迭代而不改变误差定义，滤波器可以更好地最小化当前时刻的局部代价，但它仍可能在线性化过程中产生不可观方向的虚假信息。如果只做不变误差而不迭代，系统结构更一致，但强非线性点云匹配或 bearing-only 相机观测仍可能需要多次重线性化。工程系统应当把两者看作两条正交轴，而不是互相替代。

A3.12.8 可观性一致性的推导：为什么 yaw 不应被滤波器"看见" ⭐⭐⭐⭐

以 VIO/SLAM 中最经典的不可观性为例。世界坐标系没有外部绝对参考时，以下变换不会改变相机图像和 IMU 相对测量：

1. 所有位姿和地图点整体平移同一个 \(t_0\)。
2. 所有位姿和地图点绕重力方向整体旋转同一个 yaw 角 \(\psi\)。

因此理想估计问题至少有 4 个 gauge 自由度：3 个全局平移 + 1 个全局 yaw。滤波器若声称这 4 个方向不确定性变小，就是在利用不存在的信息。

用线性系统语言表达，误差系统

\[\delta x_{k+1}=F_k\delta x_k,\qquad r_k=H_k\delta x_k+n_k\]

的不可观子空间 \(\mathcal N\) 应满足

\[H_k F_{k-1}\cdots F_0 N=0,\qquad \forall k.\]

这里 \(N\) 的列向量张成不可观方向。若 \(N\) 包含 yaw 方向，则任何观测残差一阶上都不应对该方向敏感。

标准 EKF 的问题在于，\(F_k,H_k\) 在不断变化的估计点 \(\hat x_k\) 上求 Jacobian。第 \(i\) 帧的特征观测在 \(\hat x_i\) 处线性化，第 \(j\) 帧又在另一个 \(\hat x_j\) 处线性化。每个局部 Jacobian 单独看都合理，但堆叠起来之后，它们不再共享同一个 gauge 变换。结果是

\[H_k(\hat x_k)F_{k-1}(\hat x_{k-1})\cdots F_0(\hat x_0)N\ne0.\]

这表示线性化系统误以为 yaw 会影响残差。于是 Kalman 更新会沿 yaw 方向减少协方差：

\[P^+=(I-KH)P(I-KH)^\top+KRK^\top.\]

如果 \(HN\ne0\)，则 \(K\) 会把新息投影到 yaw 方向，导致 \(N^\top P N\) 下降。真实世界并没有提供 yaw 绝对信息，协方差却下降了，这就是一致性破坏。

FEJ 的思想是把 Jacobian 固定在第一次估计点，使所有残差共享同一个 gauge 坐标。InEKF 的思想更深一层：选择不变误差，使误差传播和观测 Jacobian 天然与全局群作用兼容。

在 group-affine 系统中，误差对数满足

\[\dot\xi=A_t\xi,\]

其中 \(A_t\) 不依赖 \(\hat X_t\)。观测若为 fundamental/invariant 形式，残差一阶为

\[z=H\xi+n,\]

其中 \(H\) 也只依赖已知参考量或输入，不随全局位姿漂移改变。于是对任何 gauge 方向 \(N\)，只要物理观测满足 \(HN=0\)，传播后仍有

\[H\Phi(t,0)N=0.\]

这就是 InEKF 保持可观性一致性的核心机制：不是靠调参，不是靠小心选线性化点，而是靠误差坐标与系统对称性一致。

本质洞察：一致性问题的根源不是 EKF 的一阶近似本身，而是"一阶近似破坏了原问题的对称性"。 InEKF 的价值在于让线性化后的误差系统仍然记得原问题有哪些方向不可观。

A3.12.9 机器人 IMU + SLAM worked example：从 \(SE_2(3)\) 到 landmark 更新 ⭐⭐⭐

考虑机器人携带 IMU 与相机，状态为

\[X=\begin{bmatrix}R&v&p\\0&1&0\\0&0&1\end{bmatrix}\in SE_2(3),\qquad \ell_i\in\mathbb{R}^3.\]

为了把 landmark 一并放入群，可扩展到

\[X_N= \begin{bmatrix} R&v&p&\ell_1&\cdots&\ell_N\\ 0&1&0&0&\cdots&0\\ 0&0&1&0&\cdots&0\\ 0&0&0&1&&0\\ \vdots&\vdots&\vdots&&\ddots&\\ 0&0&0&0&&1 \end{bmatrix}\in SE_{N+2}(3).\]

IMU 传播只作用在 \(R,v,p\) 块：

\[\dot R=R\omega^\wedge,\qquad \dot v=g+Ra,\qquad \dot p=v,\qquad \dot\ell_i=0.\]

把它写成矩阵形式

\[\dot X=AX+XB,\]

其中 \(A\) 携带重力与速度积分结构，\(B\) 携带机体系 IMU 输入。这个形式一出现，group-affine 就几乎自动成立，因为

\[f(X)=AX+XB,\qquad f(I)=A+B.\]

验证：

\[\begin{aligned} f(X_1)X_2+X_1f(X_2)-X_1f(I)X_2 &=(AX_1+X_1B)X_2+X_1(AX_2+X_2B)-X_1(A+B)X_2\\ &=AX_1X_2+X_1BX_2+X_1AX_2+X_1X_2B-X_1AX_2-X_1BX_2\\ &=AX_1X_2+X_1X_2B\\ &=f(X_1X_2). \end{aligned}\]

这段推导说明，\(SE_2(3)\) 的好处不是把矩阵写大，而是把 IMU 动力学变成 \(AX+XB\) 的代数形态。

相机观测 landmark 的理想 body-frame 坐标为

\[y_i=R^\top(\ell_i-p)+n_i.\]

对常见 ESKF/product-manifold 局部误差，一阶 residual 可以写成

\[z_i=\hat R^\top(\ell_i-\hat p)-y_i\approx H_i\xi+n_i.\]

若误差坐标按

\[\xi=(\delta\theta,\delta v,\delta p,\delta\ell_1,\ldots,\delta\ell_N),\]

则第 \(i\) 个 landmark 的 Jacobian 具有典型结构

\[H_i=\begin{bmatrix} -(\hat R^\top(\hat\ell_i-\hat p))^\wedge & 0 & -\hat R^\top & 0\cdots \hat R^\top\cdots 0 \end{bmatrix},\]

其中 \(I_i\) 落在第 \(i\) 个 landmark 误差块上。推导要点如下：

1. 姿态小扰动改变 body-frame 向量方向，产生 \(-y_i^\wedge\delta\theta\)。
2. 位置小扰动 \(+\delta p\) 会让相对向量 \(\ell_i-p\) 减小，在 body frame 中产生 \(-\hat R^\top\delta p\)。
3. landmark 小扰动 \(+\delta\ell_i\) 直接增加相对向量，在 body frame 中产生 \(+\hat R^\top\delta\ell_i\)。
4. 速度块不直接进入相机静态观测，所以该块为 0。

下面的 C++ 片段只演示 Jacobian 块的组装思想，不包含完整滤波器：

#include <Eigen/Dense>

// 计算反对称矩阵：skew(w) * v = w.cross(v)
Eigen::Matrix3d skew(const Eigen::Vector3d& w) {
    Eigen::Matrix3d W;
    W << 0.0, -w.z(), w.y(),
         w.z(), 0.0, -w.x(),
        -w.y(), w.x(), 0.0;
    return W;
}

// 组装单个 landmark 的 product-manifold 局部观测 Jacobian
// 误差排序：[theta, velocity, position, landmark_0, ..., landmark_N]
Eigen::MatrixXd buildLandmarkJacobian(const Eigen::Matrix3d& R_hat,
                                      const Eigen::Vector3d& p_hat,
                                      const Eigen::Vector3d& landmark_hat,
                                      int landmark_index,
                                      int landmark_count) {
    const int state_dim = 9 + 3 * landmark_count;
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, state_dim);

    // 预测的机体系相对向量
    Eigen::Vector3d y_hat = R_hat.transpose() * (landmark_hat - p_hat);

    // 姿态误差块：小角度会旋转机体系相对向量
    H.block<3, 3>(0, 0) = -skew(y_hat);

    // 速度误差块不进入静态视觉观测，因此保持为零

    // 位置误差块：世界系位置扰动需要旋到机体系
    H.block<3, 3>(0, 6) = -R_hat.transpose();

    // landmark 误差块：地图点移动，同样旋到机体系
    int landmark_col = 9 + 3 * landmark_index;
    H.block<3, 3>(0, landmark_col) = R_hat.transpose();
    return H;
}

这个例子把 A3 的主线闭合起来：

环节	ESKF/MEKF 视角	InEKF 视角	SLAM 工程含义
预测	名义状态 IMU 积分，误差态线性化	\(SE_2(3)\) group-affine 传播	高频 IMU 传播稳定
姿态误差	\(\hat R\exp(\delta\theta^\wedge)\)	不变误差的一部分	避免四元数协方差奇异
观测	对 residual 做局部 Jacobian	invariant/fundamental observation	保持 gauge 不可观方向
更新	注入误差并 reset	群乘法更新	坐标一致、协方差有意义

常见误区与排查：

误区	现象	根本原因	正确做法
把 \(\delta\theta\) 当欧拉角	大姿态机动后发散	小角误差是切空间坐标，不是全局姿态参数	每次更新后注入并重置误差
混用左扰动和右扰动	Jacobian 符号看似只差负号，实际轨迹漂移	Adjoint 变换和残差定义不匹配	在文档、代码、测试中固定同一约定
忽略不可观方向	NEES 偏大或 yaw 协方差异常缩小	线性化破坏 gauge 对称性	检查 \(HN=0\) 与 \(\Phi N\) 是否保持
把 bias 也强行塞进 perfect InEKF	初期效果好，长期 bias 收敛异常	bias 动力学通常破坏 group-affine	使用 imperfect InEKF、EqF 或混合误差模型

练习：

1. 在草稿纸上从 \(y=R^\top(\ell-p)\) 出发，分别对 \(\delta\theta,\delta p,\delta\ell\) 做一阶展开，验证上表中的 \(H_i\) 符号。
2. 令 \(N_p\) 表示全局平移 gauge 方向，证明对 landmark 观测有 \(H_iN_p=0\)。
3. 把 IMU 动力学中的重力 \(g\) 去掉，验证此时 \(f(I)\) 如何变化，并说明为什么系统仍然 group-affine。
4. 思考带陀螺偏置的模型 \(\omega_m-\hat b_g\) 为什么不能简单写成同一个 \(AX+XB\) 形式。

A3.12.8 流形滤波的收敛性理论：从李群观测器到指数稳定性 ⭐⭐⭐

前面我们从工程角度说明了"流形滤波比欧氏滤波更好"——但"更好"可以定量化吗？从控制论视角，流形滤波器可以被解读为**非线性观测器 (nonlinear observer)**，其收敛性由误差动力学的稳定性决定。

定义（指数稳定）：误差 \(\eta(t)\) 在平衡点 \(\eta=e\)（群单位元）处**局部指数稳定**，若存在 \(C>0, \lambda>0\) 使得 $\(\|\log(\eta(t))\| \le C e^{-\lambda t}\|\log(\eta(0))\|\)$ 对所有 \(\|\log(\eta(0))\|<\delta\) 成立。

对 perfect InEKF（group-affine 动力学 + 合适的增益选择），Barrau-Bonnabel (TAC 2017, Thm 3) 证明了误差动力学在**无噪声**时满足上述指数稳定性——且吸引域由增益大小决定，而非由轨迹形态决定。这与 EKF 形成鲜明对比：EKF 的误差稳定性依赖"估计离真值多远"和"系统此刻的非线性程度"，两者都随时间变化且难以先验保证。

反事实推理：如果不要求 group-affine 会怎样？对 Imperfect InEKF（含 bias），误差动力学变为 $\(\dot\xi = A\xi + \underbrace{\Delta(\hat X, X)}_{\text{bias 引起的依赖 }\hat X\text{ 项}}\)$ 其中 \(\Delta\) 是 bias 动力学对 group-affine 结构的破坏。当 bias 估计准确时 \(\Delta\to 0\)，系统"几乎"指数稳定；当 bias 估计偏差大时，系统可能暂时失去稳定性。这就是为什么带 bias 的 InEKF 需要更保守的增益设计——它是条件性稳定的。

本质洞察：InEKF 的 group-affine 条件不是数学家为了优雅而加的限制，而是保证**全局吸引域不依赖轨迹**的充要条件。对 SLAM/VIO 这类系统，这意味着无论机器人执行什么运动轨迹（平移、转弯、悬停），只要观测充足，滤波器都能收敛。EKF 做不到这点——它在某些轨迹下（如退化运动）会系统性发散。

Lyapunov 分析思路（Bonnabel CDC 2007, Barrau PhD 2015）。取 Lyapunov 函数 $\(V(\xi) = \xi^\top P^{-1}\xi\)$ 其中 \(P\) 是 InEKF 的协方差。由于 \(\dot\xi=A\xi\)（无噪声 log-linear），有 $\(\dot V = \xi^\top(A^\top P^{-1}+P^{-1}A)\xi\)$ 选择增益使得 Riccati 方程的稳态解 \(P_\infty\) 满足 \(A^\top P_\infty^{-1}+P_\infty^{-1}A<0\)（在有持续观测的条件下可保证），则 \(V\) 单调递减 \(\Rightarrow\) \(\xi\to 0\)。

关键点在于：因为 \(A\) 不依赖 \(\hat X\)，所以这个 Lyapunov 论证**对所有轨迹同时成立**。EKF 的对应论证需要假设"Jacobian 沿轨迹有界"，这对实际 SLAM 轨迹通常无法验证。

A3.12.9 CT-ESKF：统一协方差变换框架（2025 最新进展） ⭐⭐⭐⭐

2025 年 Cui 等人提出了 Covariance Transformation-based Error-State Kalman Filter (CT-ESKF, arXiv:2511.00453)，试图从更高层次统一各种 ESKF 变体。其核心观察是：不同的 ESKF（左扰动、右扰动、\(SE_2(3)\) 不变误差等）本质上只是同一滤波问题在不同**误差坐标系**下的表达。它们之间可以通过协方差变换矩阵 \(T\) 相互转换：

\[P_{\text{new}} = T\,P_{\text{old}}\,T^\top\]

CT-ESKF 的贡献在于提供了一个**通用框架**：

1. 选择任意误差定义（左/右/混合）
2. 在该坐标系下写出传播和更新方程
3. 通过 \(T\) 矩阵与其他坐标系的结果互换

这对多传感器融合系统特别有用——不同传感器可能天然适配不同的误差定义（GPS 适配世界系加性误差，陀螺适配机体系乘性误差），CT-ESKF 允许在各自最自然的坐标系下处理，再统一到一个协方差空间。

传统做法	CT-ESKF 做法
强制所有传感器用同一误差定义	每个传感器用最自然的误差定义
不匹配的传感器需要额外 Jacobian	统一通过协方差变换对齐
切换误差定义需要重写大量代码	只需修改 \(T\) 矩阵

这一框架尚处于理论阶段，尚无成熟的开源实现，但它指明了流形滤波未来的一个方向：不是选择"最好的"误差定义，而是让框架本身对误差定义选择不敏感。

---

A3.12.10 十分钟黑板自检：从方法名回到公式 ⭐⭐

读完本节后，可以用下面 6 个问题快速判断自己是否真正理解，而不是只记住名词。

问题	合格回答应包含
MEKF 为什么不用四维四元数误差？	单位范数约束、三维切空间、协方差奇异风险
ESKF 的 nominal state 与 error state 为什么分开？	高频大运动积分与低维小扰动估计解耦
左扰动和右扰动差在哪里？	误差附着在世界侧还是机体侧，Adjoint 与 Jacobian 符号随之改变
InEKF 比 ESKF 多利用了什么结构？	group-affine 动力学与不变误差，使 \(A\) 与估计解耦
可观性一致性检查什么？	gauge 方向 \(N\) 是否满足 \(HN=0\) 与 \(H\Phi N=0\)
UKF-M 省掉了什么，又付出了什么？	省解析 Jacobian，付出 sigma 点传播和流形均值处理成本

一个实用的口头推导顺序是：

1. 先写 \(R=\hat R\exp(\delta\theta^\wedge)\)，说明为什么不用 \(R-\hat R\)。
2. 再从 \(\dot R=R\omega^\wedge\) 推出 \(\dot{\delta\theta}\)。
3. 接着把 \(p,v\) 加回来，得到 ESKF 的 \(F\) 块。
4. 然后把 \((R,v,p)\) 组成 \(SE_2(3)\) 矩阵，说明 \(AX+XB\)。
5. 最后写 group-affine 恒等式与 \(\dot\xi=A\xi\)。

如果这条链能在黑板上连续写出，说明 A3 的主干已经掌握；若卡在某一步，应回到对应小节重新手推，而不是继续背后面的库名。

更进一步的��测：如果你能回答以下三个"为什么"问题，说明不仅掌握���方法，还理解了设计决策背后的权衡：

"为什���"问题	核心答案要包含
为什么 FAST-LIO2 不用 InEKF 而用 IKFoM？	状态含 \(S^2\)（重力方向）和双外参，product manifold 不是 \(SE_2(3)\)，无法满足 group-affine；LiDAR 约束极强使一致性差距可忽略
为什么 EqVIO 比 MSCKF 更快？	EqF 只维护当前帧+landmarks 的协方差（滤波），不需要 null-space 投影消特征；等变输出近似减少了需要迭代的次数
为什么 Forster 预积分已经"够用"而 Brossard 仍有价值？	短程 VIO 下差异可忽��；长程/高动态下协方差准确性直接影响因子图权重分配——30% 的协方差偏差可能让回环约束被错误权重压制

常见的学习误区：

误区	正确理解
"流形滤波比 EKF 好"	流形滤波**在有旋转/位姿状态时**比欧氏 EKF 好；纯线性系统 KF 就是最优的
"InEKF 是 ESKF 的升级版"	它们在正交维度上改进：ESKF 解决参数化问题，InEKF 解决一致性问题
"EqF 是终极方案"	EqF 实现复杂度高，且需要手动构造 lift 映射和对称群——不适合快速原型开发
"UKF-M 免 Jacobian 所以更好"	免的是人力成本（不需手推），但计算成本更高且高维不稳定

---

§A3.12b 不变预积分——从 Forster 到 Brossard 的范式升级 ⭐⭐⭐

A3.12b.1 动机：为什么预积分需要群结构

回顾 Forster 等人（T-RO 2017, arXiv:1512.02363）的经典 IMU 预积分理论。在关键帧 \(i\) 到 \(j\) 之间，IMU 积分产生相对位移/速度/旋转增量 \(\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}\)。核心贡献是：当 bias 估计 \(\hat b\) 在优化中更新时，不需要重新积分整段 IMU 数据，只需对预积分量做一阶修正。这使得 bias 参数可以参与后端因子图优化而不引起灾难性重积分。

然而 Forster 预积分有一个**结构性遗憾**：协方差传播仍依赖沿轨迹的一阶 Taylor 展开。具体来说，旋转预积分量 \(\Delta R_{ij}\) 的协方差通过

\[P_{k+1}=F_k P_k F_k^\top + G_k Q_d G_k^\top\]

传播，其中 \(F_k\) 包含当前姿态估计处的 Jacobian——这正是经典 EKF 的局限。当角速度大或积分时间长时，一阶近似的误差会累积，导致预积分协方差**不再准确反映真实不确定性**。

反事实推理：如果不改进协方差传播会怎样？短程 VIO（几秒间隔）下误差不明显；但在长程自动驾驶（1 小时实验，Brossard T-RO 2021 Fig. 8 所示）中，协方差偏差会让因子图后端的权重分配失真——预积分因子的协方差过小会"绑架"优化，过大则浪费 IMU 信息。

Brossard, Barrau, Bonnabel（"Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth", IEEE T-RO 38(2):998–1015, 2021, arXiv:2007.14097）提出了解决方案：把预积分放在 \(SE_2(3)\) 群上，利用 group-affine 结构实现**精确的协方差传播**。

A3.12b.2 \(SE_2(3)\) 预积分的核心构造

关键观察：IMU 预积分在两个关键帧之间的相对变换，可以用 \(SE_2(3)\) 群元素表示。定义预积分群元素

\[\Upsilon_{ij}=\begin{bmatrix}\Delta R_{ij} & \Delta v_{ij} & \Delta p_{ij}\\ 0 & 1 & 0\\ 0 & 0 & 1\end{bmatrix}\in SE_2(3)\]

传播规则为

\[\Upsilon_{k+1}=\Upsilon_k\cdot\begin{bmatrix}\exp((\omega_m-b_g)\Delta t)^\wedge & (a_m-b_a)\Delta t & \frac{1}{2}(a_m-b_a)\Delta t^2\\ 0 & 1 & 0\\ 0 & 0 & 1\end{bmatrix}\]

这个递推本身就是 \(SE_2(3)\) 的群乘法。由于 §A3.7.3 已经证明 IMU 动力学在 \(SE_2(3)\) 上满足 group-affine 条件，对数坐标下的误差传播满足 log-linear 方程：

\[\dot\xi = A\xi\]

其中 \(A\) 仅依赖重力 \(g\)，不依赖当前预积分量 \(\Upsilon\)。这意味着协方差传播

\[P_{k+1}=\Phi P_k \Phi^\top + \bar Q_k\]

中的状态转移矩阵 \(\Phi=\exp(A\Delta t)\) 是**精确闭式**的，不是 Taylor 近似。

本质洞察：Forster 预积分把"bias 修正"从重积分中解放出来；Brossard 不变预积分进一步把"协方差传播"从一阶近似中解放出来。前者节省计算，后者保证统计一致性。二者解决的是不同层面的问题。

A3.12b.3 Forster 预积分 vs 不变预积分对照表

维度	Forster (T-RO 2017)	Brossard (T-RO 2021)
状态表示	\(SO(3)\times\mathbb{R}^3\times\mathbb{R}^3\) (product)	\(SE_2(3)\) (矩阵群)
误差坐标	各分量独立加性/乘性	群不变误差 \(\xi=\log(\hat\Upsilon^{-1}\Upsilon)\)
协方差传播	一阶 Taylor 近似	精确 (log-linear)
\(\Phi\) 依赖	依赖当前 \(\hat R_k\)	仅依赖 \(g\)
Bias 一阶修正	✓ 支持	✓ 支持
旋转地球	不支持	✓ Coriolis + 离心力闭式
因子图接口	\(\Delta R,\Delta v,\Delta p\) 残差	\(SE_2(3)\) 残差 (需 GTSAM 自定义因子)
代码	GTSAM ImuFactor	mbrossar/SE2-3- (Python)

如果不这样做会怎样？ 在短程 VIO（关键帧间隔 < 0.5 s）中，两种方法的协方差差异很小，因为一阶近似在小时间步下足够精确。但在长程导航（关键帧间隔达 1-10 s）或高动态场景（无人机急转弯、腿式机器人跳跃）中，一阶协方差传播会系统性低估不确定性。Brossard 论文 Fig. 7 在 1 小时自动驾驶实验中展示：传统预积分的协方差**与 Monte Carlo 真实分布偏差达 30%**，而不变预积分偏差 < 3%。

A3.12b.4 与因子图后端的衔接

不变预积分的输出仍然是关键帧间的相对约束因子，可以直接插入因子图后端（5-B 将详细讨论因子图优化框架）。区别在于：

1. 残差定义：Forster 残差分别在 \(SO(3)\) 和 \(\mathbb{R}^3\) 上定义；Brossard 残差统一在 \(SE_2(3)\) 的 Log 映射中定义，保证旋转-速度-位置的耦合结构。
2. 信息矩阵：不变预积分的信息矩阵 \(P^{-1}\) 更准确反映真实不确定性，因此因子图中各因子的权重分配更合理。
3. GTSAM 集成：当前 GTSAM 4.x 的 ImuFactor 使用 Forster 格式；使用不变预积分需要自定义 NonlinearFactor，参考 mbrossar/SE2-3- 的 Python 实现。

⚠️ 陷阱：混用两种预积分的协方差

错误做法：用 Forster 方法积分得到 \(\Delta R,\Delta v,\Delta p\)，但用 Brossard 方法传播协方差（或反过来）。

现象：因子图优化结果不稳定，或 bias 估计振荡。

根本原因：两种预积分的误差坐标系不同。Forster 的协方差定义在 product manifold 的切空间，Brossard 的定义在 \(SE_2(3)\) 的切空间。即使预积分量本身数值相同，它们对应的信息矩阵结构不一样——一个是块对角近似，另一个保留了旋转-速度-位置的交叉协方差。

正确做法：预积分量和协方差必须来自同一框架。若后端使用 GTSAM ImuFactor，就全程用 Forster；若自定义不变预积分因子，就全程用 Brossard。

练习：

1.（⭐⭐⭐）在草稿纸上，把 \(SE_2(3)\) 预积分量 \(\Upsilon_{k+1}=\Upsilon_k\cdot M_k\) 的递推写成 \(5\times 5\) 矩阵乘法的显式形式。提取 \(\Delta R,\Delta v,\Delta p\) 块，与 Forster T-RO 2017 Eq. (20)-(22) 的递推对比，验证数值等价性。

2.（⭐⭐⭐⭐）推导不变预积分的 bias 一阶修正公式。提示：令 \(b=\hat b+\delta b\)，\(\Upsilon(b)\approx\Upsilon(\hat b)\exp(J_b\delta b)\)，其中 \(J_b\) 是 \(SE_2(3)\) 切空间中的 bias Jacobian。与 Forster 的 bias 修正（分别对 \(\Delta R,\Delta v,\Delta p\) 做一阶修正）对比形式差异。

A3.12b.5 预积分的历史脉络与统一视角 ⭐⭐⭐

IMU 预积分的思想可以追溯到 Lupton-Sukkarieh (2009) 的早期工作。他们首次提出"先积分 IMU 数据，再作为因子加入后端"的范式，但协方差处理粗糙。Forster et al. (2015/2017) 是第一个严格处理流形上预积分及其 bias 修正的工作，成为 VINS-Mono、ORB-SLAM3、GTSAM ImuFactor 的直接理论来源。Brossard (2021) 则是对 Forster 的自然推广——把"积分量活在群上"这个观察从直觉升格为严格的 \(SE_2(3)\) 群乘法。

三代预积分方法的演进路线：

代数	代表	预积分量的数学结构	协方差传播	bias 修正
第一代	Lupton 2009	\(\mathbb{R}^9\)（加性）	粗糙	无
第二代	Forster 2017	\(SO(3)\times\mathbb{R}^6\)（product）	一阶 Taylor	一阶 Jacobian
第三代	Brossard 2021	\(SE_2(3)\)（矩阵群）	精确 log-linear	群切空间 Jacobian

每一代解决了上一代的什么问题？

• Lupton → Forster：旋转预积分从欧拉角推广到 \(SO(3)\)，解决了大旋转时的参数化问题；bias 修正让后端优化 bias 成为可能。
• Forster → Brossard：协方差传播从一阶近似变为精确，解决了长时间积分的统计一致性问题；旋转地球效应得到闭式处理。

未来可能的**第四代**：把 bias 也纳入群结构（如 \(SE_2(3)\times\mathbb{R}^6\) 的某种半直积），使得含 bias 的预积分也享有 group-affine 保证。目前这是一个开放问题——Barrau PhD 2015 已证明不存在使"IMU + 双 bias"同时 group-affine 的李群结构，但或许存在弱化条件下的近似构造。

---

§A3.12c IKFoM 工程实战补充：FAST-LIO2 深入 ⭐⭐

A3.12c.1 FAST-LIO2 如何具体使用 IKFoM

§A3.10 已经介绍了 IKFoM 的理论框架和 FAST-LIO2 的基本架构。本节深入 FAST-LIO2 的工程实现细节，重点关注**迭代收敛行为**和**性能特性**。

FAST-LIO2 的状态定义使用 IKFoM 的 MTK_BUILD_MANIFOLD 宏：

\[\mathcal{M}=SO(3)\times\mathbb{R}^{12}\times S^2\times SO(3)\times\mathbb{R}^3\]

切空间维度为 23（\(S^2\) 贡献 2 维而非 3 维）。每个 LiDAR scan 到达时，FAST-LIO2 执行以下流程：

1. IMU 前向传播：按 ESKF 模式积分 IMU，得到先验 \(\hat x^-\) 和 \(P^-\)。
2. 点云去畸变：用 IMU 传播的逐时刻位姿对 scan 内每个点做运动补偿。
3. ikd-Tree 最近邻查询：对每个补偿后的点查找地图中最近 5 个邻居，拟合平面法向 \(u_j\) 和质心 \(q_j\)。
4. 迭代 Kalman 更新：对所有点到面残差 \(r_j=u_j^\top(T\cdot p_j-q_j)\) 联合做 IKFoM 迭代更新。

A3.12c.2 迭代更新的流形收敛判据

FAST-LIO2 的收敛判据同时检查旋转和平移增量：

\[\|\delta\theta^{(j+1)}\|<\epsilon_\theta\quad\text{且}\quad\|\delta p^{(j+1)}\|<\epsilon_p\]

默认值 \(\epsilon_\theta=0.01°\approx 1.7\times 10^{-4}\) rad，\(\epsilon_p=0.015\) m。最大迭代数 NUM_MAX_ITERATIONS 通常设为 3–4 次。

为什么 IKFoM 收敛比平面 IEKF 更快？ 对于 LiDAR 点到面残差，每次迭代不仅重线性化观测 Jacobian \(H^{(j)}\)，还重新执行 ikd-Tree 最近邻搜索（在最后两次迭代时，即 rematch_num >= 2）。这意味着不仅线性化点在移动，观测关联本身也在更新——错误关联会随着位姿收敛而被纠正。

FAST-LIO2 论文（Xu et al. T-RO 2022, Table II）的性能数据：

数据集	平均迭代次数	单帧处理时间	轨迹误差 (RMSE)
M2DGR (室内)	2.1	18 ms	0.03 m
NCLT (室外 km 级)	2.4	25 ms	0.38 m
HKU 校园 (手持)	2.3	22 ms	0.12 m

大多数场景 2–3 次迭代即收敛，说明 LiDAR 点云提供了非常强的约束。只有在退化环境（长走廊、隧道）中才会出现迭代不收敛的情况，此时需要退化检测和回退策略。

A3.12c.3 IKFoM 曲率 Jacobian 的工程意义

IKFoM 公式（He et al. ICRA 2021, eq. (27)）中的 \(J^{(j)}\) 是把第 \(j\) 次迭代的切空间与先验切空间对齐的映射。对 \(SO(3)\) 子流形，\(J^{(j)}=J_l(u)^{-\top}\)，其中 \(u=\hat x^{(j)}\boxminus\hat x^-\) 是旋转增量的李代数坐标。

当增量很小（\(\|u\|<0.01\) rad）时，\(J^{(j)}\approx I\)，曲率 Jacobian 可以省略——这就是为什么很多 ESKF 实现"看起来正确"但在大增量时会出问题。FAST-LIO2 在 esekfom.hpp 中完整实现了 \(J^{(j)}\)，确保在旋转增量较大时（如 LiDAR 帧间旋转 > 5°）仍然正确。

反事实推理：如果省略 \(J^{(j)}\) 会怎样？在低速移动（如步行速度手持 LiDAR）下，帧间旋转通常 < 1°，省略 \(J^{(j)}\) 几乎看不出差异。但在无人机高速飞行（旋转 > 10°/帧）或四足机器人快速转弯时，省略曲率 Jacobian 会导致 (1) 迭代收敛变慢或不收敛，(2) 协方差在旋转方向上系统性偏小，(3) 退化检测失效（因为 Hessian 结构被扭曲）。

⚠️ 陷阱：把 IKFoM 当作 InEKF

错误想法："FAST-LIO2 用了流形上的 Kalman 滤波，所以它是 InEKF，享有 log-linear 的一致性保证。"

实际上：IKFoM 是 iterated EKF on manifold，I = iterated，不是 invariant。FAST-LIO2 的状态 \(SO(3)\times\mathbb{R}^{15}\times SO(3)\times\mathbb{R}^3\) 是 product manifold，不是 \(SE_2(3)\) 矩阵群；其传播 Jacobian \(F\) 依赖当前估计 \(\hat R\)，不满足 log-linear 条件。FAST-LIO2 在实际中表现优异，是因为 LiDAR 提供了极其丰富的观测约束，使得一致性的理论差距在工程中不显著——但这不意味着理论等价。

正确理解：IKFoM 解决的是"如何在流形上正确做 iterated EKF"；InEKF 解决的是"如何让误差动力学与估计解耦"。二者在正交的维度上分别改进了经典 EKF。

练习：

1.（⭐⭐⭐）下载 hku-mars/FAST_LIO，在 laserMapping.cpp 中找到迭代更新循环，追踪 rematch_num 的逻辑。解释为什么在前几次迭代中不做 rematch 可以节省计算，而最后几次 rematch 可以提升精度。

2.（⭐⭐⭐）设计一个数值实验：在一维旋转群 \(SO(2)\) 上，分别用带 \(J^{(j)}\) 和不带 \(J^{(j)}\) 的 iterated EKF 估计角度。让真实角度增量为 30°，观测噪声 \(\sigma=5°\)。跑 100 次 Monte Carlo，绘制两种方法的 NEES 曲线，验证省略曲率 Jacobian 在大增量下导致 NEES 偏离 \(\chi^2(1)\)。

---

§A3.12d 流形滤波的几何直觉：纤维丛视角 ⭐⭐⭐

A3.12d.1 为什么"名义+误差"不是技术细节而是几何必然

前面反复出现的"名义状态承载大运动，误差状态承载不确定性"这个模式，可以从微分几何的**纤维丛 (fiber bundle)** 视角获得更深的理解。

考虑流形 \(\mathcal{M}\)（如 \(SE(3)\)）上的一个点 \(\chi\)。在这个点的"附近"（切空间 \(T_\chi\mathcal{M}\)）中，微小扰动可以用向量来描述。整个"流形上每点附一个切空间"的结构就是**切丛 (tangent bundle)** \(T\mathcal{M}\)。

ESKF/InEKF 的工作原理可以这样理解：

几何对象	滤波器对应	物理意义
流形 \(\mathcal{M}\) 上的一个点	名义状态 \(\hat\chi\)	当前最佳估计（大运动）
该点的切空间 \(T_{\hat\chi}\mathcal{M}\)	误差状态空间	小扰动/不确定性的线性空间
切空间中的随机变量 \(\xi\sim\mathcal{N}(0,P)\)	协方差描述	不确定性的二阶统计量
指数映射 \(\exp:\mathfrak{g}\to G\)	注入/retraction	从线性空间回到流形
平行移动 / Adjoint	Reset / 坐标变换	在不同切空间间搬运协方差

这个视角解释了几个经常困惑初学者的问题：

问题 1：为什么 reset 之后要变换协方差？

因为注入 \(\hat\chi^+= \hat\chi\cdot\exp(\hat\xi)\) 把参考点从 \(\hat\chi\) 移到了 \(\hat\chi^+\)。旧的误差 \(\xi\) 定义在 \(T_{\hat\chi}\mathcal{M}\) 中，新的误差需要定义在 \(T_{\hat\chi^+}\mathcal{M}\) 中。两个切空间虽然都是 \(\mathbb{R}^n\)，但它们是流形上**不同点处**的切空间。把协方差从一个切空间搬到另一个，需要一个线性映射（平行移动的一阶近似），这就是 reset Jacobian \(G\)。

问题 2：为什么左扰动和右扰动会给出不同的协方差？

因为它们在不同的点做切空间。左扰动 \(X=\exp(\xi)\hat X\) 的切空间附着在**单位元** \(e\) 处（然后左移到 \(\hat X\)）；右扰动 \(X=\hat X\exp(\xi)\) 的切空间附着在 \(\hat X\) 处。对同一个物理不确定性，在不同点展开得到不同的坐标表示，它们之间的关系就是 Adjoint：

\[P_L = \mathrm{Ad}_{\hat X}\,P_R\,\mathrm{Ad}_{\hat X}^\top\]

如果把它类比为平面几何：同一个椭圆在极坐标和直角坐标下描述不同，但椭圆本身没变。协方差变换只是坐标变换，不改变物理不确定性。

问题 3：为什么 InEKF 的误差动力学不依赖估计？

因为 InEKF 选择了一种特殊的误差定义——不变误差 \(\eta=\hat X X^{-1}\)（右不变）或 \(\eta=X^{-1}\hat X\)（左不变）。这种误差定义利用了群结构的**传递性**：群上任意两点之间的"距离"可以通过群乘法表达，而群乘法对整个群是一致的（它不依赖具体位于群的哪个点）。因此误差动力学自然具有"与位置无关"的性质——前提是系统动力学本身也具有与群结构兼容的形式（group-affine）。

A3.12d.2 从 Cartan 联络到 ESKF reset

对理论物理或微分几何背景的读者，ESKF 的 reset 步骤可以理解为**Cartan 联络 (Cartan connection)** 的离散版本。

在黎曼几何中，平行移动一个向量从点 \(p\) 到点 \(q\)，需要一个联络来定义"什么是平行"。对李群，天然的联络是左不变联络（由左移映射定义）或右不变联络。

ESKF 的 reset 本质上在做： 1. 在旧参考点 \(\hat\chi\) 的切空间中有一个协方差 \(P\) 2. 把参考点移到 \(\hat\chi^+\) 3. 通过近似的平行移动（\(G\) 矩阵）把 \(P\) 搬到新切空间

Solà ESKF 教程中的 \(G_{\theta\theta}=I-\frac{1}{2}[\hat{\delta\theta}]_\times\) 正是 \(SO(3)\) 上右不变联络的平行移动在小角度下的一阶近似。完整的平行移动应该是右 Jacobian \(J_r(\hat{\delta\theta})^{-1}\)，但在 \(\|\hat{\delta\theta}\|\) 很小（ESKF 每次更新后误差被清零，所以通常 < 0.01 rad）时，一阶近似已经足够。

本质��察：流形滤波中的"协方差变换"不是工程 trick，而是微分几何中"切向量在不同切空间之间搬运需要联络"这个基本事实的直接体现。忘记做这步，等于在弯曲空间上做了一次错误的平行移动——坐标系歪了，但你不知道歪了多少。

A3.12d.3 知识树：流形滤波的三层递进

把本节所有内容组织成一棵知识树，帮助形成结构化记忆：

流形滤波（根节点）
├── 为什么需要？
│   ├── 过参数化 → 协方差奇异（MEKF 解决）
│   ├── 参数化奇点 → Jacobian 病态（ESKF 解决）
│   └── 虚假可观测性 → 一致性退化��InEKF/EqF 解决）
├── 核心思想：名义 + 误差 分离
│   ├── 名义：大运动，非线性积分，无噪声
│   ├── 误差：切空间，线性 KF，有噪声
│   └── 注入：Exp 映射回流形 + reset 协方差
├── 误差定义的选择
│   ├── MEKF：$\delta q$ 乘性，最小维度
│   ├── ESKF：姿态乘性 + 其余加性，工程主流
│   ├── InEKF：$\hat X X^{-1}$ 群不变，结构最优
│   └── EqF：齐性空间等变，最一般
├── 线性化精度层次
│   ├── EKF/ESKF：一阶 Taylor，依赖估计
│   ├── InEKF：log-linear 精确（group-affine 下）
│   ├── UKF-M：sigma 点免 Jacobian，二阶精度
│   └── EqF：三阶输出近似
└── 工程选型
    ├── 只有姿态 → MEKF
    ├── IMU+外部观测 → ESKF
    ├── 动力学 group-affine → InEKF
    ├── Jacobian 难算 → UKF-M
    └── 需要因子图/窗口 → IKFoM 或进入 5-B

练习：

1.（⭐⭐⭐）从纤维丛视角解释：为什么标准 EKF 对四元数做加性更新 \(\hat q^+=\hat q+K\nu\) 再归一化，在短时间内"似乎正确"但长期会出问题？提示：考虑归一化操作对切空间的隐式投影及其对协方差的影响。

2.（⭐⭐⭐）对 \(SO(3)\) 上的一个旋转 \(R=\hat R\exp(\delta\theta^\wedge)\)，写出完整的右 Jacobian \(J_r(\delta\theta)\) 的闭式表达（Barfoot SER 2ed Eq. 7.78）。在 \(\|\delta\theta\|=0.01\) rad 和 \(\|\delta\theta\|=0.5\) rad 两种情况下，计算 \(J_r\) 与单位矩阵 \(I\) 的 Frobenius 范数差，说明何时可以省略 reset Jacobian。

3.（⭐⭐⭐⭐）尝试把 CT-ESKF 的���方差变换矩阵 \(T\) 写成 Adjoint 和置换矩阵的乘积形式。对 IMU 状态（\(R,v,p,b_g,b_a\)），从右扰动 ESKF 变换到左不变误差 InEKF，写出 \(T\) 的块结构。

---

§A3.13 流形滤波族全景对比 ⭐⭐

表 1：方法横评

方法	状态空间	误差定义	Jacobian 需求	一致性	典型应用
标准 EKF	\(\mathbb{R}^n\)（含四元数）	加性	解析，沿轨迹	overconfident（FEJ 修补）	通用（不优）
MEKF	\(SO(3)\) + 欧氏 bias	姿态乘性 + bias 加性	解析（简单）	良好（姿态）	航天器 AOCS、AHRS
ESKF	\(SE(3)\)/IMU + 欧氏	姿态乘性 + 其他加性	解析（Solà F 矩阵）	良好（需 reset）	VIO、惯性导航
InEKF（perfect）	Lie 群 \(G\)（\(SE_2(3)\) 等）	\(\eta=\hat X X^{-1}\) 或 \(X^{-1}\hat X\)	精确，与 \(\hat X\) 无关	自动一致（log-linear）	腿式、运动学（无 bias）
Imperfect InEKF	\(G\times\mathbb{R}^k\)	pose 乘性 + bias 加性	\(A_t\) 部分依赖 \(\hat X\)	弱化一致性，经验优越	Cassie、VIO+bias
IKFoM	通用混合流形	\(\boxplus/\boxminus\)	解析（用户提供）	局部	FAST-LIO2（LiDAR-IO）
UKF-M	可平行化流形	retraction \(\varphi\)	免解析（sigma）	取决于 \(\varphi\)	雅可比难算的问题
EqF	齐性空间 \(G/H\)	\(e=\phi(\hat X^{-1},\xi)\) 固定原点	线性化于原点，\(C^\circ\) 常量	严格一致（NEES 吻合）	EqVIO、SLAM

表 2：理论谱系

EKF (1960s)
 ├─ MEKF (Lefferts-Markley-Shuster 1982, JGCD) — SO(3) 乘性误差
 │   └─ Markley 2003 JGCD — 误差表示系统对比
 ├─ UKF (Julier-Uhlmann 1997)
 │   └─ UKF-on-Lie (Brossard-Bonnabel-Condomines IROS 2017)
 │        └─ UKF-M (Brossard-Barrau-Bonnabel ICRA 2020) — 可平行化流形
 ├─ ESKF (Solà et al., 2000s-) — SE(3)/IMU 推广 MEKF
 │   ├─ VINS-Mono 预积分 (Forster T-RO 2017)
 │   └─ 高翔 SLAM-in-AD 第 3 章
 ├─ Symmetry-preserving observers (Bonnabel-Martin-Rouchon 2008)
 │   └─ Left/Right InEKF (Bonnabel CDC 2007, Barrau PhD 2015)
 │        └─ InEKF as stable observer + SE₂(3) (Barrau-Bonnabel TAC 2017)
 │             ├─ Contact-Aided InEKF (Hartley IJRR 2020) — Cassie
 │             ├─ Invariant Preintegration (Brossard T-RO 2021)
 │             └─ Imperfect IEKF — bias augmentation
 └─ IKFoM (He-Xu-Zhang ICRA 2021) — iterated, 通用流形
      ├─ FAST-LIO (Xu-Zhang RA-L 2021)
      └─ FAST-LIO2 (Xu et al. T-RO 2022) — ikd-Tree + direct

Mahony-Hamel-Trumpf Equivariant Systems Theory (arXiv:2006.08276, 2020)
 └─ EqF (van Goor-Hamel-Mahony CDC 2020, arXiv:2010.14666) — 齐性空间
      ├─ EqVIO (T-RO 2023, arXiv:2205.01980) — SE_2(3) × SOT(3)^n
      ├─ ARCRAS 综述 (Mahony-vGoor-Hamel 2022, arXiv:2108.09387)
      └─ Bias-EqF (Fornasier-Ng 2022)

表 3：Group-Affine 验证

系统	是否 group-affine	说明
\(SO(3)\) + 陀螺（无 bias）	✓	纯左不变 \(\dot R = R\omega^\wedge\)
\(SO(3)\) + 陀螺 + bias	✗	bias 加性破坏
\(SE(3)\) 运动学 \(\dot R=R\omega^\wedge,\dot p=Rv\)	✓	左不变 + 右不变组合
\(SE_2(3)\) IMU（无 bias）	✓	Barrau 关键观察（§A3.7.3）
\(SE_2(3)\) IMU + 加速度/陀螺 bias	✗	无 Lie 群同时保证加性 bias 与 group-affine（Barrau PhD 2015）
\(SE_{N+2}(3)\) Contact（无 bias）	✓	Hartley 2018/2020
\(SO(3)\times\mathbb{R}^3\times\mathbb{R}^3\) 上 IMU	✗	重力 + 速度耦合失效（对比 \(SE_2(3)\)）

---

§A3.14 摄动约定速查——集成 bug 的头号来源 ⭐⭐

表 4：七大库摄动约定对照

库	\(SE(3)\) 切空间顺序	四元数约定	摄动方向	雅可比	文件证据
manif	[ρ(trans,3), θ(rot,3)] trans-first	Hamilton	右摄动 \(X\oplus\phi=X\exp(\phi^\wedge)\)	右 \(J_r\)	README "Tangent spaces"
Sophus	[υ(trans), ω(rot)] trans-first	Hamilton	常见封装用右扰动 \(T\exp(\xi^\wedge)\)；底层提供 exp/log	需按项目 Plus 定义核对	se3.hpp / 项目封装
GTSAM	[ω(rot), ρ(trans)] rot-first ⚠	Hamilton（Rot3 多后端）	右摄动	ChartAtOrigin::Retract	Pose3.cpp Logmap：xi << omega, translation
OpenVINS	[δθ, δp, δv, b_g, b_a]（15D）	JPL (x,y,z,w)	左乘 四元数误差	JPL MSCKF	ov_type::JPLQuat；docs.openvins.com
VINS-Mono	[p, q, v, b_a, b_g]	Hamilton	乘性 \(\delta\theta\)（global 坐标系 first-order）	右 J	Qin 2017；factor/integration_base.h
ROS PoseWithCovariance	[x,y,z,rx,ry,rz] trans-first	Hamilton (x,y,z,w)	无滤波语义	—	geometry_msgs
\(SE_2(3)\) 文献（Barrau/Hartley）	[θ(rot), ν(vel), ρ(pos)] rot-first	—	右不变	Adjoint	TAC 2017 / IJRR 2020

两大集成陷阱：

1. GTSAM ↔ Sophus/manif 6D 顺序互反：把 GTSAM 优化输出的 \(6\times 6\) 协方差直接喂给 Sophus 或 manif 可视化，位置与姿态协方差椭球"对调"。修复——置换矩阵 \(\Pi=\begin{bmatrix}0 & I_3\\ I_3 & 0\end{bmatrix}\)，\(P'=\Pi P \Pi^\top\)。

2. Hamilton ↔ JPL 四元数二义性：Hamilton 满足 \(ij=k\)（Eigen、Sophus、GTSAM、ROS、ORB-SLAM、VINS-Mono）；JPL 满足 \(ji=k\)，左手代数（OpenVINS、MSCKF、AOCS 代码）。OpenVINS 官方 issue #100 指出：q_JPL_GtoI.xyzw == q_Hamilton_ItoG.xyzw——相同 \(xyzw\) 数值在两个约定下**语义相反**。跨库对接必须显式转换层。

---

§A3.15 常见陷阱清单（16 条） ⭐⭐

#	陷阱	症状	根因	解法
1	Reset Step 遗漏	长期协方差偏移；\(\chi^2\) 门限误伤	δx←0 后协方差未按新 nominal 变换	Solà §3.3 Eq. 178：\(\mathbf{P}\leftarrow\mathbf{GPG}^\top\)
2	左/右摄动混用	大机动下 NEES 持续超 \(\chi^2(0.95)\)	预测用右不变，更新用左不变	全程固定约定，代码入口 static_assert
3	切空间顺序库间不一致	协方差椭球位置/姿态互换	manif [ρ,θ] vs GTSAM [ω,ρ]	置换矩阵 \(\Pi\) 重排协方差
4	Hamilton vs JPL 四元数	RViz 姿态箭头镜像	\(ij=k\) vs \(ji=k\) 乘法顺序相反	命名空间隔离 + 显式转换层
5	小角度 \(\exp/\log\) 数值不稳	\(\theta \to 0\) 时 NaN	\(\sin\theta/\theta\) 0/0	\(\theta<10^{-8}\) 时用 Taylor 展开
6	Adjoint 协方差搬运方向错	左右 EKF 切换后椭球旋转 \(\theta\)	\(P_l=\mathrm{Ad}_XP_r\mathrm{Ad}_X^\top\) 写反	对照 Solà micro-Lie Eq. 60
7	IMU 预积分残差 Jacobian 错误	VINS 初始化 OK 但加速后尺度漂移	bias 一阶修正乘错、坐标系混淆	对照 Qin 2017 Eq. 15, 17；单元测试 b≡0
8	偏置一阶修正项遗漏	bias 估计发散	预积分未对 bias 变化重线性化	\(\|\Delta b\|\) 超阈值强制 re-propagate
9	\(SE_2(3)\) group-affine 被 bias 破坏	Perfect InEKF 理论保证失效	bias 加性非 group-affine	Imperfect InEKF（Hartley 2020）
10	观测 Jacobian 流形链式法则错	新测量加入后立即发散	\(R^{-1}\) 对 \(\delta\theta\) 的导数漏负号	autodiff::Dual 数值校验
11	协方差失对称	Cholesky 失败	舍入 + 非 Joseph 更新	P=0.5(P+P^T)；Joseph form
12	IMU-相机时间不同步	高动态抖动，频率匹配帧率	软件时间 \(\ne\) 硬件时间戳	在线估计 \(t_\text{offset}\)；IMU 插值
13	单目尺度 + \(b_a\) 耦合不可观	直线运动 + 匀加速下尺度漂移	FIM 对 \(s\) 的信息 \(\propto\|a_\text{true}\|^2\)	初始化强制"8 字"激发；FEJ；尺度先验
14	Yaw/position null-space 漂移	长轨迹 yaw 1°/min	标准 EKF 线性化破坏不可观结构	FEJ / OC-EKF / InEKF（根治）
15	FEJ 与 InEKF 冲突	协方差不升反降	两套一致性修复重复使用	二选一，InEKF 单独用
16	Iterated EKF 不收敛 fallback	GN 迭代振荡致整滤波挂	大残差 + 强非线性	限制迭代数（FAST-LIO 默认 4）；step-size backtracking；\(\chi^2\) outlier 拒绝

---

§A3.16 工程库映射与学习资源 ⭐

表 5：C++/Python 库对应表

库	URL	用途	关键入口
artivis/kalmanif	github.com/artivis/kalmanif	对比 EKF/IEKF/UKF-M	examples/demo_se_2_3.cpp；ExtendedKalmanFilter、InvariantExtendedKalmanFilter、SquareRootExtendedKalmanFilter、UnscentedKalmanFilterManifolds
artivis/manif	github.com/artivis/manif	Lie 群 header-only	SE3::rplus/rminus，Jr/Jl
strasdat/Sophus	github.com/strasdat/Sophus	Lie 群 C++	Sophus::SE3::log() 返回 [υ;ω]
borglab/gtsam	github.com/borglab/gtsam	因子图优化	Pose3::Logmap 返回 [ω;ρ]（警告！）
RossHartley/invariant-ekf	github.com/RossHartley/invariant-ekf	C++ Contact-InEKF	InEKF.h、RobotState.h
UMich-BipedLab/Contact-Aided-Invariant-EKF	github.com/UMich-BipedLab/...	MATLAB 参考实现	Cassie 实验
CAOR-MINES-ParisTech/ukfm	github.com/CAOR-MINES-ParisTech/ukfm	Python UKF-M	ukfm.UKF、phi/phi_inv
hku-mars/IKFoM	github.com/hku-mars/IKFoM	C++ 通用 ESEKF	MTK_BUILD_MANIFOLD 宏，esekfom::esekf
hku-mars/FAST_LIO	github.com/hku-mars/FAST_LIO	LiDAR-IO	src/laserMapping.cpp、include/use-ikfom.hpp
pvangoor/eqvio	github.com/pvangoor/eqvio	C++ EqVIO	—
rpng/open_vins	github.com/rpng/open_vins	MSCKF 参考（JPL）	Propagator、UpdaterHelper
HKUST-Aerial-Robotics/VINS-Mono	github.com/HKUST-Aerial-Robotics/VINS-Mono	VIO（Hamilton）	factor/integration_base.h
ethz-asl/rovio	github.com/ethz-asl/rovio	Iterated EKF VIO	Bloesch 2017 IJRR
mbrossar/SE2-3-	github.com/mbrossar/SE2-3-	Python 预积分	Brossard T-RO 2021
gaoxiang12/slam_in_autonomous_driving	github.com/gaoxiang12/slam_in_autonomous_driving	高翔 SAD 第 3 章	src/ch3/eskf.hpp

表 6：必读教材与论文

类别	资源	链接	章节
教材	Barfoot SER 2ed (2024)	asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf	Part II ch 7-8 Lie / ch 9-11 pose
教材	Solà ESKF 教程	arxiv.org/abs/1711.02508	73 页全文
教材	Solà micro Lie theory	arxiv.org/abs/1812.01537	17 页，manif 理论基础
教材	Crassidis & Junkins Optimal Estimation 2ed	—	§7 MEKF
教材	Chirikjian Stochastic Models on Lie Groups Vol.2	—	Ch 5–7
论文	Barrau-Bonnabel TAC 2017	arxiv.org/abs/1410.1465	Thm 1/2/3/4
论文	Hartley et al. IJRR 2020	arxiv.org/abs/1904.09251	§II–VI
论文	Brossard et al. ICRA 2020 UKF-M	arxiv.org/abs/2002.00878	Alg 1/2
论文	Brossard et al. T-RO 2021 \(SE_2(3)\)	arxiv.org/abs/2007.14097	§III–IV
论文	van Goor & Mahony T-RO 2023 EqVIO	arxiv.org/abs/2205.01980	全文
论文	Mahony et al. ARCRAS 2022 EqF 综述	arxiv.org/abs/2108.09387	全文
论文	He-Xu-Zhang ICRA 2021 IKFoM	arxiv.org/abs/2102.03804	全文
论文	Xu et al. T-RO 2022 FAST-LIO2	arxiv.org/abs/2107.06829	§IV-VI
论文	Lefferts-Markley-Shuster JGCD 1982	DOI 10.2514/3.56190	全文
论文	Markley JGCD 2003	DOI 10.2514/2.5048	全文
论文	Forster et al. T-RO 2017	arxiv.org/abs/1512.02363	§III 预积分
论文	Bonnabel CDC 2007	silvere-bonnabel.com/pdf/CDC07.pdf	早期 InEKF
论文	Bell & Cathey TAC 1993	DOI 10.1109/9.250476	IEKF=GN
论文	Sommer et al. 2018 四元数批判	arxiv.org/abs/1801.07478	Hamilton/JPL
视频	Solà Lie theory for the roboticist 2021	youtube.com/watch?v=gy8U7S4LWzs	110 min
视频	Solà IROS 2020 workshop	youtube.com/watch?v=QR1p0Rabuww	—
视频	Stachniss Photogrammetry & Robotics	youtube.com/c/CyrillStachniss	EKF/SLAM 系列
中文	aipiano InEKF 三部曲	aipiano.github.io/2019/04/28/不变扩展卡尔曼滤波3/	原理推导
中文	高翔《自动驾驶中的 SLAM 技术》	电子工业出版社 2023	Ch 3 ESKF，Ch 6 IEKF
中文	VINS-Mono 公式推导	arxiv.org/abs/1912.11986	全文
中文	深蓝学院 SLAM 课程	shenlanxueyuan.com	视觉 SLAM / 多传感器融合
中文	S-FAST_LIO（注释版）	github.com/zlwang7/S-FAST_LIO	逐式讲解

表 7：关键公式速查

对象	公式
右摄动集中高斯	\(X = \bar X\exp(\xi^\wedge),\ \xi\sim\mathcal{N}(0,P)\)
左右协方差换算	\(P_L = \mathrm{Ad}_{\bar X}P_R\mathrm{Ad}_{\bar X}^\top\)
\(SE_2(3)\) Ad	\(\mathrm{Ad}_X = \begin{bmatrix}R & 0 & 0\\ v_\times R & R & 0\\ p_\times R & 0 & R\end{bmatrix}\)
Group-Affine	\(f(ab)=f(a)b+af(b)-af(e)b\)
InEKF Log-Linear	无噪声时 \(\dot\xi = A_t\xi\) 精确；噪声作为一阶随机扰动进入协方差传播，\(A_t\perp\hat X\)
IMU \(A\) on \(SE_2(3)\)	\(A=\begin{bmatrix}0&0&0\\(g)_\times&0&0\\0&I&0\end{bmatrix}\)
ESKF Reset	\(\mathbf{P}\leftarrow\mathbf{GPG}^\top,\ G_{\theta\theta}=I\mp[\tfrac{1}{2}\hat{\delta\boldsymbol{\theta}}]_\times\)
InEKF 乘法更新	若 \(z\approx H\xi\) 且 \(\eta^R=\hat X X^{-1}\)，则 \(\hat X_{k\|k} = \exp((-K z)^\wedge)\hat X_{k\|k-1}\)
IKFoM 快 \(K\)	\(K=(H^\top R^{-1}H+P^{-1})^{-1}H^\top R^{-1}\)
UKF-M sigma	\(\xi_j=\pm\mathrm{col}(\sqrt{(\lambda+d)P})_j\)

---

§A3.17 学习时间预算与自测题 ⭐

学习时间预算

档位 3（博士入学，45–55 h）： - §A3.1–A3.2 前置回顾：3 h - §A3.3–A3.5 ESKF：10 h（精读 Solà 前半 + 手写 Python） - §A3.6 MEKF：3 h - §A3.7–A3.8 InEKF 理论与算法：12 h（Barrau-Bonnabel TAC 2017 + aipiano 三部曲） - §A3.9 Contact-InEKF：6 h（Hartley RSS 2018 精读 + invariant-ekf 编译） - §A3.10 IKFoM/FAST-LIO2：8 h（代码走读 + 跑 EuRoC） - §A3.11 UKF-M：5 h（ukfm Python 例子） - §A3.14–A3.15 摄动约定 + 陷阱表：4 h - 合计：51 h

档位 4（博士毕业，再加 15–25 h）： - §A3.12 EqF / EqVIO：10 h（arXiv:2010.14666 + 2205.01980 + 2108.09387） - Imperfect IEKF vs EqF 理论对比与证明细节：5 h - InEKF vs EKF 在 EuRoC 上自己跑 NEES 对比：8 h - 合计：+23 h

自测题（10 题，渐进难度）

档位 3 入门： 1. ESKF 为什么要把状态拆成 nominal + error 两部分？列出 Solà §2.1 的四条理由，并用一句话各自说明工程含义。 2. Reset Step 的协方差变换 \(G_{\theta\theta}=I-[\tfrac{1}{2}\hat{\delta\theta}]_\times\) 中为什么会出现 \(\tfrac{1}{2}\)？推导到 \(\delta q\approx[1,\tfrac{1}{2}\delta\theta]^\top\) 这一步。 3. 手推：在局部（右）摄动下，ESKF 预测矩阵 \(F\) 的 \((3,3)\) 块为什么是 \(\mathbf{R}^\top\{\omega\Delta t\}\) 而不是 \(I-[\omega]_\times\Delta t\)？给出两者在 \(\omega=100°/s,\ \Delta t=10\) ms 下的误差量级。 4. MEKF 是 ESKF 的什么特例？给出状态维度与误差定义的差别。

档位 3 进阶： 5. 写出 \(SE_2(3)\) 上 IMU 动力学的矩阵形式 \(\dot X = f_u(X)\)；验证 group-affine 恒等式 \(f(X_1X_2)=f(X_1)X_2+X_1f(X_2)-X_1f(\mathrm{Id})X_2\) 的**位置块**（\(v\) 项）。 6. 对比右不变误差 \(\eta^R=\hat X X^{-1}\) 与左不变误差 \(\eta^L=X^{-1}\hat X\)：哪一个适合配 GPS 测量？哪一个适合配 body-frame 速度计？为什么？ 7. InEKF 的 log-linear 定理为什么能"自动"保持 VIO 的 yaw 不可观子空间？用 \(A_t\) 与 \(\hat X\) 的依赖关系说明。

档位 4 研究级： 8. 证明（或查文献给出 Barrau PhD 2015 的论证）：不存在 Lie 群结构使得"IMU + 加速度/陀螺 bias"的完整动力学同时满足 group-affine。这对"完美 InEKF"的工程使用意味着什么？ 9. 对比 IKFoM 的 \(\boxplus/\boxminus\) 构造与 InEKF 的 Lie 群乘法：二者在 FAST-LIO2 中具体选择了哪一种？FAST-LIO2 在理论上是否享有 InEKF 的 trajectory-independent 吸引域？说明理由。 10. EqF 如何把 InEKF 的"状态必须是李群"放宽到"状态在齐性空间"？给出 EqVIO 的齐性空间 \(M^{VI}_n(3) = \mathcal{T}^{VI}_n(3)/\alpha\) 的具体商结构，并解释为什么商掉 \(SE_{e_3}(3)\) 子群对应 VIO 的 4 维不可观方向。

---

§A3.17b 从直觉到严格：Group-Affine 条件的完整推导 ⭐⭐⭐

为什么需要 group-affine

前面我们多次引用了 group-affine 条件的结论（误差动力学不依赖估计），但没有从头推导过这个条件**为什么**能保证这个性质。这一节补齐这个推导链。

起点：考虑李群 \(G\) 上的动力学 \(\dot X = f_u(X)\)，其中 \(u\) 是外部输入。定义右不变误差 \(\eta = \hat X X^{-1}\)。

目标：找到 \(f_u\) 需要满足什么条件，使得 \(\dot\eta\) 不依赖 \(X\)（只依赖 \(\eta\) 和 \(u\)）。

Step 1：计算 \(\dot\eta\)。

\[\dot\eta = \dot{\hat X}X^{-1} + \hat X\frac{d}{dt}(X^{-1})\]

利用 \(\frac{d}{dt}(X^{-1})=-X^{-1}\dot X X^{-1}\)（矩阵求逆的微分规则），得

\[\dot\eta = f_u(\hat X)X^{-1} - \hat X X^{-1}\dot X X^{-1} = f_u(\hat X)X^{-1} - \eta f_u(X) X^{-1}\]

Step 2：目标简化。

���望 \(\dot\eta\) 只关于 \(\eta\) 的函数。注意 \(\hat X = \eta X\)，所以

\[\dot\eta = f_u(\eta X)X^{-1} - \eta f_u(X) X^{-1}\]

要让右端不依赖 \(X\)，需要 \(f_u(\eta X)X^{-1} - \eta f_u(X) X^{-1}\) 只关于 \(\eta\)。

Step 3：group-affine 条件的充分性。

假设 \(f_u\) 满足 group-affine：\(f_u(AB) = f_u(A)B + Af_u(B) - Af_u(I)B\)。令 \(A=\eta, B=X\)：

\[f_u(\eta X) = f_u(\eta)X + \eta f_u(X) - \eta f_u(I)X\]

代入 \(\dot\eta\)：

\[\dot\eta = [f_u(\eta)X + \eta f_u(X) - \eta f_u(I)X]X^{-1} - \eta f_u(X)X^{-1}$$ $$= f_u(\eta) + \eta f_u(X)X^{-1} - \eta f_u(I) - \eta f_u(X)X^{-1}$$ $$= f_u(\eta) - \eta f_u(I)\]

结论：在 group-affine 条件下，\(\dot\eta = f_u(\eta) - \eta f_u(I)\)，完全不依赖 \(X\)（真实状态）！这就是 Barrau-Bonnabel TAC 2017 定理 1 的核心。

Step 4：log-linear 性质。

令 \(\eta = \exp(\xi^\wedge)\) 并取对数，对小 \(\xi\) 一阶展开：

\[\dot\xi \approx \frac{d}{dt}\log(\eta) = [df_u/d\eta|_I - f_u(I)]_{\text{tangent map}} \cdot \xi = A_u \xi\]

其中 \(A_u\) 只通过 \(f_u\) 在单位元附近的结构确定，不依赖 \(\hat X\)。这就是 log-linear 方程 \(\dot\xi = A\xi\)。

为什么这很重要的类比理解。考虑一个日常类比：你在开车时想估计自己偏离导航路线多远。如果道路是直的（线性），那么偏差的变化率只取决于你当前的偏差大小和方向——不管你在高速公路的第 1 公里还是第 100 公里。但如果道路是弯曲的（非线性），偏差的变化率就依���于你所处的弯道曲率——也就是依赖于你的绝对位置。Group-affine 条件本质上说的是：即使动力学是"弯曲的"（非线性），但由于群的对称性，误差的演化方式在群上所有点都是一样的。就像一条螺旋线——虽然整体看是弯曲的，但沿着螺旋线走的人在每个点感受到的局部曲率完全相同。

group-affine 条件的必要性讨论

上面证明了 group-affine 是**充分条件**。是否也是必要���件？严格来说，只要 \(f_u(AB)X^{-1}-\eta f_u(X)X^{-1}\) 最终只关于 \(\eta\)，就够了——这允许比 group-affine 更弱的条件。但 Barrau (PhD 2015, §3.3) 证明，在一些合理的正则性假设下（\(f_u\) 对 \(X\) 解析、\(G\) 连通），group-affine 也是必要条件。因此对实际系统，检验 group-affine 就是检验"InEKF 是否能精确工作"的判据。

---

§A3.18 与其他子任务的桥梁 ⭐

• ← 5-A1（贝叶斯滤波基础）：本文的 KF 方程（Kalman 增益、Joseph form、协方差传播）继承自 5-A1；本文把它们从 \(\mathbb{R}^n\) 搬到流形。
• ← 5-A2（经典非线性滤波 EKF/UKF/CKF/GHKF + FEJ/MSCKF）：本文第 §A3.3–§A3.6 的 ESKF 是 5-A2 中 EKF 的流形扩展；§A3.11 UKF-M 是 5-A2 中 UKF 的流形扩展；§A3.14 的摄动约定直接指导 MSCKF/OpenVINS 工程实现。
• ← 专题 3（李群基础）、专题 5（李群不确定性）：本文 §A3.2 及所有 \(\exp/\log/\mathrm{Ad}\) 公式来自专题 3/5；本文允许重叠，作为独立完整教学单元。
• → 5-A4（粒子滤波与 Rao-Blackwellized）：本文的流形概念（\(\boxplus/\boxminus\)）是 PF on Lie group 的直接前置。
• → 5-B（平滑算法 / 因子图）：本文 §A3.14 的 GTSAM 摄动约定直接衔接因子图优化；ESKF = 因子图的前向传播。
• → 5-D（Barrau-Bonnabel 精读 / 一致性理论）：本文 §A3.7 的 log-linear 定理、§A3.12 的 EqF NEES 一致性是 5-D 的基石。
• → 专题 5 扩展（\(SE_2(3)\) preintegration）：本文 §A3.10 的 IKFoM 与 Brossard T-RO 2021 的 \(SE_2(3)\) 预积分是前沿预积分方法。

---

§A3.18b 流形滤波的"不可能三角"与工程权衡 ⭐⭐⭐

在实际系统设计中，流形滤波面临一个类似于分布式系统 CAP 定理的**不可能三角**：

            一致性保证
           /          \
          /            \
   通用性 ——————————— 实现简单性

方法	一致性	通用性	简单性	牺牲了什么
ESKF	中等	高（混合流形）	高	一致性（需 FEJ 补救）
InEKF	高（log-linear）	低（需 group-affine）	中	通用性（bias 破坏结构）
IKFoM	中等	高	中	一致性（不自动保证）
UKF-M	中等	高	高（免 Jacobian）	计算效率（sigma 点）
EqF	高	中（需齐性空间+lift）	低	简单性（理论门槛高）

工程启示：没有一个方法能同时达到三个顶点。选型时的优先级通常是：

1. 产品级系统（PX4、ARCore）：优先简单性和通用性 → ESKF + FEJ
2. 学术 SLAM 系统（OpenVINS、VINS-Mono）：优先一致性和通用性 → ESKF + FEJ + 仔细的可观性管理
3. 腿足机器人（Cassie、MIT Mini Cheetah）：优先一致性 → InEKF（动力学天然 group-affine，无视觉长期 bias）
4. 研究前沿（EqVIO）：优先一致性和理论优雅 → EqF（接受高实现复杂度）

理解这个不可能三角，就不会犯"总是用最新最复杂方法"的错误。正确的问题不是"哪个方法最好"，而是"在我的约束下，哪个维度可以牺牲"。

本质洞察：工程系统的滤波器选型不是学术品味问题，而是在一致性、通用性和实现简单性三者之间做明确的取舍。最好的方法是**你能正确实现、正确调试、正确维护**的那个。

---

结论：一个核心原则

核心原则——"让误差活在切空间，让状态活在流形，让 Jacobian 结构独立于状态"。

这一原则统摄了从 1982 年 MEKF 到 2023 年 EqVIO 的 40 年流形滤波史。MEKF 首次把它在 \(SO(3)\) 上实现；ESKF 把它推广到 IMU 驱动的 SE(3) 系统；InEKF 通过 group-affine + log-linear 定理把它**严格化**——"Jacobian 与状态独立"从启发式（FEJ）变成精确恒等式；UKF-M 把它变为"免雅可比版本"；EqF 把它从李群放宽到齐性空间，进一步揭示 VIO/SLAM 的对称性本质。

工程师取用这一原则的最短路径： 1. 先判断状态空间：李群（\(SE_2(3)\)、\(SE_{N+2}(3)\)）→ 考虑 InEKF；齐性空间或复杂混合流形 → EqF 或 IKFoM。 2. 再判断动力学：group-affine → perfect InEKF；bias 破坏 → Imperfect InEKF 或 EqF。 3. 最后选工具：Jacobian 好算 → EKF/ESKF/InEKF；Jacobian 难算 → UKF-M；有因子图/平滑需求 → GTSAM 风格。

但所有选择的前提是同一件事——把摄动约定、切空间排序、四元数代数像宪法一样贯穿整个代码库。这比任何高深理论都更能决定你的 SLAM/VIO/腿式机器人是否能活过 10 分钟真实场景。这是本教学单元给博士申请者最朴素、也最昂贵的一条建议。

历史演进的鸟瞰。从 1982 年的 MEKF 到 2025 年的 CT-ESKF，流形滤波 40 余年的发展可以用三波浪潮概括：

• 第一波（1982-2007）：航天器需求驱动。Lefferts-Markley-Shuster 1982 提出 MEKF 解决航天器姿态估计中的四元数协方差奇异问题；之后十余年主要限于航天器 AOCS 领域，因为地面机器人的速度和角速度较小，欧拉角/四元数 EKF "够用"。Bonnabel 2007 首次把对称性理论引入滤波。

• 第二波（2008-2020）：VIO/SLAM 一致性危机。Huang-Mourikis-Roumeliotis 2008-2010 系统揭示了 EKF-SLAM 的伪观测性病理。这一发现迫使社区寻找结构性解决方案：FEJ（启发式）、OC-EKF（投影）、InEKF（代数根治）。同期 Solà 的 ESKF 教程（2017）成为 VIO 工程标准参考，Forster 预积分（2017）让因子图后端处理 IMU 成为可能。FAST-LIO/FAST-LIO2（2021-2022）证明流形方法可以在 LiDAR-IO 中实现毫秒级实时性。

• 第三波（2020-2025）：等变对称性的统一理论。Mahony-van Goor 等人把 InEKF 的群不变性推广到齐性空间上的等变性，EqVIO (2023) 在 VIO benchmark 上达到或超过优化方法的精度同时保持滤波器的计算效率。CT-ESKF (2025) 从协方差变换角度统一了各种误差定义。未来方向可能包括：(1) 含 bias 的完全等变预积分；(2) 学习驱动的自适应流形选择；(3) 与可微仿真、NeRF 联合的端到端流形估计。

这条历史线给读者的启示是：每一代方法都不是凭空出现的，而是前一代方法的具体局限催生的。理解这些局限比记住公式更重要——因为当你面对一个新问题时，判断"当前方法的局限在哪里"的能力，比"能否默写最新公式"的能力更有价值。

与后续章节的衔接。

• → A4（Kalman 族全景收口）：把 A3 的流形方法放入"先验×线性化×几何×迭代"四轴坐标系中定位，完成从方法个体到方法族的视角升级。
• → 5-B（因子图与非线性最小二乘）：ESKF 预测等价于因子图中的前向消息传递；InEKF 的群乘法更新等价于单步流形 GN；预积分因子直接插入因子图后端。
• → 5-C（iSAM2 与 Bayes 树）：当需要重估历史状态时（回环、退化恢复），从滤波进入平滑/图优化是必然路径。
• → 运动控制方向：腿足机器人的 Contact-Aided InEKF 直接服务于 MPC 的状态反馈。无人机的 ESKF 是 PX4 飞控主回路的核心。

读者自检清单（学完本章后应能回答）：

编号	自检问题	合格标准
1	为什么四元数的四维协方差会奇异？	能写出约束 \(q^\top q=1\) 并推出切空间三维
2	ESKF 的 reset 步骤在做什么？	能说出"注入误差到名义状态后，把协方差搬到新切空间"
3	Group-affine 条件的具体含义？	能写出恒等式并对 \(SE_2(3)\) IMU 验证
4	InEKF 如何解决 yaw 不可观问题？	能说出"\(A\) 与 \(\hat X\) 无关，不产生虚假信息"
5	UKF-M 和标准 UKF 的核心区��？	能说出"sigma 点通过 Exp 生成，均值通过无噪声传播"
6	左扰动和右扰动如何换算？	能写出 \(P_L=\mathrm{Ad}_{\bar X}P_R\mathrm{Ad}_{\bar X}^\top\)
7	GTSAM 和 manif 的切空间顺序差异？	能说出"GTSAM \([\omega;\rho]\) rot-first，manif \([\rho;\theta]\) trans-first"

本章常见误解汇总

误解	正确理解
ESKF 把状态"拆成三个"是为了简化计算	ESKF 三态分解（名义态 \(\bar x\)、误差态 \(\delta x\)、真实态 \(x\)）的根本动机是让误差态始终在**切空间**（欧氏空间）上演化，从而合法使用高斯分布和加法更新，而非单纯为了减少计算量
四元数归一化后就可以直接用标准 EKF	归一化只修复了模值约束，但 \(4\times 4\) 协方差沿四元数方向仍有零特征值，Cholesky 必然奇异；正确做法是用 \(3\times 3\) 误差协方差（MEKF/ESKF），而非在四维空间上打补丁
左扰动和右扰动只是记号不同，随意选都行	左/右扰动对应不同的误差定义（\(\delta\xi_L\) vs \(\delta\xi_R\)），其协方差通过 Adjoint 换算 \(P_L = \mathrm{Ad}_{\bar X}P_R\mathrm{Ad}_{\bar X}^\top\)；混用会导致更新方向错误，是流形滤波工程 bug 的头号来源
InEKF 只是"把 EKF 放到李群上"	InEKF 的核心贡献不是流形化（ESKF 已经做到），而是利用 group-affine 结构使误差动力学的 \(A\) 矩阵与估计 \(\hat X\) 无关，从而从根源消除虚假可观测性——这是结构性优势而非记号变换
UKF-M（流形 UKF）比 InEKF 更通用所以更好	UKF-M 免 Jacobian、适用于任何流形（不需 group-affine 条件），但它不享有 InEKF 的"估计无关误差动力学"性质，因此在一致性保持上不如 InEKF；两者互补而非替代
ESKF 的 reset 步骤可以省略	Reset（将误差态注入名义态后将误差归零、协方差搬到新切空间）是保持协方差与当前线性化点一致的关键步骤；省略 reset 会导致协方差在旧切空间累积，长期偏差增大
\(SE_2(3)\) 群只是数学记号的花哨包装	\(SE_2(3)\) 把 IMU 状态 \((R,v,p)\) 统一为 \(5\times 5\) 矩阵群元素，使得 IMU 动力学**精确满足** group-affine 条件（仅当重力为常数）；这不是记号而是让 InEKF 误差动力学线性化无关估计的**数学前提**

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关节
量测更新后状态反而远离真值	residual 符号或左右乘方向错	1.用一维平移群构造最小反例 2.对比 \(z=\hat X^{-1}y\) vs \(z=y-h(\hat x)\) 3.验证更新方向	§A3.7
协方差短时间内变得过小	reset Jacobian 漏掉或噪声映射漏块	1.检查 \(F_iQ_iF_i^\top\) 维度 2.打印 reset 前后 \(P\) 特征值 3.验证噪声块进入正确位置	§A3.5
VIO yaw 方差异常收缩	可观性约束被线性化破坏	1.计算 \(HN\) 是否为零 2.对比 FEJ 开关下 NEES 3.检查 InEKF 误差定义	§A3.7-8
ESKF 静止段 bias 漂走	IMU-相机外参或时间戳错误	1.静止段回放看 \(v_w\approx 0\) 2.检查 \(T_{bc}\) 方向 3.Allan 方差标定 bias 参数	§A3.4
Cholesky 失败	协方差失去正定性	1.检查 Joseph form 2.执行对称化 \(P\leftarrow\frac{1}{2}(P+P^\top)\) 3.切换 SR-UKF	§A3.11
不同库输出姿态方向相反	Hamilton/JPL 四元数或左/右摄动混用	1.同一 \(q\) 作用于已知向量对比 2.检查 Adjoint 方向 3.加显式转换层	§A3.14

⚠️ 陷阱一：ESKF 更新后忘记 reset。 误差态注入 nominal 后，协方差仍停留在旧切空间，会让后续 Jacobian 的坐标原点错位。解决方法：每次注入后严格执行 \(P\leftarrow GPG^\top\)，其中 \(G\) 的 \(\theta\theta\) 块为 \(I-\frac{1}{2}[\hat{\delta\theta}]_\times\)。

⚠️ 陷阱二：InEKF residual 与乘法更新符号不配套。 若 \(z\approx H\xi\) 表示估计相对真值的误差，更新时就必须施加反向校正 \(\hat X^+= \exp((-K z)^\wedge)\hat X^-\)（左乘负号）。

⚠️ 陷阱三：把 product retraction 当成严格 \(SE_2(3)\) 群运算。 简单的 \(v,p\) 欧氏加法便于工程实现，但不自动继承群指数中左 Jacobian \(J_l\) 对速度/位置的耦合项，长时间积分会累积几何偏差。

⚠️ 陷阱四：把 IKFoM 等同于 InEKF。 FAST-LIO2 使用 iterated EKF on manifold（I = iterated），其传播 Jacobian 仍依赖 \(\hat R\)，不享有 InEKF 的 log-linear 一致性保证。在 LiDAR 约束丰富的场景差别不大，但在退化环境中理论差距会显现。

本质洞察：流形滤波的难点不在 Kalman 公式，而在”误差代表什么”。只要误差定义、innovation 和注入方向三者不闭合，同一套增益公式会把正确量测变成错误校正。

---

本章小结

知识点	核心内容	解决什么问题	工程落地
流形滤波动机	旋转/位姿不是向量空间	过参数化协方差奇异、参数化奇点、虚假可观测性	避免四维四元数 EKF
MEKF 三维误差	\(q_t=\hat q\otimes\delta q\)，\(\delta\theta\in\mathbb{R}^3\)	姿态最小维度表示	航天器 AOCS
ESKF 三态分解	nominal + error + true	IMU 高频积分与低频校正解耦	VIO、惯性导航主流
ESKF Reset	\(P\leftarrow GPG^\top\)	注入后切空间坐标对齐	每次更新后执行
InEKF 不变误差	\(\eta=\hat X X^{-1}\)	误差动力学与估计解耦	腿式机器人、无 bias 系统
Group-affine 条件	\(f(ab)=f(a)b+af(b)-af(e)b\)	保证 log-linear 精确传播	\(SE_2(3)\) IMU 动力学
Log-linear 定理	\(\dot\xi=A\xi\)，\(A\perp\hat X\)	协方差传播精确、吸引域轨迹无关	InEKF 一致性保证
UKF-M	Sigma 点通过 \(\exp\) 生成	免解析 Jacobian 的流形滤波	雅可比难算的系统
IKFoM	\(\boxplus/\boxminus\) + 迭代 GN	通用混合流形上的 iterated EKF	FAST-LIO2
EqF/EqVIO	齐性空间等变性	放宽 InEKF 对李群的要求	单目 VIO
不变预积分	\(SE_2(3)\) 群乘法传播	协方差传播精确（vs Forster 一阶）	长程导航
摄动约定	左/右、Hamilton/JPL	跨库集成的首要 bug 源	全工程统一约定

---

累积项目：本章新增模块

项目：从零构建流形状态估计器

本章在前两章基础上新增以下模块：

模块	功能	依赖
so3_exp_log.py	\(SO(3)\) 指数/对数映射，含小角度安全处理	Ch1 向量运算
eskf_imu.py	18 维 ESKF（\(p,v,\theta,b_g,b_a,g\)），含 reset	A1 KF 更新
invariant_error.py	\(SE_2(3)\) 右不变误差定义与 Adjoint	so3_exp_log
ukfm_so3.py	\(SO(3)\) 上的 UKF-M（磁力计姿态估计）	A2 UKF sigma 点

本章新增任务：

1. 实现 eskf_imu.py，用仿真 IMU+GPS 数据跑 100 步，验证 NEES \(\approx\chi^2(18)\)。
2. 在 invariant_error.py 中实现 group-affine 验证函数：给定 \(f(X)=AX+XB\)，数值检验恒等式对随机 \(X_1,X_2\in SE_2(3)\) 成立。
3. 对比 eskf_imu.py 与标准欧氏 EKF（姿态用欧拉角）在 \(>90°\) 初始误差下的行为。

---

延伸阅读

类型	资源	难度	重点
教材	Solà, Quaternion kinematics for the error-state Kalman filter, arXiv:1711.02508	⭐⭐	ESKF 完整推导，73 页
教材	Solà et al., A micro Lie theory for state estimation in robotics, arXiv:1812.01537	⭐⭐	manif 理论基础，17 页
教材	Barfoot, State Estimation for Robotics 2ed (2024), Part II	⭐⭐⭐	Lie 群 + 位姿估计
论文	Barrau-Bonnabel, The Invariant Extended Kalman Filter as a Stable Observer, IEEE TAC 2017	⭐⭐⭐	InEKF 核心定理
论文	Hartley et al., Contact-Aided Invariant EKF for Legged Robot State Estimation, IJRR 2020	⭐⭐⭐	腿足 InEKF
论文	Brossard et al., A Code for UKF-M, ICRA 2020	⭐⭐	UKF-M 实现
论文	van Goor-Mahony, EqVIO, IEEE T-RO 2023	⭐⭐⭐⭐	等变滤波 VIO
论文	He-Xu-Zhang, IKFoM, arXiv:2102.03804	⭐⭐⭐	通用流形 IEKF
论文	Brossard et al., Associating Uncertainty to Extended Poses..., T-RO 2021	⭐⭐⭐⭐	不变预积分
论文	Cui et al., CT-ESKF, arXiv:2511.00453, 2025	⭐⭐⭐⭐	协方差变换统一框架
代码	artivis/kalmanif	⭐⭐	EKF/IEKF/UKF-M 对比
代码	hku-mars/FAST_LIO	⭐⭐⭐	IKFoM 工程实现
代码	RossHartley/invariant-ekf	⭐⭐⭐	InEKF C++ 参考
视频	Solà Lie theory for the roboticist 2021, YouTube	⭐⭐	110 min 讲座

---

练习

1. 在平移群 \(x\in\mathbb R\) 上模拟 \(\eta=\hat x-x\)，分别用 \(z=\hat x-y\) 和 \(z=y-\hat x\) 推导校正符号。
2. 按 18 维 ESKF 误差顺序写出 \(F_iQ_iF_i^\top\) 的块结构，标出加速度噪声、陀螺噪声和两个 bias 随机游走进入的位置。
3. 对同一个 landmark residual，分别采用 world 加性误差和 invariant error 推导 Jacobian，比较是否依赖 \(\hat R,\hat p\)。 4.（跨章综合题）结合 A1 的 NEES 诊断方法和 A2 的 EKF 实现，设计一个实验：对 2D bearing-only SLAM，分别用标准 EKF 和流形误差（角度用 \(SO(2)\) 乘性误差），跑 50 次 Monte Carlo 后对比 ANEES 曲线。解释为什么流形版本的 ANEES 更接近理论 \(\chi^2\) 界。

5.（⭐⭐⭐）实现一个最小的 \(SO(3)\) MEKF：状态为姿态 \(q\) 和陀螺偏置 \(b_g\)（共 6 维误差态），观测为重力方向和磁力计方向。用仿真数据验证 (a) 初始误差 \(>45°\) 时仍能收敛，(b) NEES 长期保持在 \(\chi^2(6)\) 的 95% 置信区间内。

6.（⭐⭐⭐⭐）阅读 Barrau-Bonnabel TAC 2017 的 Theorem 1 证明，在草稿纸上复现 group-affine 条件 \(\Rightarrow\) 右不变误差动力学不依赖 \(X\) 的完整推导（本章 §A3.17b 给出了简化版，论文原文处理了含噪声的情况）。

7.（⭐⭐⭐⭐）比较三种方法在相同仿真环境下的 NEES 表现：(a) 标准 EKF（姿态用欧拉角），(b) ESKF + FEJ，(c) InEKF（假设 group-affine 成立）。使用 2D 单机器人+3 路标 SLAM 设置。绘制三者的 ANEES 曲线，并标注 \(\chi^2\) 的 95% 上下界。预期 (c) 最好，(a) 最差——但具体差多少取决于轨迹的非线性程度。

---

§A3.19 流形滤波与现代机器学习的交汇 ⭐⭐⭐⭐

近年来，流形滤波与深度学习的结合成为活跃研究方向。几个值得关注的趋势：

趋势 1：学习驱动的 VIO + 群结构滤波器。 Guo et al. (J. Field Robotics, 2024) 提出将端到端 VO 网络与 \(SE_2(3)\)-EKF 结合：网络输出帧间相对位姿作为"虚拟观测"，\(SE_2(3)\)-EKF 负责融合 IMU ��保证统计一致性。这种架构把学习方法的感知优势与几何方法的一致性保证结合起来。

趋势 2：可微流形滤波。 把 ESKF/InEKF 的全部前向计算图变成可微的，允许端到端训练噪声参数 \(Q,R\) 或甚至观测模型 \(h\)。挑战在于 Exp/Log 映射的梯度在大角度时数值不稳定，需要特殊的 Taylor 展开或替代参数化。

趋势 3：Adaptive Covariance + Hybrid Error-State。 Adaptive Covariance 与混合误差态 EKF/UKF (Springer IJCIS, 2025) 提出根据运动模式自适应切换误差定义和协方差参数，兼顾低动态（ESKF 足够）和高动态（需要 UKF sigma 点）场景。

这些方向目前仍处于学术探索阶段，但预示了流形滤波的未来不是被深度学习"替代"，而是作为**几何先验**嵌入到更大的学习系统中——正如卷积是平移等变的先验，流形滤波提供旋转/刚体变换等变的先验。

> 本质洞察：流形滤波与深度学习不是对立关系。流形滤波提供"物理世界的几何约束"（旋转是旋转、位置是位置），深度学习提供"从数据中学习复杂映射"的能力。最好的系统会把两者结合——用学习处理感知，用几何处理估计。