50_李群上的不确定性
专题5：李群上的不确定性与概率分布¶
前置要求：专题 3（李群基础）+ 专题 4（Jacobian 与 BCH 公式） 档位：核心 ★3（能推导核心定理） / 进阶 ★4（能证明收敛性、理解前沿） 建议时间：档位 3 约 20–30 h（2–3 周）；档位 4 再加 10–15 h
为什么你必须学这个专题¶
一句话底线：旋转矩阵的9个元素受6个正交约束，不是独立随机变量——直接写 \(N(\bar{R}, \Sigma)\) 会立刻破坏 SO(3) 约束。 这意味着所有在欧氏空间中"理所当然"的概率操作（求均值、传播协方差、融合量测）都不能照搬到姿态估计中。正确的做法是把不确定性搬到切空间（李代数），定义 Concentrated Gaussian: \(X = \bar{X} \cdot \mathrm{Exp}(\boldsymbol{\xi}),\; \boldsymbol{\xi} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma})\)。这一模型是 InEKF、IMU 预积分、VIO 后端、多传感器融合的共同数学起点。不掌握本专题，Barrau-Bonnabel、Forster、Barfoot-Furgale 三条论文线都无法进入。
前置自测（答不出 2 题以上 -> 先回专题 3-4 复习）¶
- 为什么不能说旋转矩阵的 9 个元素各自服从高斯分布？
- 为什么单位四元数的 4 维协方差不是姿态的最小协方差？
- 右扰动协方差和左扰动协方差为什么不能直接比较？
- 为什么 \(X_1X_2\) 的协方差不是 \(\Sigma_1+\Sigma_2\)？
- 为什么大不确定性下高斯椭球会变成 banana 形状？
一、核心章节清单（档位 3 必学）¶
§5.1 为什么李群上的概率是特殊的¶
| 问题 | 欧氏空间 \(\mathbb{R}^n\) | 李群 SO(3)/SE(3) |
|---|---|---|
| 状态空间拓扑 | 全局是向量空间 | 紧致/非紧致流形，有非平凡拓扑 |
| 均匀分布 | Lebesgue 测度 | Haar 测度（双不变测度） |
| PDF 定义 | 直接写 \(p(\mathbf{x})\) | 依赖坐标卡选择，需引入体积元修正 |
| 高斯化的障碍 | 无 | 约束（正交性/行列式=1）、周期性、覆盖（四元数 2:1） |
关键洞察清单：
- Dirac delta 在流形上的定义依赖 Riemannian 度量，不能随意搬运。
- 轴角参数化有 \(2\pi\) 周期性，四元数对 SO(3) 是 2:1 覆盖——对这些参数化直接做高斯都会引入人为奇异或双模态。
- Haar 测度给出 SO(3) 上的"均匀分布"，其在四元数参数下恰好是 \(S^3\) 上的均匀分布，但在欧拉角参数下 不均匀。
§5.2 Perturbation Model：把概率搬到切空间¶
本节将专题4中"确定性扰动"升级为"随机扰动"。
两种摄动定义：
| 左摄动 (Type-1 / Global) | 右摄动 (Type-2 / Local) | |
|---|---|---|
| 定义 | \(\tilde{X} = \mathrm{Exp}(\boldsymbol{\xi}_L)\,\bar{X}\) | \(\tilde{X} = \bar{X}\,\mathrm{Exp}(\boldsymbol{\xi}_R)\) |
| 协方差所在帧 | 世界帧 / 空间帧 | 体帧 / 物体帧 |
| 互转 | \(\boldsymbol{\xi}_L = \mathrm{Ad}(\bar{X})\,\boldsymbol{\xi}_R\) | \(\boldsymbol{\xi}_R = \mathrm{Ad}(\bar{X})^{-1}\,\boldsymbol{\xi}_L\) |
| 协方差互转 | \(\boldsymbol{\Sigma}_L = \mathrm{Ad}(\bar{X})\,\boldsymbol{\Sigma}_R\,\mathrm{Ad}(\bar{X})^\top\) | 反向同理 |
| 典型使用者 | OpenVINS（JPL 四元数）、部分 InEKF 文献 | GTSAM、manif、ORB-SLAM3、VINS-Mono |
工程选择原则： 当量测在体帧（IMU、轮速计）时右摄动更自然；当量测在世界帧（GPS、地图匹配）时左摄动更自然。关键是全系统必须一致——混用左/右是最常见的 bug 来源。
§5.3 Concentrated Gaussian¶
定义： 称 \(\tilde{X}\) 服从以 \(\bar{X}\) 为均值、\(\boldsymbol{\Sigma}\) 为协方差的 Concentrated Gaussian，若
PDF 表达式（Barfoot 书 §8.3.1）：
其中归一化常数 \(\eta \approx (2\pi)^{-n/2}|\det\boldsymbol{\Sigma}|^{-1/2}\)，仅在 小协方差假设 下成立——这就是"concentrated"的含义。对于 SO(3) 协方差 \(\boldsymbol{\Sigma} \in \mathbb{R}^{3\times3}\)；对于 SE(3) 协方差 \(\boldsymbol{\Sigma} \in \mathbb{R}^{6\times6}\)。
为什么只是"近似"： 严格的归一化常数需要指数映射的 Jacobian 行列式修正（与 \(\boldsymbol{\xi}\) 的幅值有关）。当 \(\|\boldsymbol{\xi}\|\) 很小时修正趋于1，近似高度准确；大扰动下会出现系统偏差，引出下面的 banana 分布问题。
§5.4 SE(3) 上的协方差传播（Compounding）¶
核心问题： 给定两个独立的不确定位姿 \(X_1 \sim (\bar{X}_1, \boldsymbol{\Sigma}_1)\), \(X_2 \sim (\bar{X}_2, \boldsymbol{\Sigma}_2)\)，求复合 \(X_1 \cdot X_2\) 的分布。
一阶近似公式（Barfoot §8.3.3，最常用）：
为什么需要 Adjoint： 两个扰动分别定义在各自切空间，复合前必须把 \(\boldsymbol{\Sigma}_1\) 通过 \(\mathrm{Ad}(\bar{X}_2^{-1})\) 搬运到同一切空间。这本质上是"坐标系转换"的李群版本。
四阶修正（Barfoot-Furgale 2014 T-RO 的核心贡献）： 利用 BCH 公式展开到四阶项，再用 Isserlis 定理（高斯矩的 Wick 缩并）计算四阶矩，额外得到涉及 \([\boldsymbol{\Sigma}_1^{\circ}, \boldsymbol{\Sigma}_2]\) 的修正项。论文通过 Monte Carlo 验证，四阶公式在大不确定性下显著优于一阶和二阶（sigma-point 等价）方法。
逆位姿的协方差： \(X = \bar{X}\,\mathrm{Exp}(\boldsymbol{\xi})\) 则 \(X^{-1}\) 的协方差为 \(\boldsymbol{\Sigma}_{\mathrm{inv}} = \mathrm{Ad}(\bar{X})\,\boldsymbol{\Sigma}\,\mathrm{Ad}(\bar{X})^\top\)。
§5.4bis 协方差传播的完整推导路径 ⭐⭐⭐¶
为了加深理解，这里给出复合公式 \(\Sigma_{12} \approx \operatorname{Ad}(\bar X_2^{-1})\Sigma_1\operatorname{Ad}(\bar X_2^{-1})^\top + \Sigma_2\) 的完整推导。
起点：两个独立不确定位姿（右扰动）：
目标：求 \(X_{12} = X_1 X_2\) 的分布参数。
Step 1：写出复合：
Step 2：利用 Adjoint 搬运 \(\operatorname{Exp}(\xi_1)\) 越过 \(\bar X_2\)：
因此：
Step 3：用 BCH 一阶近似合并两个 Exp：
（一阶近似忽略了 \(\frac{1}{2}[\cdot,\cdot]\) 项）
Step 4：识别结果：
Step 5：计算协方差（\(\xi_1\), \(\xi_2\) 独立）：
反事实推理：如果不用 Adjoint 搬运，直接写 \(\Sigma_{12} = \Sigma_1 + \Sigma_2\)，相当于假设两个扰动已经在同一切空间——这在 \(\bar X_2 = I\) 时成立（\(\operatorname{Ad}(I) = I\)），但一般情况下是错误的。在实际 VIO 中，如果连续两帧的相对位姿 \(\bar X_2\) 包含显著旋转，忽略 Adjoint 会使协方差椭球的方向完全错误。
§5.5 Banana 分布：长时间传播的非高斯性¶
现象： 差速轮机器人沿直线行驶，航向噪声通过运动学耦合到位置——在笛卡尔坐标 \((x, y, \theta)\) 下，位置分布呈明显的 "香蕉形"，协方差椭圆完全不能描述。
关键发现（Long, Wolfe, Mashner, Chirikjian, RSS 2012）： 同一分布在 指数坐标（李代数） 下却近似高斯。Banana 形状是 Exp 映射的非线性造成的，不是分布本身的问题。
实际含义：
- 长时间 dead reckoning 后，用协方差椭球表示位置不确定性是错误的。
- IMU 预积分分段进行（keyframe 之间）而非一次从头积到尾，正是为了让每段内扰动足够小、高斯假设仍然成立。
- Forster 预积分论文中用 KL 散度验证：短段内流形高斯与 Monte Carlo 真值高度吻合，长段则偏差显著。
§5.6 Unscented Transform on Lie Groups¶
动机： 避免显式推导 Jacobian，用 sigma points 自动捕获非线性效应。
UKF-M 方法（Brossard-Barrau-Bonnabel, ICRA 2020）：
- 在李代数 \(\mathbb{R}^d\) 上按标准 UT 生成 \(2d+1\) 个 sigma points \(\boldsymbol{\xi}^{(i)}\)
- 通过 \(\chi^{(i)} = \hat{\chi} \cdot \mathrm{Exp}(\boldsymbol{\xi}^{(i)})\) 映射到流形
- 每个 sigma point 分别通过非线性动力学传播
- 用 Log 映射回切空间，加权计算均值与协方差
| EKF on Lie Group | UKF-M | |
|---|---|---|
| Jacobian 需求 | 需要解析推导 | 不需要 |
| 精度（小噪声） | 一阶，与 UKF-M 相当 | 二阶 |
| 精度（大噪声） | 显著退化 | 更鲁棒 |
| 计算成本 | \(O(d^2)\) | \(O(d^3)\)（\(2d+1\) 次传播） |
| 开源代码 | 各团队自行实现 | github.com/CAOR-MINES-ParisTech/ukfm (Python+MATLAB) |
§5.7 在 InEKF、VIO、传感器融合中的应用串讲¶
本节是承前启后的桥梁，简要预告各方向如何使用上述工具：
- InEKF：定义不变误差 \(\eta = \hat{X}^{-1}X\)，其对数 \(\boldsymbol{\xi}=\mathrm{Log}(\eta)\) 在 group-affine 系统下满足线性 ODE——这使得 EKF 的 Jacobian 不依赖状态估计，从根本上消除正反馈发散（详见专题6）。
约定警告：此处采用 Hartley IJRR 2020 常用约定 \(\eta=\hat{X}^{-1}X\)。注意 Barrau-Bonnabel TAC 2017 原文使用 \(\eta^L=X^{-1}\hat{X}\)（互为逆元），详见专题6 §6.12.2。
- VIO 预积分：keyframe 间的 \((\Delta R, \Delta v, \Delta p)\) 协方差是 \(9\times9\) 矩阵，定义在李代数上，通过递推 \(\boldsymbol{\Sigma}_{k+1} = A_k\boldsymbol{\Sigma}_k A_k^\top + B_k Q_k B_k^\top\) 传播。
- GPS-INS 融合：GPS 量测在世界帧（左摄动自然），IMU 传播在体帧（右摄动自然），两者融合时必须用 Adjoint 统一。
二、进阶章节清单（档位 4 选学）¶
| 主题 | 核心文献 | 关键概念 | 与本专题的关系 |
|---|---|---|---|
| InEKF 严格理论 | Barrau-Bonnabel TAC 2017 (arXiv:1410.1465) | Group-affine 系统定义、log-linear 误差传播、局部渐近稳定性证明 | Concentrated Gaussian + Ad 转换的直接应用 |
| Equivariant Filter | Mahony-van Goor CDC 2020 (arXiv:2010.14666)；ARCRAS 2022综述 (arXiv:2108.09387) | 齐次空间上的对称性、equivariant lift、曲率修正 Riccati 方程 | 比 InEKF 更一般的框架，InEKF 是其在群上的特例 |
| \(\mathrm{SE}_2(3)\) 扩展位姿群 | Barrau-Bonnabel ICRA 2020 (arXiv:2003.03908)；Brossard et al. T-RO 2021 (arXiv:2007.14097) | \((R, v, p)\) 统一为 \(5\times5\) 矩阵群，9维李代数；IMU 动力学在此群上恰好 group-affine | 把 §5.4 协方差传播推广到含速度的 IMU 场景 |
| 李群上的 Cramér-Rao 下界 | Chahbazian-Barrau-Bonnabel, Automatica 2024 | 递推 CRLB、Fisher 信息在李代数上的定义 | 评价滤波器最优性的基准 |
| 信息几何与 Fisher-Rao 度量 | Chirikjian Vol.2 Ch.5-7 | Riemannian 度量与 Fisher 度量的对应 | 理论深化，非工程必需 |
三、核心教材深度对照¶
| 教材 | 核心章节 | 深度 | 难度 | 获取方式 |
|---|---|---|---|---|
| Barfoot《State Estimation for Robotics》2nd ed. (2024) | §8.3 Probability and Statistics (pp.338-369)：Concentrated Gaussian 定义、PDF、compounding、fusion、逆位姿 | 工程导向，公式完整 | ★★★ | 免费 PDF: asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf |
| Chirikjian《Stochastic Models…》Vol.2 (2012) | Ch.5-7：Fokker-Planck on Lie groups、扩散过程、banana 分布完整推导 | 数学严格 | ★★★★★ | Springer（图书馆） |
| Solà "A micro Lie theory" (arXiv:1812.01537) | Section IV + Appendices A-F：六种群的完整 Jacobian 表 | 速查手册级别 | ★★ | 免费 arXiv |
| Solà "Quaternion kinematics for ESKF" (arXiv:1711.02508) | §4.4 + §5-6：ESKF 完整推导，含 nominal/error/true 三态分解 | 92页完整教程 | ★★★ | 免费 arXiv |
推荐阅读顺序（档位3）： Solà micro Lie → Barfoot §8.3 → Barfoot-Furgale 2014 → Forster 2017 → Solà ESKF
四、最关键的论文¶
必读（档位 3）¶
- Barfoot & Furgale, "Associating Uncertainty with Three-Dimensional Poses" (T-RO 2014) — 协方差传播的理论基石，含一阶与四阶公式，SLAM 社区引用量极高。
- Barrau & Bonnabel, "The Invariant EKF as a Stable Observer" (TAC 2017, arXiv:1410.1465) — InEKF 开山论文，定义 group-affine 系统，证明 log-linear 误差传播与局部渐近稳定性。
- Forster, Carlone, Dellaert, Scaramuzza, "On-Manifold Preintegration" (T-RO 2017, arXiv:1512.02363) — 将李群不确定性用于 VIO，\(9\times9\) 预积分协方差递推，代码集成于 GTSAM。
推荐（档位 3+/4）¶
- Brossard, Barrau, Bonnabel, "A Code for UKF on Manifolds" (ICRA 2020, arXiv:2002.00878) — UKF-M 开源实现，含 IMU/GNSS/LiDAR-SLAM 基准。
- van Goor & Mahony, "EqVIO" (T-RO 2023, arXiv:2205.01980) — Equivariant Filter 应用于 VIO，在 EuRoC 上达到 SOTA。
- Brossard et al., "Associating Uncertainty to Extended Poses" (T-RO 2021, arXiv:2007.14097) — \(\mathrm{SE}_2(3)\) 上的不确定性与预积分。
- Chauchat et al., "Invariant Smoothing on Lie Groups" (IROS 2018) — 从滤波到平滑的不变方法推广。
五、与 C++ 库的具体映射¶
| 库/系统 | 摄动约定 | 切空间排序 | 协方差处理 | 链接 |
|---|---|---|---|---|
| GTSAM | 右摄动 | \([\alpha,\beta,\gamma,x,y,z]\) 旋转在前 | noiseModel::Gaussian/Diagonal/Isotropic；Pose3::AdjointMap() 做帧转换 |
github.com/borglab/gtsam |
| manif | 右摄动（right-plus） | \([\rho,\theta]\) 平移在前（与 GTSAM 相反！） | plus()=\(X\cdot\mathrm{Exp}(\delta)\)；解析 Jacobian |
github.com/artivis/manif (~1700★) |
| invariant-ekf | 左不变/右不变误差 | Barrau-Bonnabel 约定 | 基于 group-affine 理论；含 ROS wrapper | github.com/RossHartley/invariant-ekf (462★) |
| UKF-M | 右摄动 | 依问题自定义 | sigma points 自动传播，无需 Jacobian | github.com/CAOR-MINES-ParisTech/ukfm (Python+MATLAB) |
| OpenVINS | 左摄动（JPL 四元数） | — | MSCKF 滤波协方差管理 | github.com/rpng/open_vins |
| VINS-Mono | 右摄动（Hamilton 四元数） | \([\delta p, \delta v, \delta\theta, \delta b_a, \delta b_g]\) 15维 | 预积分协方差递推 | github.com/HKUST-Aerial-Robotics/VINS-Mono |
| ORB-SLAM3 | 右摄动 SO(3) | Forster 预积分 | 15维协方差 | github.com/UZ-SLAMLab/ORB_SLAM3 |
警告： manif 的切空间排序（平移在前）与 GTSAM（旋转在前）相反。混用时协方差矩阵的块结构会完全错乱——这是实际工程中高频 bug。
六、关键定理与公式速查¶
| 公式名称 | 表达式 | 出处 |
|---|---|---|
| Concentrated Gaussian PDF | \(p(\tilde{X}) \approx \eta\exp(-\frac{1}{2}\boldsymbol{\xi}^\top\boldsymbol{\Sigma}^{-1}\boldsymbol{\xi})\) | Barfoot §8.3.1 |
| 左/右协方差互转 | \(\boldsymbol{\Sigma}_L = \mathrm{Ad}(\bar{X})\boldsymbol{\Sigma}_R\mathrm{Ad}(\bar{X})^\top\) | Barfoot §8.1.4 |
| 一阶协方差传播 | \(\boldsymbol{\Sigma}_{12} = \mathrm{Ad}(\bar{X}_2^{-1})\boldsymbol{\Sigma}_1\mathrm{Ad}(\bar{X}_2^{-1})^\top + \boldsymbol{\Sigma}_2\) | Barfoot-Furgale 2014 |
| 逆位姿协方差 | \(\boldsymbol{\Sigma}_{\mathrm{inv}} = \mathrm{Ad}(\bar{X})\boldsymbol{\Sigma}\mathrm{Ad}(\bar{X})^\top\) | Barfoot §8.3.4 |
| Group-affine 条件 | \(f_u(ab) = f_u(a)b + af_u(b) - af_u(e)b\) | Barrau-Bonnabel 2017 Thm.1 |
| 预积分协方差递推 | \(\boldsymbol{\Sigma}_{k+1} = A_k\boldsymbol{\Sigma}_k A_k^\top + B_k Q_k B_k^\top\) | Forster 2017 |
七、学习资源汇总¶
免费核心资源¶
- Barfoot 书 2nd ed. PDF：asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf
- Solà micro Lie theory：arxiv.org/abs/1812.01537（附 manif cheat sheet）
- Solà ESKF 四元数教程：arxiv.org/abs/1711.02508（92页）
- Barrau-Bonnabel InEKF：arxiv.org/abs/1410.1465
- Forster 预积分：arxiv.org/abs/1512.02363
- GTSAM 不确定性博客系列：gtsam.org/2021/02/23/uncertainties-part2.html（Part 2 & 3）
视频资源¶
- Cyrill Stachniss SLAM 系列（含 VIO/IMU）：ipb.uni-bonn.de/teaching
- 深蓝学院"从零手写VIO"课程（贺一家、高翔）：shenlanxueyuan.com/course/388
- Dellaert Factor Graph 教程：gtsam.org/tutorials/intro.html
中文资源¶
- 高翔《视觉SLAM十四讲》第2版 (2019)：第4讲（李群李代数扰动模型）+ 第10讲（后端协方差），配套代码 github.com/gaoxiang12/slambook2
- 邱笑晨《预积分总结与公式推导》：中文社区最广泛引用的 Forster 预积分推导文档（泡泡机器人社区传播）
- 赵宇 InEKF 博客三部曲：aipiano.github.io 系列，从李群基础到 InEKF 推导到混合误差系统
- 知乎高赞："VINS-Mono 代码详细解读3—IMU预积分残差、雅克比和协方差"（zhuanlan.zhihu.com/p/148229464）
- CSDN：搜索"VINS-Mono IMU预积分 协方差"可获得多篇含代码逐行解读的文章
八、学习时间预算与节奏¶
| 内容 | 档位 | 建议时长 | 节奏建议 |
|---|---|---|---|
| §5.1-5.3（概率基础 + Concentrated Gaussian） | 3 | 8-10h | 第1周前半 |
| §5.4（协方差传播 + Barfoot-Furgale 论文精读） | 3 | 8-10h | 第1周后半 |
| §5.5-5.6（Banana 分布 + UKF-M） | 3 | 6-8h | 第2周前半 |
| §5.7（应用串讲 + 代码映射实践） | 3 | 8-12h | 第2周后半~第3周 |
| InEKF 严格理论 + Equivariant Filter | 4 | 12-15h | 额外1-1.5周 |
| \(\mathrm{SE}_2(3)\) + CRLB on Lie groups | 4 | 8-15h | 额外0.5-1周 |
| 总计 | 3: 30-40h / 4: 额外 20-30h | 2-3周(档位3) / 4-5周(含档位4) |
九、自测题目（5题）¶
-
推导 Concentrated Gaussian PDF：从 \(\tilde{X} = \bar{X}\mathrm{Exp}(\boldsymbol{\xi})\) 出发，写出 PDF，明确说明归一化常数何时精确、何时近似，以及 Exp 映射 Jacobian 行列式的角色。（档位3）
-
证明一阶协方差传播公式：设 \(X_1, X_2\) 独立，分别用右摄动定义，推导 \(X_1 X_2\) 的协方差为 \(\mathrm{Ad}(\bar{X}_2^{-1})\boldsymbol{\Sigma}_1\mathrm{Ad}(\bar{X}_2^{-1})^\top + \boldsymbol{\Sigma}_2\)，关键步骤是用 BCH 一阶近似将两个扰动合并。（档位3）
-
编程实现 SE(3) compound pose with uncertainty：用 manif 或 GTSAM，输入两个带协方差的 Pose3，输出复合位姿及其协方差，并与 \(10^5\) 次 Monte Carlo 采样结果对比。（档位3）
-
Banana effect 可视化：在 SE(2) 上模拟差速轮机器人直行 1000 步，分别在笛卡尔坐标和李代数坐标下画散点图，对比高斯拟合的 KL 散度。解释为什么 IMU 预积分要分段。（档位3+）
-
UKF-M vs EKF 对比实验：用 UKF-M 开源代码运行 IMU-GNSS 融合 benchmark（KITTI 数据集），对比 RMSE 与一致性指标（NEES），讨论计算开销差异。（档位4）
十、常见陷阱¶
| 陷阱 | 后果 | 正确做法 |
|---|---|---|
| 把旋转矩阵元素当高斯变量 | 采样结果不满足正交约束 | 在李代数上定义高斯，通过 Exp 映射到群 |
| 协方差传播时忘了 Adjoint | 两帧的协方差直接相加，量纲/帧不匹配 | 先用 \(\mathrm{Ad}\) 统一到同一切空间再相加 |
| 全系统混用左/右摄动 | 论文A用左、代码B用右导致符号混乱 | 项目初始统一约定并写入文档 |
| 大不确定性仍用高斯假设 | Banana effect 被忽略，一致性检验失败 | 分段预积分；或用 UKF-M / 粒子滤波 |
| 混淆"李代数协方差"与"参数化协方差" | 四元数分量协方差 \(\neq\) so(3) 上的协方差 | 始终在切空间操作 |
| manif 与 GTSAM 切空间排序混用 | 协方差矩阵块结构完全错乱 | 确认各库的 \([\rho,\theta]\) vs \([\theta,\rho]\) 约定 |
十一、从欧氏高斯到李群高斯 ⭐¶
本节把本专题的概率工具按推导顺序讲清楚。
核心思想只有一句：
这句话看似简单，但它改变了高斯分布、协方差传播、滤波更新和因子图噪声建模的全部写法。
上述五个问题（已列于本专题开头的前置自测中）全部指向同一个事实：
本质洞察：李群上的概率论与欧氏空间中的概率论之间，最根本的区别不是"公式更复杂"，而是"加法失效"。在 \(\mathbb{R}^n\) 中，均值和噪声可以直接相加，因为点和向量住在同一个空间。在李群上，均值是群元素，噪声是切空间向量——它们类型不同，必须通过 Exp 映射"桥接"。所有后续公式都是这一根本区别的具体展开。
十二、为什么欧氏高斯会失败 ⭐¶
12.1 欧氏高斯的隐含结构¶
在 \(\mathbb{R}^n\) 中，高斯分布写成：
它依赖三个隐含事实：
| 隐含事实 | 在 \(\mathbb{R}^n\) 中为何成立 | 在李群上为何不成立 |
|---|---|---|
| 点可加噪声 | 点和向量在同一空间 | 群元素和李代数元素不同 |
| 全局坐标唯一 | \(\mathbb{R}^n\) 无周期/覆盖 | \(SO(3)\) 有拓扑和分支 |
| 体积元平坦 | Lebesgue 测度固定 | 流形上需要 Haar/Riemannian 体积 |
如果把旋转矩阵写成：
其中 \(E\) 的 9 个元素服从高斯，则通常：
也就是说采样结果不在 \(SO(3)\) 上。
这不是小概率问题。
只要噪声非零，连续高斯几乎必然破坏正交约束。
12.2 四元数高斯也不是根治¶
单位四元数 \(q\in S^3\) 满足：
如果写：
采样后通常不满足单位长度。
归一化可以把样本拉回 \(S^3\)。
但这样得到的分布不再是普通高斯，协方差含义也不清晰。
更重要的是，\(q\) 与 \(-q\) 表示同一个旋转。
因此 \(S^3\) 是 \(SO(3)\) 的双覆盖。
如果分布跨过这个等价边界，直接对四元数分量做均值会产生错误。
12.3 正确思路：扰动在切空间¶
取名义旋转 \(\bar{R}\in SO(3)\)。
随机扰动用 \(\xi\in\mathbb{R}^3\) 表示：
然后映射到群上：
这叫右扰动模型。
左扰动模型则为：
两者都把高斯放在切空间中，而不是放在矩阵元素中。
本质洞察：李群上的高斯不是"群元素服从高斯"，而是"相对某个均值的局部误差服从高斯"。高斯性属于切空间，不属于群本身。
⚠️ 陷阱：对四元数分量做加权平均来求"均值旋转"
- 错误做法：\(\bar{q} = \frac{1}{N}\sum_i q_i\)，然后归一化。
- 后果：当样本跨过 \(q / -q\) 等价边界时，平均结果可能指向错误方向甚至接近零四元数。
- 根本原因：四元数 \(S^3\) 是 \(SO(3)\) 的双覆盖，分量加法不尊重这个等价关系。
- 正确做法：在切空间中迭代求均值（见 §17.3 的流形均值恢复算法），或使用特征值法（Markley 2007）。
十三、Haar 测度与 PDF 的意义 ⭐⭐¶
13.1 为什么要谈测度¶
概率密度不是孤立函数。
它必须相对于某个测度定义。
在 \(\mathbb{R}^n\) 中，默认测度是 Lebesgue 测度：
在李群上，没有全局线性坐标。
因此需要能与群结构兼容的体积概念。
这就是 Haar 测度。
13.2 Haar 测度的直觉¶
左 Haar 测度满足：
右 Haar 测度满足：
对紧群 \(SO(3)\)，可以取双不变 Haar 测度。
它表示：
这比欧拉角中的"角度均匀"更可靠。
欧拉角三个角分别均匀，并不等于 \(SO(3)\) 上均匀。
因为欧拉角坐标的体积元含有非平凡 Jacobian。
13.3 Concentrated Gaussian 的 PDF¶
右扰动：
其中：
定义：
这里的近似来自两个地方：
- 使用局部坐标 \(\xi\)。
- 忽略指数映射体积元的高阶修正。
当协方差很小、质量集中在均值附近时，这个近似很好。
这就是 concentrated 的含义。
💡 提示：concentrated 不是修饰语，而是假设条件
如果不确定性很大，样本覆盖 log 映射分支或流形曲率明显区域，则切空间高斯不再可靠。
这时应考虑 sigma point、粒子方法、混合分布或重新选取线性化点。
⚠️ 陷阱：认为"归一化常数不重要，可以忽略"
- 错误做法：在最大似然估计或重要性采样中省略归一化常数。
- 后果：当不同假设对应不同群元素时，归一化常数中的 Exp 映射 Jacobian 行列式项会影响似然排序，省略会导致次优解。
- 根本原因：在平坦空间中归一化常数对所有点相同，可以省略；在弯曲空间中体积元随位置变化，不可一概省略。
- 正确做法：至少检查量级。若协方差很小（concentrated 假设成立），省略安全；否则需保留修正项。
十四、左扰动、右扰动与协方差搬运 ⭐⭐¶
14.1 两种扰动¶
右扰动：
左扰动：
二者描述同一个随机群元素。
因此：
左乘 \(\bar{X}^{-1}\)：
利用共轭关系：
所以一阶甚至精确地有：
等价地：
14.2 协方差互转¶
若：
其中：
则：
反向：
这就是协方差搬运公式。
14.3 工程含义¶
在 \(SE(3)\) 中，Adjoint 含有平移项：
采用 \([\rho,\phi]\) 排序。
因此旋转不确定性会影响平移不确定性。
如果忽略 Adjoint，等价于假设所有扰动都在同一坐标系。
这在位姿链传播中通常是错的。
⚠️ 陷阱：协方差矩阵没有写明所在切空间就没有完整意义
同样一个 \(6\times6\) 矩阵，可能表示体帧扰动协方差，也可能表示世界帧扰动协方差。
数值相同不代表含义相同。
文档和代码必须写明 left/right、frame、排序。
本质洞察：左扰动和右扰动描述的是同一个物理不确定性，只是"观察视角"不同——左摄动从世界帧看误差，右摄动从体帧看误差。两者通过 Adjoint 互转，就像同一个向量在不同坐标系下有不同分量。选择哪种约定不影响物理结论，但全系统必须一致。
⚠️ 陷阱：在代码中混用两个库的协方差而不做 Adjoint 变换
- 错误做法：把 OpenVINS（左摄动）输出的协方差直接传给 GTSAM（右摄动）节点。
- 后果：协方差的物理含义被错误解读，融合结果可能看起来"合理"但一致性检验（NEES）会持续偏高。
- 根本原因：同一个 \(6 \times 6\) 矩阵在左右摄动下代表不同的物理量。
- 正确做法：通过 \(\Sigma_R = \mathrm{Ad}(\bar{X}^{-1}) \Sigma_L \mathrm{Ad}(\bar{X}^{-1})^\top\) 显式转换。
十五、复合位姿的协方差传播 ⭐⭐¶
15.1 问题设置¶
设：
其中 \(\xi_1,\xi_2\) 独立，均为右扰动。
要求：
的右扰动协方差。
名义值为：
15.2 把扰动搬到名义值右侧¶
展开：
在中间插入 \(\bar{X}_2\bar{X}_2^{-1}\)：
共轭项为：
因此：
用 BCH 一阶近似：
得到：
15.3 协方差公式¶
由于独立：
这就是常用一阶 compounding 公式。
如果 \(\xi_1,\xi_2\) 不独立，还需要交叉协方差项：
其中：
15.4 为什么不是简单相加¶
如果直接写：
等价于假设 \(\xi_1\) 与 \(\xi_2\) 已在同一切空间、同一坐标系、同一扰动侧。
实际 \(X_1\) 的扰动位于 \(\bar{X}_1\) 右侧。
复合后，它必须穿过 \(\bar{X}_2\) 才能落到 \(\bar{Y}\) 的右侧。
穿过群元素的操作正是 Adjoint。
🧠 本质洞察：Adjoint 是协方差传播中的坐标搬运器
在欧氏空间中，所有点的切空间天然等同，所以协方差可以直接相加。
在李群上，不同点的局部误差属于不同切空间表示。
Adjoint 的作用就是把它们搬到同一线性空间后再相加。
⚠️ 陷阱：用四阶公式却忘了 Isserlis 定理的高斯假设
- 错误做法：对非高斯噪声（如均匀分布、截断分布）套用 Barfoot-Furgale 2014 的四阶修正公式。
- 后果：四阶修正项依赖高斯矩的 Wick 缩并（即 Isserlis 定理：\(E[\xi_i \xi_j \xi_k \xi_l] = \Sigma_{ij}\Sigma_{kl} + \Sigma_{ik}\Sigma_{jl} + \Sigma_{il}\Sigma_{jk}\)），非高斯分布不满足此等式，修正项会给出错误结果。
- 正确做法：非高斯情况下回退到 Monte Carlo 验证，或使用 UKF-M 等无需解析矩公式的方法。
十六、逆位姿与 between factor 的不确定性 ⭐⭐¶
16.1 逆位姿协方差¶
设右扰动：
则：
希望写成 \(\bar{X}^{-1}\) 的右扰动：
令两式相等：
左乘 \(\bar{X}\)、右乘 \(\bar{X}\)：
因此：
协方差中负号消失：
16.2 between factor 的一阶结构¶
位姿图中常见相对位姿：
设：
名义：
一阶扰动可以分成两部分：
- \(X_i^{-1}\) 的扰动来自 \(-\operatorname{Ad}_{\bar{X}_i}\xi_i\)。
- 再与 \(X_j\) 复合时，需要按 compounding 公式搬运。
工程中更常用的写法是直接对残差：
求 Jacobian。
无论写法如何，核心都离不开：
16.3 因子图噪声模型¶
在 GTSAM 中，BetweenFactor 的噪声不是加在矩阵元素上，而是加在局部坐标残差上：
噪声模型：
这里 \(\Sigma_{ij}\) 是切空间协方差。
它的排序、单位和扰动约定必须与残差一致。
⚠️ 陷阱：把传感器标定协方差直接塞进 Pose3 噪声模型
传感器给出的误差可能在传感器坐标系、机体系或世界系中。
BetweenFactor 的残差却有自己的局部坐标定义。
二者之间通常需要 Adjoint 变换。
本质洞察：逆位姿协方差公式 \(\Sigma_{X^{-1}} = \mathrm{Ad}(\bar{X})\Sigma_X\mathrm{Ad}(\bar{X})^\top\) 中，负号消失不是巧合——它是因为协方差是二次型 \(E[\xi\xi^\top]\)，线性变换 \(\eta = -A\xi\) 中的负号在二次型中被平方抵消。这个"负号消失"现象在欧氏空间中也成立（\(\mathrm{Var}(-X) = \mathrm{Var}(X)\)），但初学者经常在李群场景下忘记这一点，试图手动添加负号。
十七、Unscented Transform on Manifold ⭐⭐⭐¶
17.1 为什么需要 sigma points¶
一阶传播：
只保留局部线性效应。
当不确定性较大或系统非线性明显时，均值和协方差会产生偏差。
Unscented Transform 的想法是：
17.2 流形 sigma points¶
给定名义状态 \(\bar{X}\) 和协方差 \(\Sigma\)。
在切空间生成：
映射到群：
传播：
然后需要在流形上恢复均值。
17.3 均值恢复¶
流形均值不能简单相加。
常用迭代：
- 初始化 \(\bar{Y}\) 为某个 sigma point。
- 计算局部误差：
- 加权平均：
- 更新：
- 直到 \(\|\bar{\eta}\|\) 足够小。
协方差再用：
17.4 与 EKF 的取舍¶
| 方法 | 优点 | 缺点 | 适合场景 |
|---|---|---|---|
| EKF on Lie group | 快，解析结构清楚 | 需要 Jacobian，只有一阶精度 | 高频 VIO/INS |
| UKF-M | 不需解析 Jacobian，非线性更准 | sigma point 成本高 | 中低维状态、强非线性 |
| 粒子滤波 | 可表示多峰 | 成本高，维数灾难 | 全局定位、重定位 |
| 因子图平滑 | 可重线性化，利用稀疏性 | 后端求解复杂 | SLAM/VIO 后端 |
💡 提示：UKF-M 不是 EKF 的无条件替代品
在高维 VIO 状态中，sigma points 数量随维度线性增长，每个点还要完整传播。
如果系统 Jacobian 可解析且频率很高，EKF 或 InEKF 仍然更适合。
⚠️ 陷阱：sigma points 映射到流形后直接做加权平均
- 错误做法：\(\bar{Y} = \sum_i w_i Y^{(i)}\)，像在欧氏空间中那样对群元素直接加权求和。
- 后果：结果不在流形上（例如加权平均后的矩阵不再正交），或者即使强制投影回流形也会引入系统性偏差。
- 根本原因：群元素不构成线性空间，"加法"没有定义。
- 正确做法：使用迭代流形均值算法（§17.3）：在切空间中做加权平均，用 Exp 映射更新，反复迭代直至收敛。
十八、工程实现与代码注释示例 ⭐⭐¶
18.1 SE(3) 协方差复合伪代码¶
// 采用右扰动、切向量排序 [rho, phi]。
// 输入的 covariance 都必须定义在各自名义位姿的右扰动切空间中。
struct PoseWithCov {
SE3 T;
Eigen::Matrix<double, 6, 6> Sigma;
};
PoseWithCov ComposeRightPerturbation(const PoseWithCov& a,
const PoseWithCov& b) {
PoseWithCov out;
out.T = a.T * b.T;
// a 的扰动要穿过 b.T，搬运到 out.T 的右侧切空间。
Eigen::Matrix<double, 6, 6> A = b.T.inverse().Adjoint();
out.Sigma = A * a.Sigma * A.transpose() + b.Sigma;
// 数值计算后做一次对称化，抵消浮点误差；这不是修复公式错误的方法。
out.Sigma = 0.5 * (out.Sigma + out.Sigma.transpose());
return out;
}
18.2 Monte Carlo 验证流程¶
给定 X1, Sigma1, X2, Sigma2
生成 N 个 xi1, xi2
构造样本 X1_i = X1 Exp(xi1)
构造样本 X2_i = X2 Exp(xi2)
计算 Y_i = X1_i X2_i
计算名义 Y_bar = X1 X2
把误差 eta_i = Log(Y_bar^{-1} Y_i) 收集起来
比较样本协方差与一阶公式
如果噪声很小，两者应接近。
如果噪声变大，样本分布会逐渐偏离单高斯。
这不是公式写错，而是一阶近似达到边界。
⚠️ 陷阱：Monte Carlo 验证时采样数不够就下结论
- 错误做法：用 \(N=100\) 次采样对比一阶公式和样本协方差，发现"很接近"就认为公式正确。
- 后果：\(6 \times 6\) 协方差矩阵有 21 个独立元素，\(N=100\) 的采样协方差估计本身就有很大方差，无法可靠区分一阶与四阶公式的差异。
- 正确做法：至少使用 \(N = 10^4 \sim 10^5\) 次采样。同时检查 Frobenius 范数相对误差和最大元素相对误差两个指标。
⚠️ 陷阱：对称化 0.5*(S + S.T) 掩盖了公式错误
- 错误做法：协方差公式推导有误，但因为最后做了对称化，数值"看起来没问题"。
- 后果：对称化只能修复浮点舍入误差，不能修复 Adjoint 方向错误或排序错误等公式 bug。如果对称化前后的 Frobenius 范数差异超过 \(10^{-10}\) 量级，说明公式本身可能有问题。
- 正确做法：先确认公式正确（Monte Carlo 验证），再加对称化作为数值稳定手段。
十九、李群上的 Fokker-Planck 方程与扩散 ⭐⭐⭐⭐¶
19.1 为什么需要连续时间概率演化¶
前面各节的 Concentrated Gaussian 模型处理的是**离散时刻**的不确定性快照：给定当前均值和协方差，经过一次运动或观测后更新。但很多物理场景需要描述概率分布**随时间连续演化**的过程——例如 IMU 积分中角度不确定性如何随时间扩散、多智能体系统中位姿信念如何在通信延迟下弥散、以及生成式模型（扩散模型）中噪声如何在 SE(3) 上逐步注入或移除。
在欧氏空间 \(\mathbb{R}^n\) 中，连续时间概率演化由 Fokker-Planck 方程（又称 Kolmogorov 前向方程）刻画：
其中 \(\mathbf{f}\) 是漂移项，\(D\) 是扩散系数。纯扩散情况（\(\mathbf{f}=0\)，\(D\) 为常数）退化为热方程 \(\partial_t p = \frac{D}{2}\Delta p\)。
问题是：在 SO(3) 或 SE(3) 上，\(\nabla\) 和 \(\Delta\)（Laplacian）怎么定义？直接搬用欧氏版本会犯两个错误：（1）流形上没有全局线性坐标，偏导数 \(\partial/\partial x_i\) 失去意义；（2）体积元不是常数，散度算子必须包含度量修正。Chirikjian 的《Stochastic Models, Information Theory, and Lie Groups》Vol.2（Birkhäuser, 2012）系统解决了这一问题。
19.2 SO(3) 上的热方程¶
在紧致李群 \(G\) 上，左不变扩散由**左不变 Laplacian**驱动。对 SO(3)，取标准正交基 \(\{e_1, e_2, e_3\}\) 为 \(\mathfrak{so}(3)\) 的基底（对应 \(\hat{x}, \hat{y}, \hat{z}\)），左不变 Laplacian 定义为：
其中 \(\tilde{E}_i\) 是 \(e_i\) 对应的左不变向量场。对光滑函数 \(p: SO(3) \to \mathbb{R}\)，\(\tilde{E}_i p(R)\) 的含义是"沿 \(e_i\) 方向微小旋转时 \(p\) 的变化率"：
SO(3) 上的热方程为：
由于采用的是**左不变**向量场，该扩散过程的物理含义是：在**体帧**中各方向均匀施加独立角速度白噪声（各向同性扩散）。这与 §14 中右扰动模型的直觉一致——体帧噪声对应右摄动。
19.3 Peter-Weyl 展开与精确解¶
回顾 §6.2：Peter-Weyl 定理保证紧致群上的 \(L^2\) 函数可按不可约表示的矩阵元素展开。对 SO(3)：
其中 \(D^{\ell}_{mm'}\) 是 Wigner D-矩阵。热方程在该展开下对角化——因为 \(\Delta_G D^{\ell}_{mm'} = -\ell(\ell+1) D^{\ell}_{mm'}\)（SO(3) 的 Casimir 元素作用）。因此：
高频分量（大 \(\ell\)）指数衰减，低频分量（\(\ell=0\) 对应常数，即均匀分布）不衰减。
这个精确解揭示三个重要事实：
- 任何初始分布最终趋向 Haar 均匀分布——\(\ell \ge 1\) 的所有分量衰减殆尽，只剩 \(\ell=0\)。
- 衰减速率由 \(\ell(\ell+1)\) 决定——\(\ell=1\) 分量半衰期最长，是分布"形状"消失最慢的模态。
- Concentrated Gaussian 是 \(t\) 很小时的近似——当扩散时间短、概率质量集中在均值附近时，只有低阶 \(\ell\) 分量显著，指数映射的局部展开有效，Peter-Weyl 展开退化为切空间上的高斯核。
19.4 与 Concentrated Gaussian 的联系¶
当扩散时间 \(t\) 很小，概率集中在初始点 \(\bar{R}\) 附近时，用 \(\xi = \mathrm{Log}(\bar{R}^{-1}R)\) 做局部坐标，热方程在切空间中近似为标准热方程 \(\partial_t p \approx \frac{D}{2} \nabla^2_\xi p\)，其解是 \(\xi \sim \mathcal{N}(0, Dt\,I_3)\)。这正是 §13 中 Concentrated Gaussian 的特殊情况——协方差 \(\Sigma = Dt\,I_3\) 随时间线性增长。
随着 \(t\) 增大，两个效应使 Concentrated Gaussian 失效：
| 效应 | 物理含义 | 数学表现 |
|---|---|---|
| 曲率效应 | SO(3) 不是平的，大角度偏差时切空间近似失效 | 归一化常数需 Exp 映射 Jacobian 修正 |
| 周期效应 | SO(3) 是紧致的（"转一圈回来"），概率开始"绕回来" | 分布不再单峰，最终趋向均匀 |
这与 §5.5 的 banana 分布问题本质相同：长时间扩散 = 大不确定性 = 切空间高斯失效。
本质洞察：Concentrated Gaussian 是 Fokker-Planck 精确解在短时间/小协方差极限下的局部近似。这不是两个独立理论，而是同一扩散过程在不同时间尺度下的两种观察视角。专题 5 前面各节处理的"协方差传播"，对应 Fokker-Planck 方程的离散时间步进；Peter-Weyl 展开对应完整频域分析。
19.5 物理解释：角度不确定性的扩散¶
想象一个陀螺仪持续积分角速度，但角速度含白噪声。初始时刻姿态确定（Dirac delta），之后不确定性在 SO(3) 上扩散：
- 短时间：不确定性是以初始姿态为中心的小"高斯球"，协方差 \(\Sigma \propto t\)。
- 中等时间：分布展开但仍为单峰，banana 效应开始出现（高阶修正变得重要）。
- 长时间：分布趋向均匀——陀螺仪完全"忘记"初始姿态。
这解释了为什么 INS 长时间 dead reckoning 后姿态不确定性会爆炸——不是协方差矩阵数值溢出，而是物理上确实不知道自己朝哪个方向了。IMU 预积分分段进行（keyframe 间隔 0.1-0.5 秒），正是为了把每段扩散控制在 Concentrated Gaussian 有效的范围内。
19.6 与等变扩散模型的桥梁¶
近年来，扩散生成模型（Diffusion Model, DDPM/Score Matching）在 SE(3) 上的推广引起了广泛关注（详见专题 6 §6.4）。其核心思想是：
- 前向过程：从数据分布出发，在 SE(3) 上逐步注入噪声，直到达到均匀分布——这正是 Fokker-Planck 的前向演化。
- 反向过程：学习逆时间 SDE（Score function），从均匀分布出发逐步去噪——这需要估计 \(\nabla_{\xi} \log p(X, t)\)，即流形上的 score。
Chirikjian Vol.2 的左不变扩散理论为这类模型提供了严格的数学基础。具体联系包括：
- 前向 SDE 在 SE(3) 上的 well-posedness 需要左不变扩散的存在性理论。
- Score function 的定义涉及左不变梯度 \(\tilde{E}_i \log p\)，而非欧氏梯度。
- 转移核（transition kernel）的解析形式来自 Peter-Weyl 展开。
Equivariant Diffusion Policy（CoRL 2024）和 Diffusion-EDFs（CVPR 2024）正是利用了这些工具在 SE(3) 上构建生成模型用于机器人操作。
⚠️ 陷阱：把欧氏热方程的核直接当作 SO(3) 上的转移核
- 错误做法：取 SO(3) 上的某个参数化（如欧拉角或轴角），把欧氏高斯核 \(\mathcal{N}(\bar{\theta}, Dt)\) 当作扩散转移概率。
- 后果：（1）欧拉角的体积元非常数，导致转移核在 Haar 测度下不归一；（2）轴角在 \(\|\theta\| = \pi\) 处有覆盖问题；（3）由此训练的 score function 会在测度密集区产生系统性偏差。
- 根本原因：欧氏热方程的基本解是高斯核，SO(3) 热方程的基本解是 Wigner D-矩阵的加权和——二者形式完全不同。
- 正确做法：使用 SO(3) 上的精确热核（IGSO3 kernel，Leach et al. ICLR 2022），或在小 \(t\) 极限下使用切空间高斯作为近似但明确标注适用范围。
19.7 练习¶
-
（⭐⭐⭐）稳态解推导：考虑 \(S^1\)（单位圆，即 SO(2)）上的热方程 \(\partial_t p(\theta, t) = \frac{D}{2}\frac{\partial^2 p}{\partial \theta^2}\)，其中 \(\theta \in [0, 2\pi)\) 且 \(p\) 满足周期边界条件。用 Fourier 级数 \(p(\theta, t) = \sum_{n} c_n(t) e^{in\theta}\) 求解，并证明当 \(t \to \infty\) 时 \(p \to \frac{1}{2\pi}\)（均匀分布）。讨论衰减最慢的模态对应什么物理含义。
-
（⭐⭐⭐⭐） Concentrated Gaussian 与热核的短时近似对比：在 SO(3) 上取 \(\bar{R}=I\)，\(D=0.01\)，分别计算 \(t=0.1\) 和 \(t=10\) 时精确热核（保留到 \(\ell=10\)）和 Concentrated Gaussian 近似的 PDF 值，在 \(\|\xi\| \in [0, \pi]\) 范围内绘图比较。标注两种方法偏差超过 5% 的区域。
二十、NEES 一致性测试实操 ⭐⭐¶
20.1 为什么需要一致性检验¶
前面各节推导了李群上的协方差传播、Concentrated Gaussian、UKF-M 等工具。但一个关键问题悬而未决：滤波器算出的协方差到底对不对？
所谓"对"，不是说协方差的数值精确等于某个真值——协方差本身描述的是统计量——而是说滤波器的置信度与真实误差分布是否匹配。如果滤波器说"我 95% 确信误差在 1° 以内"，但实际跑 100 次实验只有 60% 的误差在 1° 以内，那协方差就过度自信了；反之如果 99.5% 都在 1° 以内，协方差就过于保守。
**NEES（Normalized Estimation Error Squared）**是检验滤波器一致性最标准的统计工具。它源自经典的 \(\chi^2\) 检验，但在李群上的使用需要特别注意误差的定义方式。
20.2 NEES 定义¶
在欧氏空间中，设真实状态 \(x\)，估计 \(\hat{x}\)，估计协方差 \(P\)，则 NEES 定义为：
若滤波器一致（估计误差确实服从 \(\mathcal{N}(0, P)\)），则 \(\varepsilon\) 服从 \(\chi^2(n)\) 分布，其中 \(n\) 是状态维度。
\(\chi^2(n)\) 分布的均值为 \(n\)，方差为 \(2n\)。因此一致的滤波器应有 \(E[\varepsilon] = n\)。
20.3 Monte Carlo NEES 检验流程¶
单次实验的 NEES 是随机变量，无法从一次运行判断一致性。标准做法是 Monte Carlo 多次运行：
| 步骤 | 操作 | 目的 |
|---|---|---|
| 1 | 固定初始条件和系统参数 | 保证统计可比 |
| 2 | 生成 \(N\) 组独立的过程噪声和观测噪声序列 | Monte Carlo 采样 |
| 3 | 对每组噪声分别运行滤波器，记录每个时刻的 \(\varepsilon_k^{(i)}\) | 收集 NEES 样本 |
| 4 | 在每个时刻 \(k\) 计算平均 NEES：\(\bar{\varepsilon}_k = \frac{1}{N}\sum_{i=1}^{N} \varepsilon_k^{(i)}\) | 估计期望 NEES |
| 5 | 构造置信区间：\(N\bar{\varepsilon}_k \sim \chi^2(Nn)\)，取双侧 95% 区间 | 假设检验 |
若 \(\bar{\varepsilon}_k\) 在所有时刻（或绝大多数时刻）都落在 \(\chi^2(Nn)\) 的 \([2.5\%, 97.5\%]\) 区间内，则滤波器在该时刻通过一致性检验。
解释规则：
| 现象 | 含义 | 典型原因 |
|---|---|---|
| \(\bar{\varepsilon}_k \approx n\) | 一致 | 协方差估计正确 |
| \(\bar{\varepsilon}_k \gg n\)（偏高） | 过度自信 | 协方差太小、噪声建模偏低、线性化误差 |
| \(\bar{\varepsilon}_k \ll n\)（偏低） | 过度保守 | 协方差太大、噪声建模偏高 |
| 前期一致后期偏高 | 累积退化 | 非线性效应、banana 分布、不可观方向错误处理 |
20.4 李群上的 NEES：切空间误差¶
在李群上，"\(x - \hat{x}\)"没有意义——群元素不能相减。正确做法是用对数映射定义切空间误差：
这是右摄动下的误差。若采用左摄动，则为 \(\boldsymbol{\xi} = \mathrm{Log}(X_{\text{true}} \hat{X}^{-1})\)。
NEES 在李群上的计算公式为：
其中 \(\Sigma\) 是滤波器输出的**切空间协方差**，必须与误差 \(\boldsymbol{\xi}\) 的定义方式（左/右、排序）完全一致。
20.5 具体示例：2D 旋转估计¶
考虑一个简单场景：在 SO(2) 上估计旋转角 \(\theta\)，过程模型 \(\theta_{k+1} = \theta_k + \omega_k + w_k\)（\(w_k \sim \mathcal{N}(0, Q)\)），观测模型 \(z_k = \theta_k + v_k\)（\(v_k \sim \mathcal{N}(0, R)\)）。设 \(Q = 0.01\)，\(R = 0.1\)，运行 \(K = 50\) 步。
Monte Carlo NEES 检验步骤：
- 设 \(N = 100\) 次 Monte Carlo 运行。
- 每次生成独立噪声序列 \(\{w_k^{(i)}, v_k^{(i)}\}\)。
- 对每次运行，使用 EKF 估计 \(\hat{\theta}_k^{(i)}\) 和 \(P_k^{(i)}\)。
- 计算 NEES：\(\varepsilon_k^{(i)} = (\theta_k^{(i)} - \hat{\theta}_k^{(i)})^2 / P_k^{(i)}\)（SO(2) 维度 \(n=1\)）。
- 计算 \(\bar{\varepsilon}_k = \frac{1}{100}\sum_{i=1}^{100} \varepsilon_k^{(i)}\)。
- 95% 置信区间：\(\frac{1}{N}\chi^2_{0.025}(N) \le \bar{\varepsilon}_k \le \frac{1}{N}\chi^2_{0.975}(N)\)，即 \([0.737, 1.293]\)。
一致的 EKF 应让 \(\bar{\varepsilon}_k\) 大部分在此区间内。
20.6 Python 代码：NEES 计算核心¶
import numpy as np
from scipy.stats import chi2
def compute_nees(errors: np.ndarray, covariances: np.ndarray) -> np.ndarray:
"""
计算 NEES。
errors: shape (N, K, d)，N 次 MC 运行，K 个时刻，d 维切空间误差
covariances: shape (N, K, d, d)，对应协方差矩阵
返回: shape (K,)，每个时刻的平均 NEES
"""
N, K, d = errors.shape
nees = np.zeros((N, K))
for i in range(N):
for k in range(K):
xi = errors[i, k] # d 维切空间误差
P_inv = np.linalg.inv(covariances[i, k]) # 协方差的逆
nees[i, k] = xi @ P_inv @ xi # 二次型
avg_nees = nees.mean(axis=0) # 对 MC 维取平均
return avg_nees
def nees_bounds(N: int, d: int, alpha: float = 0.05):
"""计算双侧 (1-alpha) 置信区间"""
lower = chi2.ppf(alpha / 2, N * d) / N # 下界
upper = chi2.ppf(1 - alpha / 2, N * d) / N # 上界
return lower, upper
对 SO(3) 上的滤波器，errors 中每个条目应为 \(\mathrm{Log}(\hat{R}^{-1} R_{\text{true}}) \in \mathbb{R}^3\)，covariances 对应 \(3 \times 3\) 的切空间协方差。对 SE(3) 滤波器，维度为 6。
20.7 NEES 偏高/偏低的诊断¶
| NEES 偏向 | 物理含义 | 常见原因 | 修复策略 |
|---|---|---|---|
| 持续偏高 | 滤波器过于自信 | 过程噪声 \(Q\) 设太小、观测噪声 \(R\) 设太小、忽略系统性偏差 | 增大 \(Q\) 或 \(R\)；检查是否漏掉 bias 状态 |
| 持续偏低 | 滤波器过于保守 | \(Q\) 或 \(R\) 设太大 | 适当减小噪声参数 |
| 前期正常后期偏高 | 累积非线性效应 | 长时间传播中高斯假设失效、不可观方向被错误估计 | 分段预积分；使用 InEKF 或 EqF |
| 个别时刻突刺 | 外点或模型失配 | 数据关联错误、传感器故障 | 加鲁棒代价或外点剔除 |
20.8 NEES 在 InEKF 一致性验证中的角色¶
InEKF 的核心优势之一正是更好的一致性——即 NEES 更接近理论值 \(n\)。Barrau-Bonnabel TAC 2017 的理论预测是：在 group-affine 系统上，不变误差的线性化**不依赖估计状态**，因此即使状态估计偏了，Jacobian 也不会跟着偏。这消除了普通 EKF 的正反馈发散机制（§6.12.1）。
实验验证方式：对同一个 VIO/INS 系统，分别用普通 EKF 和 InEKF 跑 \(N=50\) 次 Monte Carlo，画出两者的平均 NEES 曲线。典型结果是：
- 普通 EKF 的 NEES 在长时间后逐渐偏高（过度自信）。
- InEKF 的 NEES 始终在置信带内（一致）。
这个对比是 InEKF 优势最直观的量化证据，也是 Brossard et al. T-RO 2021、van Goor-Mahony T-RO 2023 等论文的标准评估手段之一。
⚠️ 陷阱：NEES 计算时使用环境空间误差而非切空间误差
- 错误做法：在 SO(3) 估计中，计算误差为 \(e = \hat{R} - R_{\text{true}}\)（矩阵差），或 \(e = \hat{q} - q_{\text{true}}\)（四元数差），然后用该误差向量计算二次型。
- 后果：（1）矩阵差不在切空间中，协方差 \(\Sigma\) 的维度和含义与误差不匹配；（2）四元数差无物理意义（差不是旋转误差的正确度量）；（3）得到的 NEES 既不服从 \(\chi^2\) 分布，也无法与理论值 \(n\) 比较。
- 根本原因：NEES 的 \(\chi^2\) 性质依赖"误差向量服从高斯"这一前提。只有切空间误差 \(\mathrm{Log}(\hat{X}^{-1}X)\) 在 Concentrated Gaussian 假设下近似高斯，环境空间误差不满足此条件。
- 正确做法：始终使用 \(\boldsymbol{\xi} = \mathrm{Log}(\hat{X}^{-1}X)\)（或左摄动版本），确保与协方差的摄动约定一致。
⚠️ 陷阱：NEES 的 Monte Carlo 运行次数不足
- 错误做法：只跑 \(N=10\) 次 Monte Carlo 就判断一致性。
- 后果：\(N\) 太小时，\(N\bar{\varepsilon}_k \sim \chi^2(Nn)\) 的置信区间非常宽，几乎任何滤波器都能"通过"。例如 \(N=10\)，\(n=3\) 时 95% 区间为 \([0.490, 1.690]\)，宽度达 1.2，灵敏度很低。
- 正确做法：至少 \(N=50\)（维度 \(n \le 6\)），推荐 \(N=100\)。\(N=100\)，\(n=3\) 时置信区间收窄到 \([0.818, 1.196]\)，能有效区分一致和不一致的滤波器。
20.9 练习¶
-
（⭐⭐） 对 SO(2) 上的简单旋转估计问题（§20.5 的设置），实现完整的 Monte Carlo NEES 检验（\(N=100\)，\(K=50\)）。画出 \(\bar{\varepsilon}_k\) 随时间的曲线和 95% 置信带。若 EKF 一致，曲线应在带内。
-
（⭐⭐⭐） 将上述实验扩展到 SO(3)：模拟一个 IMU 陀螺仪积分 + 磁力计观测的系统。比较普通 EKF（欧拉角线性化）和李群 EKF（右摄动）的 NEES 曲线，讨论哪个更一致，以及在什么噪声水平下差异变得显著。
二十一、Banana 分布可视化与定量分析 ⭐⭐⭐¶
21.1 为什么 banana 形状"看得见"才算真正理解¶
§5.5 已经定性解释了 banana 分布：切空间中的高斯经过 Exp 映射后在群上变形。但定性理解远远不够——工程中需要回答三个定量问题：（1）不确定性多大时 banana 效应开始显著？（2）如何可视化验证自己的协方差传播是否正确？（3）Concentrated Gaussian 的适用边界在哪里？本节用 SO(3) 上的具体数值和 Python 代码回答这三个问题。
21.2 banana 形状的几何成因¶
考虑 SO(3) 上的 Concentrated Gaussian：\(R = \bar{R}\,\mathrm{Exp}(\boldsymbol{\xi})\)，\(\boldsymbol{\xi} \sim \mathcal{N}(\mathbf{0}, \sigma^2 I_3)\)。在切空间 \(\mathbb{R}^3\) 中，\(\boldsymbol{\xi}\) 是一个各向同性球形高斯。但 Exp 映射是非线性的——Rodrigues 公式 \(\mathrm{Exp}(\boldsymbol{\xi}) = I + \frac{\sin\theta}{\theta}[\boldsymbol{\xi}]_\times + \frac{1-\cos\theta}{\theta^2}[\boldsymbol{\xi}]_\times^2\)（其中 \(\theta=\|\boldsymbol{\xi}\|\)）在 \(\theta\) 较大时显著弯曲切空间。
跨领域类比：这与地图投影的变形本质相同。墨卡托投影在赤道附近（小偏差）几乎保面积，但在极地附近（大偏差）严重拉伸格陵兰的面积。Exp 映射在原点附近（小 \(\|\boldsymbol{\xi}\|\)）几乎等距，在远处（大 \(\|\boldsymbol{\xi}\|\)）引入系统性弯曲。Banana 形状就是"高纬度地区的格陵兰变形"在旋转群上的类比。
可以用 \(S^2\)（单位球面）来可视化 SO(3) 上的分布。取 \(\bar{R}=I\)，令参考向量 \(\mathbf{e}_3 = (0,0,1)^\top\)，则每个采样 \(R_i\) 对应球面上的一个点 \(\mathbf{p}_i = R_i \mathbf{e}_3\)。当 \(\sigma\) 很小时，\(\{\mathbf{p}_i\}\) 在北极附近形成近似圆形的点云；当 \(\sigma\) 增大，点云沿大圆方向拉伸，呈现明显的 banana 弯曲。
21.3 定量判据：何时 banana 效应不可忽略¶
Long, Wolfe, Mashner, Chirikjian（RSS 2012）给出的关键定量结论：
| 角度标准差 \(\sigma\) | Banana 效应 | 实际影响 |
|---|---|---|
| \(\sigma < 5°\)（~0.09 rad） | 可忽略 | 协方差椭圆是精确的好近似，EKF 完全足够 |
| \(5° < \sigma < 15°\)（~0.26 rad） | 轻微 | 一阶传播仍可用但开始偏差，四阶修正（Barfoot-Furgale 2014）有帮助 |
| \(\sigma > 15°\) | 显著 | 协方差椭圆严重失真，必须使用 UKF-M 或粒子方法 |
对 SE(2) 的差速轮机器人，banana 效应更直观：航向角噪声 \(\sigma_\theta\) 通过运动学耦合到 \((x,y)\) 位置。当直行距离 \(L\) 较大且 \(\sigma_\theta L \gg 1\) 时，位置分布的"香蕉"弯曲变得不可忽略。
反事实推理：如果不考虑 banana 效应会怎样？IMU 死推（dead reckoning）30 秒后，姿态标准差可能达到 \(5°\)-\(10°\)（取决于陀螺噪声密度），位置不确定性的实际分布已不是椭圆。此时若仍用 3-sigma 椭球做安全边界，会**低估**某些方向的风险（banana 的"外弯"侧不确定性更大）并**高估**另一些方向的风险。这正是为什么 Forster 预积分采用 keyframe 间隔 0.1-0.5 秒——控制每段内 \(\sigma < 5°\)。
21.4 Python 可视化：SO(3) 上的 banana 分布¶
以下代码生成 SO(3) 上的采样，投影到 \(S^2\) 可视化，并对比不同 \(\sigma\) 下的分布形状：
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
def sample_concentrated_gaussian_so3(mean_R, sigma, N=5000):
"""
在 SO(3) 上采样 Concentrated Gaussian。
mean_R: 均值旋转（scipy Rotation 对象）
sigma: 切空间各向同性标准差（rad）
N: 采样数
"""
# 在切空间中采样高斯向量
xi = np.random.randn(N, 3) * sigma # (N, 3)
# 通过 Exp 映射到 SO(3)
delta_R = Rotation.from_rotvec(xi) # 切空间 -> 群
# 复合：R = mean_R * Exp(xi)（右扰动）
samples = mean_R * delta_R
return samples
def project_to_sphere(rotations, ref_vec=np.array([0, 0, 1])):
"""将 SO(3) 采样投影到 S^2：p_i = R_i @ ref_vec"""
return rotations.apply(ref_vec) # (N, 3)
# === 对比三个不同的 sigma ===
fig = plt.figure(figsize=(18, 6))
mean_R = Rotation.identity()
sigmas = [np.deg2rad(3), np.deg2rad(10), np.deg2rad(25)]
titles = [r'$\sigma=3°$（球形高斯）', r'$\sigma=10°$（轻微 banana）',
r'$\sigma=25°$（显著 banana）']
for idx, (sig, title) in enumerate(zip(sigmas, titles)):
ax = fig.add_subplot(1, 3, idx + 1, projection='3d')
samples = sample_concentrated_gaussian_so3(mean_R, sig, N=5000)
pts = project_to_sphere(samples)
# 绘制单位球面（半透明）
u, v = np.mgrid[0:2*np.pi:40j, 0:np.pi:20j]
ax.plot_wireframe(np.cos(u)*np.sin(v), np.sin(u)*np.sin(v),
np.cos(v), alpha=0.08, color='gray')
# 绘制采样点
ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2], s=0.5, alpha=0.4)
ax.set_title(title)
ax.set_xlim([-1.1, 1.1]); ax.set_ylim([-1.1, 1.1])
ax.set_zlim([-1.1, 1.1])
plt.tight_layout()
plt.savefig('banana_so3_comparison.png', dpi=150)
plt.show()
运行结果中可以清晰看到：\(\sigma=3°\) 时点云是北极附近的小圆斑（高斯近似精确）；\(\sigma=10°\) 时开始沿大圆拉伸；\(\sigma=25°\) 时呈现明显的 banana 弧度，协方差椭圆已无法描述真实分布的形状。
21.5 Concentrated Gaussian 有效性的定量检验¶
实际工程中检验高斯假设是否成立的标准方法是计算切空间中样本分布与高斯分布的 KL 散度。具体步骤：
- 生成 \(N\) 个样本 \(R_i = \bar{R}\,\mathrm{Exp}(\boldsymbol{\xi}_i)\)
- 通过非线性传播（如 IMU 积分）得到 \(R'_i = f(R_i)\)
- 计算传播后的切空间误差 \(\boldsymbol{\eta}_i = \mathrm{Log}(\bar{R}'^{-1} R'_i)\)
- 拟合高斯 \(\hat{\Sigma} = \frac{1}{N}\sum \boldsymbol{\eta}_i \boldsymbol{\eta}_i^\top\)
- 检查 \(\{\boldsymbol{\eta}_i\}\) 相对于 \(\mathcal{N}(0, \hat{\Sigma})\) 的偏度（skewness）和峰度（kurtosis）
若三维偏度 \(>0.1\) 或超额峰度 \(|k-3| > 0.5\)，说明高斯假设已不充分。Forster（T-RO 2017）正是用 KL 散度验证了短段预积分（0.1-0.5 秒）内流形高斯与 Monte Carlo 真值高度吻合，而长段（>2 秒）偏差显著。
⚠️ 陷阱：用协方差椭圆的"形状"判断高斯性
- 错误做法：在二维投影中看协方差椭圆"覆盖了大部分点"就认为高斯假设成立。
- 后果：二维投影会掩盖三维空间中的 banana 弯曲。一个 banana 分布的二维投影可能"看起来像椭圆"。
- 根本原因：投影丢失了弯曲方向的信息。
- 正确做法：在切空间三维坐标中检查偏度/峰度，或直接用 KL 散度/NEES 做统计检验（见 §20）。
二十一A、实用协方差调参指南：从 Allan 方差到噪声矩阵 ⭐⭐⭐¶
21A.1 工程中最头疼的问题¶
前面各节给出了协方差传播的数学公式。但工程师面对的第一个问题往往不是"怎么传播"，而是"\(Q\) 矩阵里的数值到底填多少"。这个看似简单的问题，实际上需要把传感器数据手册上的噪声参数正确转换为滤波器需要的连续时间功率谱密度或离散时间协方差。转换过程中有多个易混淆的单位和约定，差一个 \(\sqrt{\Delta t}\) 就能让滤波器发散或过于保守。
21A.2 Allan 方差基础¶
Allan 方差（Allan Variance, AVAR）是表征惯性传感器随机误差的标准工具。它由 David W. Allan 于 1966 年提出，最初用于原子钟频率稳定性分析，后被 IEEE Std 952-1997 引入 IMU 噪声标定。
基本思想：将静态采集的 IMU 数据按不同时间窗口 \(\tau\) 分组，计算相邻组均值之差的方差。在双对数坐标（\(\log\tau\) vs \(\log\sigma_{Allan}\)）下，不同噪声源呈现不同斜率：
| 噪声类型 | 斜率 | 物理来源 | 对滤波器的影响 |
|---|---|---|---|
| 量化噪声（Quantization） | \(-1\) | ADC 分辨率 | 高频短时，通常可忽略 |
| 角度随机游走 ARW（\(N\)） | \(-1/2\) | 宽带白噪声 | 直接进入 \(Q_c\) 矩阵 |
| 零偏不稳定性 BI（\(B\)） | \(0\) | 电子学 1/f 噪声 | 确定 bias 估计的极限精度 |
| 速率随机游走 RRW（\(K\)） | \(+1/2\) | 温漂等慢变 | 进入 bias 随机游走模型 |
| 速率斜坡 | \(+1\) | 确定性温度漂移 | 需要温度补偿，不进入随机模型 |
读图方法：在 Allan 偏差曲线（\(\log\tau\) vs \(\log\sigma_{ADEV}\)）上——
- 斜率 \(-1/2\) 的直线在 \(\tau=1\,\text{s}\) 处的截距即为**噪声密度** \(N\)（单位：\(°/\sqrt{\text{h}}\) 或 \(°/\text{s}/\sqrt{\text{Hz}}\)）
- 曲线最低点的纵坐标值乘以 \(0.664\) 即为**零偏不稳定性** \(B\)（Brossard et al. T-RO 2021 采用的系数）
- 斜率 \(+1/2\) 的直线在 \(\tau=3\,\text{s}\) 处的截距即为**速率随机游走** \(K\)
21A.3 从数据手册到连续时间噪声矩阵¶
IMU 数据手册通常给出**噪声密度**（Noise Density）和**零偏不稳定性**（Bias Instability），需要转换为连续时间白噪声功率谱密度 \(Q_c\)。
陀螺仪：数据手册给出的角速度噪声密度 \(\sigma_g\)（单位：\(°/\text{s}/\sqrt{\text{Hz}}\)），对应连续时间白噪声模型 \(\dot{\theta} = \omega + w_g\)，\(w_g\) 的功率谱密度为：
注意单位转换：\(°/\text{s}/\sqrt{\text{Hz}} \to \text{rad}/\text{s}/\sqrt{\text{Hz}}\) 需乘以 \(\pi/180\)。
加速度计：数据手册给出的加速度噪声密度 \(\sigma_a\)（单位：\(\mu g/\sqrt{\text{Hz}}\) 或 \(\text{m}/\text{s}^2/\sqrt{\text{Hz}}\)），同理：
Bias 随机游走：bias 建模为 \(\dot{b} = w_b\)，\(w_b\) 的功率谱密度 \(Q_{c,\text{bias}}\) 可从 Allan 方差的 RRW 斜率提取。工程中常直接根据 bias 不稳定性经验设置。
21A.4 离散化：连续 \(Q_c\) 到离散 \(Q_d\)¶
滤波器在离散时间步 \(\Delta t\) 上运行，需要把连续噪声 \(Q_c\) 转换为离散噪声 \(Q_d\)。对零阶保持（ZOH）离散化：
这是最简单的一阶近似。对更精确的离散化（考虑 \(A\) 矩阵的影响），使用 Van Loan 方法：
其中 \(\Phi = \exp(A\Delta t)\) 是状态转移矩阵。
⚠️ 陷阱一（最常见）：混淆噪声密度和标准差
- 错误做法：把数据手册上的 \(0.01\,°/\text{s}/\sqrt{\text{Hz}}\)（噪声密度）直接当作离散时间标准差 \(\sigma = 0.01\,°/\text{s}\) 填入 \(Q_d\)。
- 后果：若采样率 \(f_s = 200\,\text{Hz}\)（\(\Delta t = 0.005\,\text{s}\)），正确的离散标准差应为 \(\sigma_d = 0.01 / \sqrt{0.005} \approx 0.141\,°/\text{s}\)。直接用噪声密度会**低估** 20 倍，导致滤波器严重过度自信、NEES 持续偏高。
- 根本原因：噪声密度是连续时间功率谱密度的平方根，离散化需要除以 \(\sqrt{\Delta t}\)（或等价地，\(Q_d = \sigma_{\text{density}}^2 / \Delta t\)）。两者差一个 \(\sqrt{\Delta t}\) 因子。
- 正确做法：\(Q_d = \sigma_{\text{density}}^2 \cdot \Delta t\)（功率谱密度 \(\times\) 步长），或等价地，离散标准差 \(\sigma_d = \sigma_{\text{density}} / \sqrt{\Delta t}\)。
21A.5 常见 IMU 噪声参数速查表¶
| IMU 型号 | 类别 | 陀螺噪声密度 | 陀螺零偏不稳定性 | 加速度计噪声密度 | 加速度计零偏不稳定性 | 典型应用 |
|---|---|---|---|---|---|---|
| MPU-6050 | 消费级 MEMS | \(0.005\,°/\text{s}/\sqrt{\text{Hz}}\) | \(5\,°/\text{h}\) | \(400\,\mu g/\sqrt{\text{Hz}}\) | \(\sim 60\,\mu g\) | 教学、DIY |
| BMI088 | 工业级 MEMS | \(0.014\,°/\text{s}/\sqrt{\text{Hz}}\) | \(2\,°/\text{h}\) | \(175\,\mu g/\sqrt{\text{Hz}}\) | \(\sim 30\,\mu g\) | 无人机（PX4） |
| ADIS16470 | 战术级 MEMS | \(0.008\,°/\text{s}/\sqrt{\text{Hz}}\) | \(8\,°/\text{h}\) | \(13\,\mu g/\sqrt{\text{Hz}}\) | \(\sim 13\,\mu g\) | 足式机器人 |
| STIM300 | 高端战术级 | \(0.0015\,°/\text{s}/\sqrt{\text{Hz}}\) | \(0.3\,°/\text{h}\) | \(50\,\mu g/\sqrt{\text{Hz}}\) | \(\sim 5\,\mu g\) | 高精度定位 |
本质洞察：噪声密度和零偏不稳定性描述的是两个完全不同的时间尺度。噪声密度（ARW）主导**短时间**误差积累（秒级），决定了 VIO keyframe 间隔内的姿态精度；零偏不稳定性主导**长时间**精度极限（分钟-小时级），决定了 GPS 拒止环境中 INS 能自主工作多久。滤波器中 \(Q_c\) 矩阵主要由 ARW 驱动，bias 状态的过程噪声 \(Q_b\) 由零偏不稳定性/RRW 驱动——两者不可混淆。
21A.6 实用调参工作流¶
完整的"数据手册 → 滤波器参数"工作流如下：
| 步骤 | 操作 | 产出 | 工具 |
|---|---|---|---|
| 1. 数据手册速查 | 找到噪声密度和零偏不稳定性 | \(\sigma_g\), \(\sigma_a\), \(B_g\), \(B_a\) 的典型值 | 厂商 PDF |
| 2. 静态数据采集 | IMU 静置 2-4 小时，恒温采集 | 原始角速度和加速度时间序列 | rosbag / 串口 |
| 3. Allan 方差计算 | 画出 \(\log\tau\)-\(\log\sigma_{ADEV}\) 曲线 | 实测 \(N\), \(B\), \(K\) | allantools（Python）或 kalibr_allan |
| 4. 对比数据手册 | 实测值与典型值对比 | 确认传感器状态正常 | 人工判断 |
| 5. 转换为 \(Q_c\) | \(Q_{c,g} = \sigma_g^2 I_3\)，\(Q_{c,a} = \sigma_a^2 I_3\) | 连续时间噪声矩阵 | 公式 |
| 6. 离散化 | \(Q_d = Q_c \cdot \Delta t\) | 离散时间噪声矩阵 | 公式或 Van Loan |
| 7. 在线调参 | 先用数据手册值，再根据 NEES 微调 | 最终 \(Q_d\), \(R\) | NEES 检验（§20） |
与 CD-KF 的联系：连续-离散卡尔曼滤波（CD-KF，见 滤波/10 新增的 CD-KF 章节）直接在连续时间 \(Q_c\) 上工作，避免了手动离散化的误差。对 IMU 高频积分场景（200-1000 Hz），CD-KF 的 Riccati ODE 积分比 ZOH 离散化更精确，尤其是当 \(A\) 矩阵在采样间隔内有显著变化时。使用 CD-KF 时，直接把 Allan 方差提取的 \(\sigma_g^2\) 填入 \(Q_c\) 的对角线即可，无需额外离散化步骤——这是 CD-KF 的主要工程便利性之一。
21A.7 练习¶
- （⭐⭐） 查找 BMI088 和 ADIS16470 的数据手册，分别提取陀螺噪声密度。若滤波器运行在 \(\Delta t = 5\,\text{ms}\)（200 Hz），计算两者的离散时间标准差 \(\sigma_d\)，对比哪个 IMU 的短时姿态精度更高。
- （⭐⭐⭐） 使用
allantoolsPython 包，对一段静态 IMU 数据计算 Allan 偏差曲线，提取 ARW 和零偏不稳定性。将提取的参数填入一个简单的 SO(3) EKF，通过 NEES 检验（§20）验证参数是否合理。 - （⭐⭐⭐） 解释为什么消费级 IMU（如 MPU-6050）的 VIO 系统通常需要更高的 keyframe 频率（10-20 Hz），而战术级 IMU（如 ADIS16470）可以用更低的 keyframe 频率（2-5 Hz）。从 Allan 方差参数和 banana 效应两个角度分析。
二十一B、SE(3) 协方差传播的完整 Monte Carlo 验证 ⭐⭐⭐¶
21B.1 为什么 SO(3) 验证不够¶
前面 §18.2 和附录中的 Monte Carlo 示例都在 SO(3) 上进行。但工程中最常见的协方差传播发生在 SE(3) 上——VIO 预积分、里程计累积、坐标变换链都涉及 \(6\times6\) 协方差矩阵。SO(3) 验证只覆盖了旋转部分，无法暴露平移-旋转耦合中的 bug。本节给出 SE(3) 上的完整 Monte Carlo 验证流程，包含代码、诊断指标和常见失败模式分析。
21B.2 SE(3) 验证的三个额外陷阱¶
相比 SO(3)，SE(3) 的 Monte Carlo 验证多出三个容易出错的环节：
| 环节 | SO(3) | SE(3) 额外困难 |
|---|---|---|
| Adjoint 矩阵 | \(3\times3\)，就是旋转矩阵 \(R\) | \(6\times6\)，含 \([t]_\times R\) 块，排序敏感 |
| 切向量排序 | 无歧义（\(\phi \in \mathbb{R}^3\)） | \([\rho, \phi]\) vs \([\phi, \rho]\)，直接影响 Adjoint 块结构 |
| 耦合效应 | 无 | 旋转不确定性通过 \([t]_\times\) 耦合到平移 |
第三点尤其微妙：即使两个位姿的旋转协方差完全正确，如果 Adjoint 中 \([t]_\times R\) 块的排序与切向量排序不匹配，平移部分的协方差会出现方向性错误——Monte Carlo 验证中表现为平移块的对角线正确但非对角线符号相反。
21B.3 完整验证代码¶
"""SE(3) 协方差传播 Monte Carlo 验证（平移在前排序 [rho, phi]）"""
import numpy as np
from scipy.spatial.transform import Rotation
def se3_exp(xi):
"""SE(3) 指数映射，xi = [rho(3), phi(3)]"""
rho, phi = xi[:3], xi[3:]
theta = np.linalg.norm(phi)
R = Rotation.from_rotvec(phi).as_matrix()
if theta < 1e-10:
V = np.eye(3) + 0.5 * hat(phi)
else:
n_hat = hat(phi / theta)
V = (np.eye(3) + ((1 - np.cos(theta)) / theta**2) * hat(phi)
+ ((theta - np.sin(theta)) / theta**3) * (hat(phi) @ hat(phi)))
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = V @ rho
return T
def se3_log(T):
"""SE(3) 对数映射，返回 [rho, phi]"""
R, t = T[:3, :3], T[:3, 3]
phi = Rotation.from_matrix(R).as_rotvec()
theta = np.linalg.norm(phi)
if theta < 1e-10:
V_inv = np.eye(3) - 0.5 * hat(phi)
else:
half = theta / 2
V_inv = (np.eye(3) - 0.5 * hat(phi)
+ (1/theta**2) * (1 - theta * np.cos(half) / (2 * np.sin(half)))
* (hat(phi) @ hat(phi)))
rho = V_inv @ t
return np.concatenate([rho, phi])
def hat(v):
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
def se3_adjoint(T):
"""6x6 Adjoint，平移在前排序"""
R, t = T[:3, :3], T[:3, 3]
Ad = np.zeros((6, 6))
Ad[:3, :3] = R # 左上
Ad[:3, 3:] = hat(t) @ R # 右上：平移-旋转耦合
Ad[3:, 3:] = R # 右下
return Ad
def se3_inv(T):
R, t = T[:3, :3], T[:3, 3]
T_inv = np.eye(4)
T_inv[:3, :3] = R.T
T_inv[:3, 3] = -R.T @ t
return T_inv
def mc_verify_se3_compounding(T1, Sigma1, T2, Sigma2, N=100000):
"""完整 Monte Carlo 验证 SE(3) 协方差复合"""
# 解析公式
Ad = se3_adjoint(se3_inv(T2))
Sigma_analytic = Ad @ Sigma1 @ Ad.T + Sigma2
# Monte Carlo 采样
T12_mean = T1 @ T2
xi12_samples = []
for _ in range(N):
xi1 = np.random.multivariate_normal(np.zeros(6), Sigma1)
xi2 = np.random.multivariate_normal(np.zeros(6), Sigma2)
T1_sample = T1 @ se3_exp(xi1)
T2_sample = T2 @ se3_exp(xi2)
T12_sample = T1_sample @ T2_sample
xi12 = se3_log(se3_inv(T12_mean) @ T12_sample)
xi12_samples.append(xi12)
Sigma_mc = np.cov(np.array(xi12_samples).T)
# 诊断输出
frob_err = np.linalg.norm(Sigma_analytic - Sigma_mc) / np.linalg.norm(Sigma_mc)
max_elem_err = np.max(np.abs(Sigma_analytic - Sigma_mc)) / np.max(np.abs(Sigma_mc))
print(f"Frobenius 相对误差: {frob_err:.4f}")
print(f"最大元素相对误差: {max_elem_err:.4f}")
print(f"解析协方差对称性: {np.linalg.norm(Sigma_analytic - Sigma_analytic.T):.2e}")
print(f"MC 协方差最小特征值: {np.min(np.linalg.eigvalsh(Sigma_mc)):.2e}")
return frob_err, Sigma_analytic, Sigma_mc
# 使用示例
T1 = se3_exp(np.array([0.5, 0.2, -0.1, 0.3, -0.2, 0.1]))
T2 = se3_exp(np.array([-0.3, 0.4, 0.1, 0.1, 0.2, -0.3]))
sig_rho, sig_phi = 0.03, 0.02
Sigma1 = np.diag([sig_rho**2]*3 + [sig_phi**2]*3)
Sigma2 = np.diag([sig_rho**2]*3 + [sig_phi**2]*3)
# frob_err, _, _ = mc_verify_se3_compounding(T1, Sigma1, T2, Sigma2)
21B.4 诊断失败模式¶
| Frobenius 误差范围 | 含义 | 下一步 |
|---|---|---|
| \(<0.01\) | 一阶公式高度准确 | 可直接用于工程 |
| \(0.01\)-\(0.05\) | 一阶近似开始偏差 | 考虑四阶修正或增加 keyframe 频率 |
| \(0.05\)-\(0.20\) | 非线性效应显著 | 检查 \(\Sigma\) 的大小是否超出 concentrated 范围 |
| \(>0.20\) | 公式可能有 bug 或协方差太大 | 逐块诊断：旋转块、平移块、耦合块分开检查 |
逐块诊断法：当整体误差偏大时，将 \(6\times6\) 协方差分为四个 \(3\times3\) 块——\(\Sigma_{\rho\rho}\)（平移-平移）、\(\Sigma_{\phi\phi}\)（旋转-旋转）、\(\Sigma_{\rho\phi}\)（耦合块）——分别计算各块的相对误差。如果旋转块正确但耦合块错误，几乎可以断定是 Adjoint 中 \([t]_\times R\) 的排序问题。
21B.5 与 GTSAM 的交叉验证¶
当自行实现的协方差传播通过了 Monte Carlo 验证后，还应与成熟库（如 GTSAM）做交叉验证。GTSAM 的 Pose3::AdjointMap() 方法返回 \(6\times6\) Adjoint 矩阵，但其切向量排序为 \([\phi, \rho]\)（旋转在前）。这意味着与本文（及 manif/Sophus）的 \([\rho, \phi]\) 排序恰好互换。交叉验证时必须用置换矩阵 \(P\) 做转换：
使得 \(\mathrm{Ad}_{\text{GTSAM}} = P \cdot \mathrm{Ad}_{\text{manif}} \cdot P\)，对应的协方差转换为 \(\Sigma_{\text{GTSAM}} = P \Sigma_{\text{manif}} P\)。这个 \(6\times6\) 置换矩阵是排序差异的完整解决方案——忘记它是 GTSAM 与 manif/Sophus 混用时最高频的工程 bug。
⚠️ 陷阱：GTSAM 与 manif 交叉验证时协方差数值"差不多"就认为正确
- 错误做法：打印两个库的协方差矩阵，发现旋转块和平移块的对角线元素接近，就认为一致。
- 后果：耦合块（\(\Sigma_{\rho\phi}\)）的位置互换了，但由于数值量级较小容易被忽略。在后续融合中，这个耦合块错误会导致平移估计的协方差方向偏转 90°。
- 正确做法：显式构造置换矩阵 \(P\)，用 \(\|P\Sigma_{\text{manif}}P - \Sigma_{\text{GTSAM}}\|_F < 10^{-10}\) 做精确验证。
二十二、故障排查表¶
| 现象 | 可能原因 | 检查方法 | 修复思路 |
|---|---|---|---|
| Allan 方差曲线无明显 \(-1/2\) 斜率 | 数据采集时间不够或有外部振动 | 延长采集至 4h 以上，隔振放置 | 重新采集，使用温箱恒温 |
| 采样旋转不正交 | 在矩阵元素上加高斯 | 检查 \(R^\top R-I\) | 在李代数采样后 Exp |
| 协方差传播方向错 | Adjoint 用了 \(\bar{X}\) 而非 \(\bar{X}^{-1}\) | 用 Monte Carlo 验证 compounding | 重推扰动搬运侧 |
| 平移协方差异常变大 | 旋转噪声通过 \([t]_\times\) 耦合 | 检查 Adjoint 块结构 | 确认排序和单位 |
| GTSAM 与 manif 结果不同 | 切向量排序不同 | 打印单位扰动效果 | 使用置换矩阵转换 |
| 长时间里程计 NEES 偏大 | 高斯近似失效 | 画样本散点和 log 坐标分布 | 分段预积分或改用 UKF/粒子 |
| 四元数均值跳变 | \(q\) 与 \(-q\) 双覆盖 | 检查符号是否统一 | 均值前统一半球或用 SO(3) log |
| 协方差不正定 | 线性化或数值误差 | 看最小特征值 | Joseph 形式、重线性化、对称化 |
练习：李群上的不确定性¶
- 证明右扰动与左扰动之间满足 \(\xi_L=\operatorname{Ad}_{\bar{X}}\xi_R\)。
- 从右扰动模型出发，完整推导 \(X_1X_2\) 的一阶协方差传播公式。
- 推导 \(X^{-1}\) 的右扰动协方差，并解释负号为什么在协方差中消失。
- 写一个 Monte Carlo 脚本验证 \(SE(2)\) 位姿复合公式。
- 说明为什么欧拉角均匀采样不是 \(SO(3)\) 均匀采样。
- 对一个 BetweenFactor，标明残差、噪声协方差、切向量排序和扰动方向。
- 比较 EKF on Lie group 与 UKF-M，在什么情况下你会选择后者？
- 分析 banana 分布：为什么笛卡尔坐标下非高斯，而局部指数坐标下更接近高斯？
- 查找一款 IMU 数据手册，提取噪声密度并正确转换为 \(Q_d\)（注意单位！），然后用 NEES 验证。
跨章综合题¶
考虑一个 VIO 预积分因子。
- 说明预积分量 \(\Delta R,\Delta v,\Delta p\) 中哪些部分属于李群，哪些属于向量空间。
- 解释噪声为什么先在 IMU 体坐标中定义。
- 使用专题 4 的 BCH 思想，说明如何把指数内部的旋转噪声搬到乘法右侧。
- 写出协方差递推 \(\Sigma_{k+1}=A_k\Sigma_kA_k^\top+B_kQ_kB_k^\top\) 的含义。
- 讨论如果预积分时间太长，小协方差假设为什么会变差。
二十三、与后续专题/批次的桥梁¶
本专题建立的"李群上的高斯"框架是后续多条研究线的数学地基：
- → 专题6（等变理论）： InEKF 是 Equivariant Filter 在李群 + group-affine 条件下的特例；本专题的 Concentrated Gaussian 与 Ad 变换是理解 equivariant error 的前提。
- → 第五批（SLAM后端）： Factor graph 中每个 BetweenFactor 的残差定义 \(r = \mathrm{Log}(\Delta T_{\mathrm{meas}}^{-1} T_i^{-1} T_j)\) 直接来自本专题；iSAM2 的增量更新依赖切空间协方差。
- → 第六批（RL数学）： Policy gradient 在流形参数空间上的 natural gradient 使用 Fisher 信息度量——本专题的信息几何内容是其先导。
- → 第八批（具身AI）： SE(3)-equivariant diffusion model 中的"加噪-去噪"过程需要在李群上定义扩散核——本专题 §19 的 Fokker-Planck 框架正是其数学基础，Peter-Weyl 展开给出精确热核，Concentrated Gaussian 给出短时近似。
二十三bis、从 EKF 到 InEKF：等变性如何改善滤波性能 ⭐⭐⭐¶
23bis.1 传统 EKF 在 SO(3) 上的问题 ⭐⭐⭐¶
传统 EKF 在旋转状态上的线性化误差与当前估计有关：
当估计误差大（如初始化不准）时，\(A\) 计算在错误的线性化点上，导致协方差传播和增益计算都有偏差。这是 EKF 在大初始误差下发散的根本原因。
23bis.2 InEKF 的核心思想 ⭐⭐⭐¶
Invariant EKF (Barrau & Bonnabel, 2017) 的关键洞察：
如果系统满足 group-affine 条件，则误差动力学可以写成与状态无关的形式：
其中 \(A_0\) 不依赖于当前估计 \(\hat X\)。
这意味着：即使估计误差很大，线性化矩阵仍然是正确的。
类比理解：普通 EKF 像是在摇晃的船上用望远镜看目标——船的晃动（估计误差）直接影响你的观测精度。InEKF 像是固定在岸上的望远镜——目标再怎么动，望远镜的测量框架是稳定的。
23bis.3 左不变/右不变误差 ⭐⭐⭐¶
符号约定：本节采用 Barrau-Bonnabel TAC 2017 原文约定（与本教程概率方向 A3/D 子专题一致）。注意 §5.7 中引用 Hartley IJRR 2020 时采用了 \(\eta=\hat{X}^{-1}X\) 的约定（互为逆元），两者通过 \(\eta_{\text{Hartley}}=(\eta^L_{\text{Barrau}})^{-1}\) 联系。
| 误差类型 | 定义（Barrau-Bonnabel TAC 2017） | 适用场景 |
|---|---|---|
| 右不变误差 | \(\eta^R = \hat X X^{-1}\) | 观测在 body frame（landmark/FK 经机体坐标） |
| 左不变误差 | \(\eta^L = X^{-1}\hat X\) | 观测在 world frame（GPS/绝对位置） |
选择哪种误差取决于系统的对称性结构。对大部分 VIO/INS 系统，右不变误差更自然（接触足端位置、IMU landmark 观测均为右不变型）。
23bis.4 InEKF 的实际优势 ⭐⭐⭐¶
| 性质 | 传统 EKF | InEKF |
|---|---|---|
| 线性化依赖 | 依赖当前估计 | 不依赖（group-affine 下） |
| 大初始误差 | 可能发散 | 仍然收敛 |
| 一致性 | 通常过于乐观 | 更接近真实 |
| 观测模型 | 任意 | 需要满足对称性 |
| 计算量 | 标准 | 略高（Adjoint 计算） |
限制：不是所有系统都满足 group-affine 条件。当观测模型不具有李群对称性时（如某些非线性传感器模型），InEKF 退化为普通 EKF。
本质洞察：InEKF 的优越性不是因为"公式更复杂"，而是因为它利用了系统的内在对称性来定义误差——这使得线性化的有效性不再依赖于估计精度。这是"对称性优先"思想在滤波领域的典范应用，也是专题6等变理论的直接预告。
23bis.5 与 ESKF/MEKF 的关系 ⭐⭐⭐¶
误差状态卡尔曼滤波（ESKF）和乘性 EKF（MEKF）是 InEKF 的"弱版本"：
- ESKF：在切空间定义误差，但线性化点仍依赖当前估计
- MEKF：类似 ESKF，常用于航天姿态估计
- InEKF：利用不变误差定义，使线性化与估计解耦
三者共享"状态在群上、误差在代数上"的框架（本专题核心原则），但 InEKF 额外利用了系统的群对称性来获得更好的一致性。
二十四、Concentrated Gaussian 的极限与修正 ⭐⭐⭐¶
24.1 归一化常数的精确形式 ⭐⭐⭐⭐¶
Concentrated Gaussian 的近似 PDF：
精确的归一化需要考虑指数映射的 Jacobian 行列式：
\(|\det J_r(\xi)|\) 是从切空间到群上的体积元修正。
对 SO(3)：
当 \(\theta \to 0\)，\(|\det J_r| \to 1\)（修正消失）。当 \(\theta \to \pi\)，\(|\det J_r| \to 4/\pi^2 \approx 0.405\)（修正显著）。
这就是"concentrated"假设的数学含义：当 \(\Sigma\) 很小时，\(\xi\) 主要集中在 \(\theta \ll 1\) 区域，\(|\det J_r| \approx 1\)，近似高度准确。
24.2 大不确定性下的替代方案 ⭐⭐⭐⭐¶
当协方差很大（如 GPS 拒止后长时间积分），Concentrated Gaussian 失效。替代方案：
| 方法 | 适用场景 | 核心思想 |
|---|---|---|
| Bingham 分布 | SO(3) 上的大不确定性 | 直接在 \(S^3\)（四元数）上定义 |
| von Mises-Fisher | 方向统计 | 球面高斯的推广 |
| 热核展开 | SO(3)/SE(3) | Peter-Weyl 级数精确解 |
| 粒子滤波 | 任意分布 | 非参数，计算量大 |
| UKF on Manifold (UKF-M) | 中等非线性 | Sigma 点在切空间采样后映回群 |
Bingham 分布特别值得关注：它在 \(S^3\) 上定义为 \(p(q) \propto \exp(q^\top M q)\)，其中 \(M\) 是 \(4\times4\) 对称矩阵。它自然处理了四元数的 \(q \sim -q\) 等价性，且不需要小扰动假设。
24.3 二阶协方差传播（Fourth-moment 修正） ⭐⭐⭐⭐¶
一阶传播公式 \(\Sigma_{12} \approx \operatorname{Ad}(\bar X_2^{-1})\Sigma_1\operatorname{Ad}(\bar X_2^{-1})^\top + \Sigma_2\) 忽略了 BCH 的二阶项 \(\frac{1}{2}[\xi_1, \xi_2]\)。
Bourmaud et al. (2015) 的二阶传播保留了交叉项：
（精确表达式涉及四阶矩）
实践中一阶已足够应对大部分 SLAM 场景。但在需要精确不确定性量化的场景（如安全关键系统的碰撞概率计算），二阶修正可能有价值。
二十五、本章知识树总结 ⭐¶
李群上的不确定性
├── 动机：SO(3)/SE(3) 不是向量空间，不能直接做高斯
├── Perturbation Model
│ ├── 左扰动 (world frame)
│ ├── 右扰动 (body frame)
│ └── Adjoint 互转
├── Concentrated Gaussian
│ ├── 定义：Log(X̄⁻¹X̃) ~ N(0,Σ)
│ ├── PDF 与归一化修正
│ └── 小协方差假设的含义
├── 协方差传播
│ ├── 复合 (Compounding)
│ ├── 逆 (Inverse)
│ └── Adjoint 搬运
├── banana 分布
│ ├── 大协方差下的非高斯效应
│ ├── 平移-旋转耦合
│ └── Monte Carlo 验证
├── 特殊分布
│ ├── Haar 测度（均匀分布）
│ ├── Bingham 分布
│ └── 热核与 Peter-Weyl
├── 滤波器应用
│ ├── ESKF / MEKF
│ ├── InEKF (left/right)
│ ├── UKF-M
│ └── 预积分噪声模型
├── IMU 噪声建模
│ ├── Allan 方差
│ ├── 连续到离散转换
│ └── NEES 验证
└── 桥接
├── → 等变理论（专题6）
├── → SLAM 后端（因子图）
└── → VIO 预积分
二十六、本章小结 ⭐¶
| 核心概念 | 一句话定义 | 工程对应 | 关键假设 |
|---|---|---|---|
| Concentrated Gaussian | 切空间中的高斯 + 指数映射回群 | VIO/SLAM 中的所有不确定位姿 | 小协方差 |
| 协方差复合 | 两个不确定位姿乘积的协方差 | 里程计累积不确定性 | 一阶线性化 |
| Adjoint 搬运 | 左/右协方差互转 | 多传感器融合的统一框架 | 无 |
| banana 分布 | 大协方差下的非高斯形变 | 长距离积分后的分布退化 | 小协方差假设失效 |
| Haar 测度 | 李群上的均匀分布 | 无先验信息时的初始化 | 紧致群 |
| NEES | 归一化估计误差平方 \(\xi^\top\Sigma^{-1}\xi\) | 滤波器一致性检验 | 高斯假设 |
| Allan 方差 | IMU 噪声参数的频域提取方法 | \(Q_c\) 标定 | 静态/恒温采集 |
二十七、累积项目：本章新增模块 ⭐¶
项目方向：手写几何验证库
本章新增：Concentrated Gaussian 采样与协方差传播
import numpy as np
def sample_concentrated_gaussian_SO3(R_mean, Sigma, n_samples=1000):
"""在 SO(3) 上采样 Concentrated Gaussian"""
samples = []
for _ in range(n_samples):
# 在切空间（R^3）中采样高斯
xi = np.random.multivariate_normal(np.zeros(3), Sigma)
# 通过指数映射回到 SO(3)
R_sample = R_mean @ SO3.exp(xi)
samples.append(R_sample)
return samples
def compounding_covariance(Ad_X2_inv, Sigma1, Sigma2):
"""一阶协方差复合公式"""
return Ad_X2_inv @ Sigma1 @ Ad_X2_inv.T + Sigma2
# 用 Monte Carlo 验证一阶公式
# (省略完整实现，原理：生成大量样本，计算经验协方差，与解析公式对比)
延伸阅读 ⭐¶
| 资源 | 难度 | 核心价值 |
|---|---|---|
| Barfoot "State Estimation for Robotics" 2nd ed. Ch.7-8 | ⭐⭐⭐ | SE(3) 不确定性的标准参考 |
| Chirikjian "Stochastic Models on Lie Groups" Vol.2 | ⭐⭐⭐⭐ | 最深入的李群概率论 |
| Bourmaud et al. "Discrete Extended Kalman Filter on Lie Groups" (EUSIPCO 2013) | ⭐⭐⭐ | EKF on Lie group 的标准推导 |
| Barrau & Bonnabel "Invariant Kalman Filtering" (Annual Reviews 2018) | ⭐⭐⭐⭐ | InEKF 理论的综述 |
| Forster et al. "On-Manifold Preintegration" (T-RO 2017) | ⭐⭐⭐ | 预积分噪声模型 |
| Brossard, Barrau, Bonnabel "A Code for Unscented KF on Manifolds" (ICRA 2020) | ⭐⭐⭐ | UKF-M Python 实现 |
| Mardia & Jupp "Directional Statistics" (2000) | ⭐⭐⭐⭐ | 方向统计经典教材 |
| Sola "A micro Lie theory" (2018) §VI | ⭐⭐ | 不确定性传播的速查表 |
🔧 故障排查手册 ⭐¶
| 症状 | 可能原因 | 排查步骤 | 相关节 |
|---|---|---|---|
| 旋转采样不在 SO(3) 上 | 在矩阵元素上加噪声 | 1.检查 \(R^\top R - I\) 2.改为切空间采样后 Exp | §5.2 |
| 协方差传播方向错 | Adjoint 用了 \(\bar X\) 而非 \(\bar X^{-1}\) | 1.用 Monte Carlo 验证 2.重推搬运公式 | §5.4 |
| NEES 系统性偏大 | 协方差估计过小或非线性 | 1.画 NEES 直方图 2.增大过程噪声 3.缩短线性化间隔 | §20 |
| 长距离后位姿分布呈 banana | 累积旋转不确定性耦合平移 | 1.画 Monte Carlo 散点 2.分段处理 3.考虑 UKF-M | §5.6 |
| Allan 方差无明显斜率 | 采集条件不佳 | 1.延长至 4h 2.隔振恒温 3.检查数据有无跳变 | §21A |
| GTSAM 因子图结果与手推不一致 | 切向量排序或左右扰动不同 | 1.打印单位扰动效果 2.对照 GTSAM convention | §5.2 |
总结¶
本专题的核心信息可以压缩为一个原则：均值在群上，不确定性在代数上。 Concentrated Gaussian 通过 \(X = \bar{X}\cdot\mathrm{Exp}(\boldsymbol{\xi})\) 将这一原则变为可操作的数学工具；Ad 变换负责帧间搬运；BCH 公式负责协方差复合；banana 分布提醒你近似的边界在哪里；Fokker-Planck 方程（§19）揭示了 Concentrated Gaussian 的连续时间根源——它是李群热方程的短时近似；NEES 检验（§20）提供了验证上述所有工具是否正确工作的统计手段。掌握这套工具链后，Barfoot 的 factor graph 后端、Forster 的预积分、Barrau-Bonnabel 的 InEKF 都只是同一数学框架的不同工程实例化——这正是本专题作为"枢纽节点"的价值所在。