机器人状态估计与 SLAM—概率推理到 simultaneous Localization and Mapping

第一部分：状态估计基础理论
- 第一章：绪论------机器人为什么需要状态估计
- 第二章：概率论与贝叶斯推理基础
- 第三章：递归贝叶斯估计------滤波理论
第二部分：卡尔曼滤波族
- 第四章：线性卡尔曼滤波
- 第五章：扩展卡尔曼滤波（EKF）
- 第六章：无迹卡尔曼滤波（UKF）与粒子滤波
第三部分：SLAM 问题的形式化
- 第七章：SLAM 问题的数学定义
- 第八章：EKF-SLAM
- 第九章：基于图的 SLAM 与因子图优化
第四部分：视觉 SLAM
- 第十章：视觉里程计------特征点法
- 第十一章：视觉里程计------直接法
- 第十二章：回环检测与全局优化
- 第十三章：深度学习与 SLAM
第五部分：完整可运行代码实现
- 第十四章：从零实现卡尔曼滤波
- 第十五章：从零实现 EKF-SLAM
- 第十六章：从零实现简单的视觉里程计
- 第十七章：完整 SLAM Pipeline 与精度对比
附录

第一部分：状态估计基础理论

第一章：绪论------机器人为什么需要状态估计

1.1 机器人状态估计的核心问题

1.1.1 什么是状态？

定义 1.1（机器人状态） ：机器人在时刻 ttt 的状态 xt\mathbf{x}_txt 是描述其物理配置的最小变量集合。

常见状态变量：

机器人类型	状态变量	维度
移动机器人（2D）	$x,y,θ$ T $x, y, \\theta$ ^T $x,y,θ$ T	3
移动机器人（3D）	$x,y,z,ϕ,θ,ψ$ T $x, y, z, \\phi, \\theta, \\psi$ ^T $x,y,z,ϕ,θ,ψ$ T	6
无人机	$x,y,z,ϕ,θ,ψ,vx,vy,vz$ T $x, y, z, \\phi, \\theta, \\psi, v_x, v_y, v_z$ ^T $x,y,z,ϕ,θ,ψ,vx,vy,vz$ T	9
机械臂	$q1,q2,...,qn$ T $q_1, q_2, \\dots, q_n$ ^T $q1,q2,...,qn$ T	nnn
自动驾驶车辆	$x,y,z,ϕ,θ,ψ,v,ω$ T $x, y, z, \\phi, \\theta, \\psi, v, \\omega$ ^T $x,y,z,ϕ,θ,ψ,v,ω$ T	8+

1.1.2 为什么状态估计是困难的？

挑战 1：传感器噪声

所有传感器都有噪声------测量值不等于真实值。

zt=h(xt)+vt\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_tzt=h(xt)+vt

其中 vt\mathbf{v}_tvt 是测量噪声。

挑战 2：模型不确定性

机器人的运动模型也不完美------预测值不等于真实值。

xt=f(xt−1,ut)+wt\mathbf{x}t = f(\mathbf{x}{t-1}, \mathbf{u}_t) + \mathbf{w}_txt=f(xt−1,ut)+wt

其中 wt\mathbf{w}_twt 是过程噪声，ut\mathbf{u}_tut 是控制输入。

挑战 3：数据关联

在 SLAM 中，需要判断当前观测是否对应于之前见过的地标------这本身就是一个难题。

挑战 4：计算复杂度

随着地图规模增大，状态维度增长，计算成本急剧上升。

1.2 传感器与测量模型

1.2.1 常见传感器

传感器	测量内容	噪声特性
编码器	里程计（位置增量）	漂移（累积误差）
IMU	加速度、角速度	偏置、漂移
激光雷达	距离、角度	高斯噪声
相机	图像（像素）	光照变化、遮挡
GPS	全局位置	多径效应、精度有限
超声波	距离	角度分辨率低

1.2.2 测量模型

定义 1.2（测量模型）：测量模型描述了传感器读数与机器人状态之间的关系：

zt=h(xt,m)+vt\mathbf{z}_t = h(\mathbf{x}_t, \mathbf{m}) + \mathbf{v}_tzt=h(xt,m)+vt

其中 m\mathbf{m}m 是地图特征的位置。

例子：激光雷达测量

zt(i)=(xt−mix)2+(yt−miy)2+vt(i)z_t^{(i)} = \sqrt{(x_t - m_i^x)^2 + (y_t - m_i^y)^2} + v_t^{(i)}zt(i)=(xt−mix)2+(yt−miy)2 +vt(i)

其中 (mix,miy)(m_i^x, m_i^y)(mix,miy) 是第 iii 个地标的位置。

1.3 运动模型

1.3.1 离散时间运动模型

定义 1.3（运动模型）：运动模型描述了机器人状态随时间的演化：

xt=f(xt−1,ut)+wt\mathbf{x}t = f(\mathbf{x}{t-1}, \mathbf{u}_t) + \mathbf{w}_txt=f(xt−1,ut)+wt

例子：差速驱动机器人

(xtytθt)=(xt−1+Δscos⁡(θt−1+Δθ/2)yt−1+Δssin⁡(θt−1+Δθ/2)θt−1+Δθ)+wt\begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1} + \Delta s \cos(\theta_{t-1} + \Delta\theta/2) \\ y_{t-1} + \Delta s \sin(\theta_{t-1} + \Delta\theta/2) \\ \theta_{t-1} + \Delta\theta \end{pmatrix} + \mathbf{w}_t xtytθt = xt−1+Δscos(θt−1+Δθ/2)yt−1+Δssin(θt−1+Δθ/2)θt−1+Δθ +wt

其中 Δs\Delta sΔs 是位移，Δθ\Delta\thetaΔθ 是角度变化。

第二章：概率论与贝叶斯推理基础

2.1 概率论基础

2.1.1 概率分布

定义 2.1（概率密度函数） ：连续随机变量 XXX 的概率密度函数 p(x)p(x)p(x) 满足：

p(x)≥0,∫−∞∞p(x)dx=1p(x) \geq 0, \quad \int_{-\infty}^{\infty} p(x) dx = 1p(x)≥0,∫−∞∞p(x)dx=1

定义 2.2（联合分布） ：两个随机变量 X,YX, YX,Y 的联合分布为 p(x,y)p(x, y)p(x,y)。

定义 2.3（边缘分布）：

p(x)=∫p(x,y)dyp(x) = \int p(x, y) dyp(x)=∫p(x,y)dy

定义 2.4（条件分布）：

p(x∣y)=p(x,y)p(y)p(x | y) = \frac{p(x, y)}{p(y)}p(x∣y)=p(y)p(x,y)

2.1.2 贝叶斯定理

定理 2.1（贝叶斯定理）：

p(x∣z)=p(z∣x)p(x)p(z)p(x | z) = \frac{p(z | x) p(x)}{p(z)}p(x∣z)=p(z)p(z∣x)p(x)

其中：

p(x)p(x)p(x)：先验（prior） ------观测前对 xxx 的信念
p(z∣x)p(z | x)p(z∣x)：似然（likelihood） ------给定 xxx 时观测 zzz 的概率
p(x∣z)p(x | z)p(x∣z)：后验（posterior） ------观测 zzz 后对 xxx 的信念
p(z)p(z)p(z)：证据（evidence）------归一化常数

物理含义：贝叶斯定理告诉我们如何用新的观测来更新我们的信念。

2.1.3 高斯分布

定义 2.5（多元高斯分布）：

N(x;μ,Σ)=1(2π)d/2∣Σ∣1/2exp⁡(−12(x−μ)TΣ−1(x−μ))\mathcal{N}(\mathbf{x}; \boldsymbol{\mu}, \boldsymbol{\Sigma}) = \frac{1}{(2\pi)^{d/2} |\boldsymbol{\Sigma}|^{1/2}} \exp\left(-\frac{1}{2}(\mathbf{x} - \boldsymbol{\mu})^T \boldsymbol{\Sigma}^{-1} (\mathbf{x} - \boldsymbol{\mu})\right)N(x;μ,Σ)=(2π)d/2∣Σ∣1/21exp(−21(x−μ)TΣ−1(x−μ))

性质：

边缘分布仍是高斯
条件分布仍是高斯
线性变换后仍是高斯

定理 2.2（高斯分布的条件分布） ：设 x= $x1,x2$ T\mathbf{x} = $\\mathbf{x}_1, \\mathbf{x}_2$ ^Tx= $x1,x2$ T 服从联合高斯分布：

(x1x2)∼N((μ1μ2),(Σ11Σ12Σ21Σ22))\begin{pmatrix} \mathbf{x}1 \\ \mathbf{x}2 \end{pmatrix} \sim \mathcal{N}\left(\begin{pmatrix} \boldsymbol{\mu}1 \\ \boldsymbol{\mu}2 \end{pmatrix}, \begin{pmatrix} \boldsymbol{\Sigma}{11} & \boldsymbol{\Sigma}{12} \\ \boldsymbol{\Sigma}{21} & \boldsymbol{\Sigma}{22} \end{pmatrix}\right)(x1x2)∼N((μ1μ2),(Σ11Σ21Σ12Σ22))

则条件分布为：

p(x1∣x2)=N(μ1∣2,Σ1∣2)p(\mathbf{x}1 | \mathbf{x}2) = \mathcal{N}(\boldsymbol{\mu}{1|2}, \boldsymbol{\Sigma}{1|2})p(x1∣x2)=N(μ1∣2,Σ1∣2)

其中：

μ1∣2=μ1+Σ12Σ22−1(x2−μ2)\boldsymbol{\mu}{1|2} = \boldsymbol{\mu}1 + \boldsymbol{\Sigma}{12} \boldsymbol{\Sigma}{22}^{-1} (\mathbf{x}_2 - \boldsymbol{\mu}_2)μ1∣2=μ1+Σ12Σ22−1(x2−μ2)

Σ1∣2=Σ11−Σ12Σ22−1Σ21\boldsymbol{\Sigma}{1|2} = \boldsymbol{\Sigma}{11} - \boldsymbol{\Sigma}{12} \boldsymbol{\Sigma}{22}^{-1} \boldsymbol{\Sigma}_{21}Σ1∣2=Σ11−Σ12Σ22−1Σ21

这个公式是卡尔曼滤波的核心。

2.2 贝叶斯推理在机器人中的应用

2.2.1 定位问题

问题：给定传感器测量 z1:t\mathbf{z}{1:t}z1:t 和控制输入 u1:t\mathbf{u}{1:t}u1:t，估计机器人状态 xt\mathbf{x}_txt。

贝叶斯解：

p(xt∣z1:t,u1:t)=p(zt∣xt)p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1)p(\mathbf{x}t | \mathbf{z}{1:t}, \mathbf{u}_{1:t}) = \frac{p(\mathbf{z}_t | \mathbf{x}t) p(\mathbf{x}t | \mathbf{z}{1:t-1}, \mathbf{u}{1:t})}{p(\mathbf{z}t | \mathbf{z}{1:t-1})}p(xt∣z1:t,u1:t)=p(zt∣z1:t−1)p(zt∣xt)p(xt∣z1:t−1,u1:t)

2.2.2 SLAM 问题

问题：同时估计机器人状态 x1:t\mathbf{x}_{1:t}x1:t 和地图 m\mathbf{m}m。

贝叶斯解：

p(x1:t,m∣z1:t,u1:t)p(\mathbf{x}{1:t}, \mathbf{m} | \mathbf{z}{1:t}, \mathbf{u}_{1:t})p(x1:t,m∣z1:t,u1:t)

这是一个高维概率分布------直接求解不可行。

第三章：递归贝叶斯估计------滤波理论

3.1 贝叶斯滤波

3.1.1 马尔可夫假设

假设 3.1（马尔可夫性）：当前状态只依赖于上一时刻的状态和当前的控制输入：

p(xt∣x1:t−1,u1:t)=p(xt∣xt−1,ut)p(\mathbf{x}t | \mathbf{x}{1:t-1}, \mathbf{u}_{1:t}) = p(\mathbf{x}t | \mathbf{x}{t-1}, \mathbf{u}_t)p(xt∣x1:t−1,u1:t)=p(xt∣xt−1,ut)

3.1.2 贝叶斯滤波的递推公式

算法 3.1（贝叶斯滤波）：

预测步（Prediction）：

pˉ(xt)=∫p(xt∣xt−1,ut)p(xt−1)dxt−1\bar{p}(\mathbf{x}t) = \int p(\mathbf{x}t | \mathbf{x}{t-1}, \mathbf{u}t) p(\mathbf{x}{t-1}) d\mathbf{x}{t-1}pˉ(xt)=∫p(xt∣xt−1,ut)p(xt−1)dxt−1

更新步（Update）：

p(xt)=η⋅p(zt∣xt)⋅pˉ(xt)p(\mathbf{x}_t) = \eta \cdot p(\mathbf{z}_t | \mathbf{x}_t) \cdot \bar{p}(\mathbf{x}_t)p(xt)=η⋅p(zt∣xt)⋅pˉ(xt)

其中 η=1/p(zt∣z1:t−1)\eta = 1/p(\mathbf{z}t | \mathbf{z}{1:t-1})η=1/p(zt∣z1:t−1) 是归一化常数。

3.1.3 贝叶斯滤波的物理含义

预测步：根据运动模型，将上一时刻的信念向前传播------"我预计我现在在哪里"。

更新步：用新的测量来修正预测------"看到这个测量后，我更确定我在哪里"。

直觉：贝叶斯滤波是一个"预测-更新"的循环，不断融合运动信息和观测信息。

3.2 最大后验估计与最小均方误差估计

3.2.1 最大后验估计（MAP）

定义 3.1（MAP 估计）：

x^tMAP=arg⁡max⁡xtp(xt∣z1:t)\hat{\mathbf{x}}t^{\text{MAP}} = \arg\max{\mathbf{x}_t} p(\mathbf{x}t | \mathbf{z}{1:t})x^tMAP=argxtmaxp(xt∣z1:t)

等价于：

x^tMAP=arg⁡max⁡xt $p(zt∣xt)pˉ(xt)$ \hat{\mathbf{x}}t^{\text{MAP}} = \arg\max{\mathbf{x}_t} $p(\\mathbf{z}_t \| \\mathbf{x}_t) \\bar{p}(\\mathbf{x}_t)$ x^tMAP=argxtmax $p(zt∣xt)pˉ(xt)$

3.2.2 最小均方误差估计（MMSE）

定义 3.2（MMSE 估计）：

x^tMMSE=E $xt∣z1:t$ =∫xtp(xt∣z1:t)dxt\hat{\mathbf{x}}_t^{\text{MMSE}} = \mathbb{E} $\\mathbf{x}_t \| \\mathbf{z}_{1:t}$ = \int \mathbf{x}_t p(\mathbf{x}t | \mathbf{z}{1:t}) d\mathbf{x}_tx^tMMSE=E $xt∣z1:t$ =∫xtp(xt∣z1:t)dxt

定理 3.1（MAP vs MMSE）：对于高斯后验分布，MAP 和 MMSE 估计相同。对于非高斯分布，它们可能不同。

3.2.3 协方差矩阵的传播

定理 3.2（线性变换下的协方差传播） ：设 y=Ax+b\mathbf{y} = A\mathbf{x} + \mathbf{b}y=Ax+b，则：

μy=Aμx+b\boldsymbol{\mu}_y = A\boldsymbol{\mu}_x + \mathbf{b}μy=Aμx+b

Σy=AΣxAT\boldsymbol{\Sigma}_y = A\boldsymbol{\Sigma}_x A^TΣy=AΣxAT

第二部分：卡尔曼滤波族

第四章：线性卡尔曼滤波

4.1 线性系统模型

4.1.1 状态空间模型

运动模型：

xt=Ftxt−1+Btut+wt\mathbf{x}t = F_t \mathbf{x}{t-1} + B_t \mathbf{u}_t + \mathbf{w}_txt=Ftxt−1+Btut+wt

测量模型：

zt=Htxt+vt\mathbf{z}_t = H_t \mathbf{x}_t + \mathbf{v}_tzt=Htxt+vt

其中：

FtF_tFt：状态转移矩阵
BtB_tBt：控制输入矩阵
HtH_tHt：观测矩阵
wt∼N(0,Qt)\mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)wt∼N(0,Qt)：过程噪声
vt∼N(0,Rt)\mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, R_t)vt∼N(0,Rt)：测量噪声

4.1.2 卡尔曼滤波的假设

假设 4.1：

运动模型和测量模型都是线性的
噪声是高斯白噪声
初始状态服从高斯分布

定理 4.1（卡尔曼滤波的最优性） ：在线性高斯系统中，卡尔曼滤波是最优的贝叶斯估计器------它给出的后验分布是精确的。

4.2 卡尔曼滤波算法

4.2.1 预测步

先验均值：

x^t∣t−1=Ftx^t−1∣t−1+Btut\hat{\mathbf{x}}{t|t-1} = F_t \hat{\mathbf{x}}{t-1|t-1} + B_t \mathbf{u}_tx^t∣t−1=Ftx^t−1∣t−1+Btut

先验协方差：

Pt∣t−1=FtPt−1∣t−1FtT+QtP_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_tPt∣t−1=FtPt−1∣t−1FtT+Qt

4.2.2 更新步

卡尔曼增益：

Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1K_t = P_{t|t-1} H_t^T (H_t P_{t|t-1} H_t^T + R_t)^{-1}Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1

后验均值：

x^t∣t=x^t∣t−1+Kt(zt−Htx^t∣t−1)\hat{\mathbf{x}}{t|t} = \hat{\mathbf{x}}{t|t-1} + K_t (\mathbf{z}t - H_t \hat{\mathbf{x}}{t|t-1})x^t∣t=x^t∣t−1+Kt(zt−Htx^t∣t−1)

后验协方差：

Pt∣t=(I−KtHt)Pt∣t−1P_{t|t} = (I - K_t H_t) P_{t|t-1}Pt∣t=(I−KtHt)Pt∣t−1

4.2.3 卡尔曼增益的直觉

定理 4.2（卡尔曼增益的含义） ：卡尔曼增益 KtK_tKt 是预测不确定性 和测量不确定性之间的权衡。

推论 4.1：

当测量噪声 RtR_tRt 很大时，Kt→0K_t \to 0Kt→0------更信任预测
当预测不确定性 Pt∣t−1P_{t|t-1}Pt∣t−1 很大时，Kt→Ht−1K_t \to H_t^{-1}Kt→Ht−1------更信任测量

证明：设标量情况，H=1H = 1H=1，Pt∣t−1=σp2P_{t|t-1} = \sigma_p^2Pt∣t−1=σp2，R=σm2R = \sigma_m^2R=σm2。

K=σp2σp2+σm2K = \frac{\sigma_p^2}{\sigma_p^2 + \sigma_m^2}K=σp2+σm2σp2

当 σm2→∞\sigma_m^2 \to \inftyσm2→∞，K→0K \to 0K→0；当 σp2→∞\sigma_p^2 \to \inftyσp2→∞，K→1K \to 1K→1。□\square□

4.3 卡尔曼滤波的信息论解释

4.3.1 信息形式

定义 4.1（信息向量和信息矩阵）：

Ωt=Pt∣t−1,ωt=Pt∣t−1x^t∣t\Omega_t = P_{t|t}^{-1}, \quad \boldsymbol{\omega}t = P{t|t}^{-1} \hat{\mathbf{x}}_{t|t}Ωt=Pt∣t−1,ωt=Pt∣t−1x^t∣t

信息形式的卡尔曼滤波：

预测步：

Ωˉt=(FtPt−1∣t−1FtT+Qt)−1\bar{\Omega}t = (F_t P{t-1|t-1} F_t^T + Q_t)^{-1}Ωˉt=(FtPt−1∣t−1FtT+Qt)−1

ωˉt=Ωˉt(Ftx^t−1∣t−1+Btut)\bar{\boldsymbol{\omega}}_t = \bar{\Omega}t (F_t \hat{\mathbf{x}}{t-1|t-1} + B_t \mathbf{u}_t)ωˉt=Ωˉt(Ftx^t−1∣t−1+Btut)

更新步：

Ωt=Ωˉt+HtTRt−1Ht\Omega_t = \bar{\Omega}_t + H_t^T R_t^{-1} H_tΩt=Ωˉt+HtTRt−1Ht

ωt=ωˉt+HtTRt−1zt\boldsymbol{\omega}_t = \bar{\boldsymbol{\omega}}_t + H_t^T R_t^{-1} \mathbf{z}_tωt=ωˉt+HtTRt−1zt

4.3.2 信息增益

定理 4.3（测量的信息增益）：一次测量带来的信息增益为：

ΔI=12log⁡∣Pt∣t−1∣∣Pt∣t∣=12log⁡∣I+KtHt∣\Delta I = \frac{1}{2} \log \frac{|P_{t|t-1}|}{|P_{t|t}|} = \frac{1}{2} \log |I + K_t H_t|ΔI=21log∣Pt∣t∣∣Pt∣t−1∣=21log∣I+KtHt∣

第五章：扩展卡尔曼滤波（EKF）

5.1 非线性系统

5.1.1 非线性模型

运动模型：

xt=f(xt−1,ut)+wt\mathbf{x}t = f(\mathbf{x}{t-1}, \mathbf{u}_t) + \mathbf{w}_txt=f(xt−1,ut)+wt

测量模型：

zt=h(xt)+vt\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_tzt=h(xt)+vt

其中 fff 和 hhh 是非线性函数。

5.1.2 EKF 的核心思想

思想：在当前估计点处对非线性函数进行一阶泰勒展开，用线性近似代替非线性。

定义 5.1（雅可比矩阵）：

Ft=∂f∂x∣x^t−1∣t−1,utF_t = \frac{\partial f}{\partial \mathbf{x}}\Bigg|{\hat{\mathbf{x}}{t-1|t-1}, \mathbf{u}_t}Ft=∂x∂f x^t−1∣t−1,ut

Ht=∂h∂x∣x^t∣t−1H_t = \frac{\partial h}{\partial \mathbf{x}}\Bigg|{\hat{\mathbf{x}}{t|t-1}}Ht=∂x∂h x^t∣t−1

5.2 EKF 算法

5.2.1 预测步

先验均值：

x^t∣t−1=f(x^t−1∣t−1,ut)\hat{\mathbf{x}}{t|t-1} = f(\hat{\mathbf{x}}{t-1|t-1}, \mathbf{u}_t)x^t∣t−1=f(x^t−1∣t−1,ut)

先验协方差：

Pt∣t−1=FtPt−1∣t−1FtT+QtP_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_tPt∣t−1=FtPt−1∣t−1FtT+Qt

5.2.2 更新步

卡尔曼增益：

Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1K_t = P_{t|t-1} H_t^T (H_t P_{t|t-1} H_t^T + R_t)^{-1}Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1

后验均值：

x^t∣t=x^t∣t−1+Kt(zt−h(x^t∣t−1))\hat{\mathbf{x}}{t|t} = \hat{\mathbf{x}}{t|t-1} + K_t (\mathbf{z}t - h(\hat{\mathbf{x}}{t|t-1}))x^t∣t=x^t∣t−1+Kt(zt−h(x^t∣t−1))

后验协方差：

Pt∣t=(I−KtHt)Pt∣t−1P_{t|t} = (I - K_t H_t) P_{t|t-1}Pt∣t=(I−KtHt)Pt∣t−1

5.2.3 EKF 的局限

定理 5.1（EKF 的次优性）：EKF 使用一阶近似，忽略了高阶项。当非线性很强时，EKF 可能发散。

问题来源：

雅可比矩阵的计算可能复杂或不存在
一阶近似可能不准确
协方差的传播可能不准确

5.3 迭代扩展卡尔曼滤波（IEKF）

5.3.1 核心思想

IEKF：在更新步中，多次迭代线性化点，使线性化点更接近后验均值。

算法 5.1（IEKF 更新步）：

复制代码

初始化：x̂ = x̂_{t|t-1}
重复：
    1. 计算雅可比：H = ∂h/∂x |_{x̂}
    2. 计算卡尔曼增益：K = P H^T (H P H^T + R)^{-1}
    3. 更新估计：x̂ = x̂_{t|t-1} + K (z_t - h(x̂) - H(x̂_{t|t-1} - x̂))
直到 收敛

第六章：无迹卡尔曼滤波（UKF）与粒子滤波

6.1 无迹卡尔曼滤波（UKF）

6.1.1 核心思想

问题：EKF 需要计算雅可比矩阵------对于复杂系统可能不可行。

UKF 的思想 ：使用确定性采样（sigma 点）来传播均值和协方差，不需要计算雅可比。

6.1.2 Sigma 点的生成

定义 6.1（Sigma 点） ：对于 nnn 维状态 x\mathbf{x}x，均值 μ\boldsymbol{\mu}μ，协方差 Σ\boldsymbol{\Sigma}Σ，生成 2n+12n+12n+1 个 sigma 点：

X0=μ\mathcal{X}_0 = \boldsymbol{\mu}X0=μ

Xi=μ+((n+λ)Σ)i,i=1,...,n\mathcal{X}_i = \boldsymbol{\mu} + (\sqrt{(n+\lambda)\boldsymbol{\Sigma}})_i, \quad i = 1, \dots, nXi=μ+((n+λ)Σ )i,i=1,...,n

Xi+n=μ−((n+λ)Σ)i,i=1,...,n\mathcal{X}_{i+n} = \boldsymbol{\mu} - (\sqrt{(n+\lambda)\boldsymbol{\Sigma}})_i, \quad i = 1, \dots, nXi+n=μ−((n+λ)Σ )i,i=1,...,n

其中 λ=α2(n+κ)−n\lambda = \alpha^2(n + \kappa) - nλ=α2(n+κ)−n 是缩放参数。

6.1.3 UKF 算法

算法 6.1（UKF）：

预测步：

生成 sigma 点 Xt−1\mathcal{X}_{t-1}Xt−1
通过运动模型传播：Xt∗=f(Xt−1)\mathcal{X}t^* = f(\mathcal{X}{t-1})Xt∗=f(Xt−1)
计算先验均值和协方差：
x^t∣t−1=∑iwi(m)Xt,i∗\hat{\mathbf{x}}{t|t-1} = \sum_i w_i^{(m)} \mathcal{X}{t,i}^*x^t∣t−1=i∑wi(m)Xt,i∗
Pt∣t−1=∑iwi(c)(Xt,i∗−x^t∣t−1)(Xt,i∗−x^t∣t−1)T+QtP_{t|t-1} = \sum_i w_i^{(c)} (\mathcal{X}{t,i}^* - \hat{\mathbf{x}}{t|t-1})(\mathcal{X}{t,i}^* - \hat{\mathbf{x}}{t|t-1})^T + Q_tPt∣t−1=i∑wi(c)(Xt,i∗−x^t∣t−1)(Xt,i∗−x^t∣t−1)T+Qt

更新步：

生成新的 sigma 点
通过测量模型传播：Zt=h(Xt)\mathcal{Z}_t = h(\mathcal{X}_t)Zt=h(Xt)
计算测量的均值和协方差
计算卡尔曼增益和后验估计

6.1.4 UKF vs EKF

定理 6.1（UKF 的精度） ：UKF 对均值和协方差的近似精度达到二阶（对于高斯噪声），而 EKF 只有一阶。

推论 6.1：UKF 在强非线性系统中通常优于 EKF。

6.2 粒子滤波

6.2.1 核心思想

粒子滤波 ：用一组粒子（加权样本）来表示任意概率分布。

定义 6.2（粒子表示）：

p(xt∣z1:t)≈∑i=1Nwt(i)δ(xt−xt(i))p(\mathbf{x}t | \mathbf{z}{1:t}) \approx \sum_{i=1}^{N} w_t^{(i)} \delta(\mathbf{x}_t - \mathbf{x}_t^{(i)})p(xt∣z1:t)≈i=1∑Nwt(i)δ(xt−xt(i))

其中 xt(i)\mathbf{x}_t^{(i)}xt(i) 是第 iii 个粒子，wt(i)w_t^{(i)}wt(i) 是其权重。

6.2.2 粒子滤波算法

算法 6.2（粒子滤波 / Sequential Importance Resampling）：

复制代码

输入：粒子集 {x_{t-1}^{(i)}, w_{t-1}^{(i)}}_{i=1}^N, 控制 u_t, 观测 z_t

1. 重采样：从 {x_{t-1}^{(i)}} 中按权重采样 N 个粒子

2. 预测：对每个粒子，从运动模型采样
   x_t^{(i)} ~ p(x_t | x_{t-1}^{(i)}, u_t)

3. 更新权重：
   w_t^{(i)} ∝ p(z_t | x_t^{(i)})

4. 归一化权重：
   w_t^{(i)} = w_t^{(i)} / Σ_j w_t^{(j)}

输出：{x_t^{(i)}, w_t^{(i)}}_{i=1}^N

6.2.3 粒子滤波的理论性质

定理 6.2（粒子滤波的收敛性） ：当粒子数 N→∞N \to \inftyN→∞ 时，粒子滤波的估计收敛到真实后验。

定理 6.3（粒子退化问题）：经过多次迭代，大部分粒子的权重趋近于零------只有少数粒子有有效贡献。

解决方案：

重采样（Resampling）
更好的提议分布
正则化粒子滤波

第三部分：SLAM 问题的形式化

第七章：SLAM 问题的数学定义

7.1 SLAM 的形式化

7.1.1 问题定义

定义 7.1（SLAM 问题）：给定：

控制输入序列 u1:t\mathbf{u}_{1:t}u1:t
观测序列 z1:t\mathbf{z}_{1:t}z1:t

估计：

机器人轨迹 x1:t\mathbf{x}_{1:t}x1:t
地图 m={m1,m2,...,mN}\mathbf{m} = \{m_1, m_2, \dots, m_N\}m={m1,m2,...,mN}

数学形式：

p(x1:t,m∣z1:t,u1:t)p(\mathbf{x}{1:t}, \mathbf{m} | \mathbf{z}{1:t}, \mathbf{u}_{1:t})p(x1:t,m∣z1:t,u1:t)

7.1.2 SLAM 的概率图模型

定义 7.2（SLAM 的因子图）：

复制代码

x_0 --- x_1 --- x_2 --- ... --- x_t
|       |       |                 |
z_0     z_1     z_2               z_t
|       |       |                 |
m_1     m_1     m_2               m_k

节点：

xi\mathbf{x}_ixi：机器人位姿
mjm_jmj：地标位置

边（因子）：

运动因子：f(xt∣xt−1,ut)f(\mathbf{x}t | \mathbf{x}{t-1}, \mathbf{u}_t)f(xt∣xt−1,ut)
观测因子：g(zt∣xt,mj)g(\mathbf{z}_t | \mathbf{x}_t, m_j)g(zt∣xt,mj)

7.1.3 SLAM 的两个子问题

前端（Front-end）：

数据关联（哪个观测对应哪个地标）
里程计估计（短期位姿估计）
回环检测（发现之前去过的地方）

后端（Backend）：

优化（最小化总误差）
滤波（递归估计）

7.2 SLAM 的分类

类别	方法	优点	缺点
基于滤波	EKF-SLAM, FastSLAM	在线、增量	误差累积、线性化
基于优化	Graph-SLAM, g2o	全局一致、精度高	计算量大
基于特征	激光 SLAM	精确、稳定	特征提取困难
基于直接法	LSD-SLAM, DSO	不需要特征	对光照敏感
视觉惯性	VINS-Mono, OKVIS	互补、鲁棒	标定复杂

第八章：EKF-SLAM

8.1 EKF-SLAM 的状态表示

8.1.1 状态向量

定义 8.1（EKF-SLAM 状态）：

x= $xrobot,m1,m2,...,mN$ T\mathbf{x} = $\\mathbf{x}_{\\text{robot}}, m_1, m_2, \\dots, m_N$ ^Tx= $xrobot,m1,m2,...,mN$ T

其中：

xrobot= $x,y,θ$ T\mathbf{x}_{\text{robot}} = $x, y, \\theta$ ^Txrobot= $x,y,θ$ T：机器人位姿
mi= $mix,miy$ Tm_i = $m_i\^x, m_i\^y$ ^Tmi= $mix,miy$ T：第 iii 个地标的位置

状态维度 ：3+2N3 + 2N3+2N

8.1.2 协方差矩阵

P=(PxxPxmPmxPmm)P = \begin{pmatrix} P_{\text{xx}} & P_{\text{xm}} \\ P_{\text{mx}} & P_{\text{mm}} \end{pmatrix}P=(PxxPmxPxmPmm)

其中：

Pxx∈R3×3P_{\text{xx}} \in \mathbb{R}^{3 \times 3}Pxx∈R3×3：机器人位姿的协方差
Pmm∈R2N×2NP_{\text{mm}} \in \mathbb{R}^{2N \times 2N}Pmm∈R2N×2N：地标的协方差
Pxm∈R3×2NP_{\text{xm}} \in \mathbb{R}^{3 \times 2N}Pxm∈R3×2N：机器人与地标的交叉协方差

8.2 EKF-SLAM 算法

8.2.1 预测步

机器人状态预测：

x^robot,t∣t−1=f(x^robot,t−1∣t−1,ut)\hat{\mathbf{x}}{\text{robot}, t|t-1} = f(\hat{\mathbf{x}}{\text{robot}, t-1|t-1}, \mathbf{u}_t)x^robot,t∣t−1=f(x^robot,t−1∣t−1,ut)

协方差预测：

Pt∣t−1=FtPt−1∣t−1FtT+QtP_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_tPt∣t−1=FtPt−1∣t−1FtT+Qt

其中 FtF_tFt 是包含机器人和地标的状态转移雅可比：

Ft=(Frobot00I2N)F_t = \begin{pmatrix} F_{\text{robot}} & 0 \\ 0 & I_{2N} \end{pmatrix}Ft=(Frobot00I2N)

地标位置不随机器人运动改变。

8.2.2 更新步

观测模型：

zt(i)=h(xrobot,t,mi)+vt\mathbf{z}t^{(i)} = h(\mathbf{x}{\text{robot}, t}, m_i) + \mathbf{v}_tzt(i)=h(xrobot,t,mi)+vt

雅可比矩阵：

Ht(i)= $Hrobot,0,...,0,Hmi,0,...,0$ H_t^{(i)} = $H_{\\text{robot}}, 0, \\dots, 0, H_{m_i}, 0, \\dots, 0$ Ht(i)= $Hrobot,0,...,0,Hmi,0,...,0$

其中：

Hrobot=∂h∂xrobot,Hmi=∂h∂miH_{\text{robot}} = \frac{\partial h}{\partial \mathbf{x}{\text{robot}}}, \quad H{m_i} = \frac{\partial h}{\partial m_i}Hrobot=∂xrobot∂h,Hmi=∂mi∂h

更新：

Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1K_t = P_{t|t-1} H_t^T (H_t P_{t|t-1} H_t^T + R_t)^{-1}Kt=Pt∣t−1HtT(HtPt∣t−1HtT+Rt)−1

x^t∣t=x^t∣t−1+Kt(zt−h(x^t∣t−1))\hat{\mathbf{x}}{t|t} = \hat{\mathbf{x}}{t|t-1} + K_t (\mathbf{z}t - h(\hat{\mathbf{x}}{t|t-1}))x^t∣t=x^t∣t−1+Kt(zt−h(x^t∣t−1))

Pt∣t=(I−KtHt)Pt∣t−1P_{t|t} = (I - K_t H_t) P_{t|t-1}Pt∣t=(I−KtHt)Pt∣t−1

8.3 EKF-SLAM 的复杂度分析

定理 8.1（EKF-SLAM 的复杂度）：

操作	复杂度
预测步	O(N2)O(N^2)O(N2)
更新步	O(N2)O(N^2)O(N2)
存储	O(N2)O(N^2)O(N2)

其中 NNN 是地标数量。

推论 8.1 ：EKF-SLAM 的复杂度随地标数量二次增长------对于大规模地图不可行。

第九章：基于图的 SLAM 与因子图优化

9.1 图 SLAM 的形式化

9.1.1 因子图

定义 9.1（因子图） ：因子图是一个二部图 G=(X,F,E)G = (\mathcal{X}, \mathcal{F}, \mathcal{E})G=(X,F,E)，其中：

X={x0,x1,...,xt,m1,...,mN}\mathcal{X} = \{\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_t, m_1, \dots, m_N\}X={x0,x1,...,xt,m1,...,mN}：变量节点
F={f1,f2,... }\mathcal{F} = \{f_1, f_2, \dots\}F={f1,f2,...}：因子节点
E\mathcal{E}E：连接变量和因子的边

9.1.2 因子的类型

运动因子：

fmotion(xt−1,xt)=∥xt−f(xt−1,ut)∥Qt−12f_{\text{motion}}(\mathbf{x}_{t-1}, \mathbf{x}_t) = \| \mathbf{x}t - f(\mathbf{x}{t-1}, \mathbf{u}t) \|{Q_t^{-1}}^2fmotion(xt−1,xt)=∥xt−f(xt−1,ut)∥Qt−12

观测因子：

fobs(xt,mj)=∥zt−h(xt,mj)∥Rt−12f_{\text{obs}}(\mathbf{x}_t, m_j) = \| \mathbf{z}_t - h(\mathbf{x}t, m_j) \|{R_t^{-1}}^2fobs(xt,mj)=∥zt−h(xt,mj)∥Rt−12

先验因子：

fprior(x0)=∥x0−x^0∥P0−12f_{\text{prior}}(\mathbf{x}_0) = \| \mathbf{x}_0 - \hat{\mathbf{x}}0 \|{P_0^{-1}}^2fprior(x0)=∥x0−x^0∥P0−12

9.1.3 最大后验估计

定义 9.2（SLAM 的 MAP 问题）：

X∗=arg⁡min⁡X∑i∥zi−hi(X)∥Σi−12\mathbf{X}^* = \arg\min_{\mathbf{X}} \sum_i \| \mathbf{z}i - h_i(\mathbf{X}) \|{\Sigma_i^{-1}}^2X∗=argXmini∑∥zi−hi(X)∥Σi−12

这是一个非线性最小二乘问题。

9.2 非线性最小二乘优化

9.2.1 高斯-牛顿法

算法 9.1（高斯-牛顿法）：

复制代码

初始化：X = X_0
重复：
    1. 计算残差：r_i = z_i - h_i(X)
    2. 计算雅可比：J_i = ∂h_i/∂X
    3. 构建正规方程：(J^T Σ^{-1} J) ΔX = J^T Σ^{-1} r
    4. 求解：ΔX = (J^T Σ^{-1} J)^{-1} J^T Σ^{-1} r
    5. 更新：X = X + ΔX
直到 收敛

9.2.2 列文伯格-马夸特法

算法 9.2（列文伯格-马夸特法）：

复制代码

初始化：X = X_0, λ = λ_0
重复：
    1. 计算残差和雅可比
    2. 构建增广正规方程：
       (J^T Σ^{-1} J + λ I) ΔX = J^T Σ^{-1} r
    3. 计算新残差
    4. 如果新残差更小：
         接受更新，减小 λ
       否则：
         拒绝更新，增大 λ
直到 收敛

定理 9.1（LM 的收敛性）：列文伯格-马夸特法保证每次迭代的残差不增加------单调收敛。

9.3 稀疏性与高效求解

9.3.1 信息矩阵的稀疏性

定理 9.2（SLAM 信息矩阵的稀疏性） ：SLAM 的信息矩阵 H=JTΣ−1JH = J^T \Sigma^{-1} JH=JTΣ−1J 是稀疏的------大部分元素为零。

原因：每个因子只涉及少数变量------运动因子只涉及相邻位姿，观测因子只涉及位姿和地标。

9.3.2 变量消除

定理 9.3（Schur 补）：通过消除地标变量，可以将 SLAM 问题简化为只涉及位姿的问题：

Hxx−HxmHmm−1Hmx=Hxx−∑jHxmjHmjmj−1HmjxH_{\text{xx}} - H_{\text{xm}} H_{\text{mm}}^{-1} H_{\text{mx}} = H_{\text{xx}} - \sum_j H_{\text{x}m_j} H_{m_j m_j}^{-1} H_{m_j \text{x}}Hxx−HxmHmm−1Hmx=Hxx−j∑HxmjHmjmj−1Hmjx

推论 9.1 ：消除地标后，问题维度从 O(t+N)O(t + N)O(t+N) 降低到 O(t)O(t)O(t)------对于大规模地图非常有效。

第四部分：视觉 SLAM

第十章：视觉里程计------特征点法

10.1 特征提取与匹配

10.1.1 特征点

定义 10.1（特征点） ：图像中具有可区分性 和可重复性的点。

常见特征：

Harris 角点：基于图像梯度的角点检测
FAST：基于亮度比较的快速角点检测
SIFT：尺度不变特征变换
ORB：Oriented FAST and Rotated BRIEF

10.1.2 特征描述子

定义 10.2（描述子）：描述特征点周围图像 patch 的向量。

常见描述子：

SIFT：128 维浮点向量
ORB：256 位二进制向量
BRISK：二进制描述子

10.1.3 特征匹配

定义 10.3（特征匹配）：在两幅图像之间找到对应的特征点。

匹配准则：

暴力匹配：计算所有描述子对的距离
FLANN：快速最近邻搜索
比率测试：最近距离/次近距离 < 阈值

10.2 两视图几何

10.2.1 对极几何

定义 10.4（对极约束） ：对于两幅图像中的匹配点 p1,p2\mathbf{p}_1, \mathbf{p}_2p1,p2：

p2TFp1=0\mathbf{p}_2^T F \mathbf{p}_1 = 0p2TFp1=0

其中 FFF 是基础矩阵（Fundamental Matrix）。

10.2.2 本质矩阵

定义 10.5（本质矩阵）：

E=KTFKE = K^T F KE=KTFK

其中 KKK 是相机内参矩阵。

本质矩阵的分解：

E= $t$ ×RE = $t$ _\times RE= $t$ ×R

其中 RRR 是旋转矩阵，ttt 是平移向量， $t$ × $t$ _\times $t$ × 是 ttt 的反对称矩阵。

10.2.3 位姿恢复

算法 10.1（从本质矩阵恢复位姿）：

复制代码

输入：本质矩阵 E
输出：R, t

1. SVD 分解：E = U Σ V^T
2. 构造旋转矩阵：
   R = U W V^T 或 R = U W^T V^T
   其中 W = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]
3. 平移向量：t = U[:, 2]
4. 验证：检查三角化点是否在相机前方

10.3 三角化

10.3.1 三角化问题

定义 10.6（三角化）：给定两幅图像中的匹配点和相机位姿，恢复三维点的位置。

数学形式：

min⁡P∥p1−π1(P)∥2+∥p2−π2(P)∥2\min_{\mathbf{P}} \| \mathbf{p}_1 - \pi_1(\mathbf{P}) \|^2 + \| \mathbf{p}_2 - \pi_2(\mathbf{P}) \|^2Pmin∥p1−π1(P)∥2+∥p2−π2(P)∥2

其中 πi\pi_iπi 是第 iii 个相机的投影函数。

10.3.2 DLT 三角化

算法 10.2（DLT 三角化）：

复制代码

输入：匹配点 p_1, p_2, 投影矩阵 P_1, P_2
输出：三维点 X

1. 构建方程组：
   p_1 × (P_1 X) = 0
   p_2 × (P_2 X) = 0

2. 整理为 AX = 0 的形式

3. SVD 求解：X = V[:, -1]（最小奇异值对应的右奇异向量）

4. 归一化：X = X / X[3]

第十一章：视觉里程计------直接法

11.1 直接法的动机

11.1.1 特征点法的局限

问题：

特征提取和描述子计算耗时
丢失了特征点以外的信息
在纹理贫乏区域可能失败

11.1.2 直接法的核心思想

直接法 ：直接比较像素亮度，不需要显式的特征提取和匹配。

光度误差：

e(ξ)=I1(p)−I2(π(Kexp⁡(ξ^)⋅K−1⋅p))e(\xi) = I_1(\mathbf{p}) - I_2(\pi(K \exp(\hat{\xi}) \cdot K^{-1} \cdot \mathbf{p}))e(ξ)=I1(p)−I2(π(Kexp(ξ^)⋅K−1⋅p))

其中 ξ\xiξ 是相机位姿的李代数表示。

11.2 光流

11.2.1 Lucas-Kanade 光流

假设 11.1（亮度恒定假设）：同一点在不同图像中的亮度不变：

I(x,y,t)=I(x+dx,y+dy,t+dt)I(x, y, t) = I(x + dx, y + dy, t + dt)I(x,y,t)=I(x+dx,y+dy,t+dt)

泰勒展开：

Ixu+Iyv+It=0I_x u + I_y v + I_t = 0Ixu+Iyv+It=0

其中 u=dx/dtu = dx/dtu=dx/dt，v=dy/dtv = dy/dtv=dy/dt 是光流。

11.2.2 Lucas-Kanade 方程

定理 11.1（LK 光流） ：在窗口 Ω\OmegaΩ 内，光流满足：

(∑Ix2∑IxIy∑IxIy∑Iy2)(uv)=(−∑IxIt−∑IyIt)\begin{pmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{pmatrix} \begin{pmatrix} u \\ v \end{pmatrix} = \begin{pmatrix} -\sum I_x I_t \\ -\sum I_y I_t \end{pmatrix}(∑Ix2∑IxIy∑IxIy∑Iy2)(uv)=(−∑IxIt−∑IyIt)

即：

Gd=bG \mathbf{d} = \mathbf{b}Gd=b

其中 GGG 是结构张量（Structure Tensor）。

定理 11.2（光流的可解性） ：当结构张量 GGG 的两个特征值都较大时，光流可解------这对应于角点区域。

11.3 直接法的优化

11.3.1 光度误差的最小化

目标函数：

min⁡ξ∑i∥I1(pi)−I2(π(exp⁡(ξ^)⋅Pi))∥2\min_\xi \sum_{i} \| I_1(\mathbf{p}_i) - I_2(\pi(\exp(\hat{\xi}) \cdot \mathbf{P}_i)) \|^2ξmini∑∥I1(pi)−I2(π(exp(ξ^)⋅Pi))∥2

雅可比矩阵：

∂ei∂ξ=∂I2∂u∂π∂P∂P∂ξ\frac{\partial e_i}{\partial \xi} = \frac{\partial I_2}{\partial \mathbf{u}} \frac{\partial \pi}{\partial \mathbf{P}} \frac{\partial \mathbf{P}}{\partial \xi}∂ξ∂ei=∂u∂I2∂P∂π∂ξ∂P

11.3.2 半稠密直接法

LSD-SLAM ：只在梯度较大的像素上计算光度误差------这些像素提供了足够的信息。

第十二章：回环检测与全局优化

12.1 回环检测

12.1.1 什么是回环？

定义 12.1（回环）：当机器人回到之前去过的地方时，形成回环。

回环的作用：消除累积漂移------将轨迹的首尾连接起来。

12.1.2 回环检测方法

基于外观的回环检测：

score(i,j)=sim(f(Ii),f(Ij))\text{score}(i, j) = \text{sim}(f(I_i), f(I_j))score(i,j)=sim(f(Ii),f(Ij))

其中 f(I)f(I)f(I) 是图像的特征描述。

词袋模型（Bag of Words）：

离线训练视觉词汇表
将每幅图像表示为词袋向量
计算词袋向量的相似度

12.1.3 回环验证

问题：回环检测可能产生误报。

验证方法：

几何验证：检查对极约束
一致性验证：检查与现有地图的一致性

12.2 位姿图优化

12.2.1 位姿图

定义 12.2（位姿图） ：位姿图是一个图 G=(V,E)G = (V, E)G=(V,E)，其中：

节点 V={x0,x1,...,xt}V = \{\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_t\}V={x0,x1,...,xt}：机器人位姿
边 E={(xi,xj,Δij,Σij)}E = \{(\mathbf{x}i, \mathbf{x}j, \Delta{ij}, \Sigma{ij})\}E={(xi,xj,Δij,Σij)}：位姿之间的约束

12.2.2 位姿图优化

目标函数：

min⁡X∑(i,j)∈E∥xj−f(xi,Δij)∥Σij−12\min_{\mathbf{X}} \sum_{(i,j) \in E} \| \mathbf{x}j - f(\mathbf{x}i, \Delta{ij}) \|{\Sigma_{ij}^{-1}}^2Xmin(i,j)∈E∑∥xj−f(xi,Δij)∥Σij−12

这是一个非线性最小二乘问题，可以用高斯-牛顿法或 LM 方法求解。

第十三章：深度学习与 SLAM

13.1 深度学习在 SLAM 中的应用

13.1.1 特征提取

SuperPoint：使用 CNN 提取特征点和描述子。

SuperGlue：使用 GNN 进行特征匹配。

13.1.2 深度估计

单目深度估计：从单张图像估计深度。

d^=fdepth(I)\hat{d} = f_{\text{depth}}(I)d^=fdepth(I)

13.1.3 位姿估计

PoseNet：直接从图像回归相机位姿。

ξ^=fpose(I)\hat{\xi} = f_{\text{pose}}(I)ξ^=fpose(I)

13.2 端到端 SLAM

13.2.1 深度 SLAM

DROID-SLAM：使用深度学习进行可微分的稠密 SLAM。

13.2.2 神经隐式表示

iMAP：使用神经网络表示场景------每个 3D 点的颜色和占据概率由 MLP 预测。

$c,o$ =fθ(x) $c, o$ = f_\theta(\mathbf{x}) $c,o$ =fθ(x)

NICE-SLAM：使用分层的特征网格来表示场景。

第五部分：完整可运行代码实现

第十四章：从零实现卡尔曼滤波

python 复制代码

"""
卡尔曼滤波的完整实现。
包含：线性卡尔曼滤波、扩展卡尔曼滤波。
"""

import numpy as np
from typing import Tuple, Callable


class KalmanFilter:
    """线性卡尔曼滤波器。

    状态空间模型：
        x_t = F @ x_{t-1} + B @ u_t + w_t,  w_t ~ N(0, Q)
        z_t = H @ x_t + v_t,                  v_t ~ N(0, R)
    """

    def __init__(
        self,
        dim_x: int,
        dim_z: int,
        F: np.ndarray = None,
        H: np.ndarray = None,
        Q: np.ndarray = None,
        R: np.ndarray = None,
        B: np.ndarray = None,
        x0: np.ndarray = None,
        P0: np.ndarray = None,
    ):
        self.dim_x = dim_x
        self.dim_z = dim_z

        self.F = F if F is not None else np.eye(dim_x)
        self.H = H if H is not None else np.eye(dim_z, dim_x)
        self.Q = Q if Q is not None else np.eye(dim_x) * 0.01
        self.R = R if R is not None else np.eye(dim_z) * 0.1
        self.B = B if B is not None else np.zeros((dim_x, 1))

        self.x = x0 if x0 is not None else np.zeros(dim_x)
        self.P = P0 if P0 is not None else np.eye(dim_x)

    def predict(self, u: np.ndarray = None):
        """预测步。

        Args:
            u: 控制输入
        """
        if u is not None:
            self.x = self.F @ self.x + self.B @ u
        else:
            self.x = self.F @ self.x

        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z: np.ndarray):
        """更新步。

        Args:
            z: 测量值
        """
        # 预测测量
        z_pred = self.H @ self.x

        # 新息
        y = z - z_pred

        # 新息协方差
        S = self.H @ self.P @ self.H.T + self.R

        # 卡尔曼增益
        K = self.P @ self.H.T @ np.linalg.inv(S)

        # 更新状态
        self.x = self.x + K @ y

        # 更新协方差
        I = np.eye(self.dim_x)
        self.P = (I - K @ self.H) @ self.P

    def get_state(self) -> Tuple[np.ndarray, np.ndarray]:
        """获取当前状态估计。"""
        return self.x.copy(), self.P.copy()


class ExtendedKalmanFilter:
    """扩展卡尔曼滤波器。

    非线性状态空间模型：
        x_t = f(x_{t-1}, u_t) + w_t
        z_t = h(x_t) + v_t
    """

    def __init__(
        self,
        dim_x: int,
        dim_z: int,
        f: Callable,
        h: Callable,
        F_jacobian: Callable,
        H_jacobian: Callable,
        Q: np.ndarray,
        R: np.ndarray,
        x0: np.ndarray = None,
        P0: np.ndarray = None,
    ):
        self.dim_x = dim_x
        self.dim_z = dim_z
        self.f = f
        self.h = h
        self.F_jac = F_jacobian
        self.H_jac = H_jacobian
        self.Q = Q
        self.R = R

        self.x = x0 if x0 is not None else np.zeros(dim_x)
        self.P = P0 if P0 is not None else np.eye(dim_x)

    def predict(self, u: np.ndarray = None):
        """预测步。"""
        # 非线性预测
        self.x = self.f(self.x, u)

        # 雅可比矩阵
        F = self.F_jac(self.x, u)

        # 协方差传播
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z: np.ndarray):
        """更新步。"""
        # 预测测量
        z_pred = self.h(self.x)

        # 雅可比矩阵
        H = self.H_jac(self.x)

        # 新息
        y = z - z_pred

        # 新息协方差
        S = H @ self.P @ H.T + self.R

        # 卡尔曼增益
        K = self.P @ H.T @ np.linalg.inv(S)

        # 更新状态
        self.x = self.x + K @ y

        # 更新协方差
        I = np.eye(self.dim_x)
        self.P = (I - K @ H) @ self.P

    def get_state(self) -> Tuple[np.ndarray, np.ndarray]:
        return self.x.copy(), self.P.copy()


def demonstrate_kalman_filter():
    """演示卡尔曼滤波。"""
    np.random.seed(42)

    print("=" * 70)
    print("卡尔曼滤波演示")
    print("=" * 70)

    # 设置：匀速运动模型
    # 状态：[位置, 速度]
    # 测量：位置

    dt = 1.0
    F = np.array([[1, dt], [0, 1]])
    H = np.array([[1, 0]])
    Q = np.array([[0.01, 0], [0, 0.01]])
    R = np.array([[0.5]])
    B = np.zeros((2, 1))

    # 初始状态
    x0 = np.array([0, 1])  # 位置 0，速度 1
    P0 = np.eye(2)

    # 创建滤波器
    kf = KalmanFilter(dim_x=2, dim_z=1, F=F, H=H, Q=Q, R=R, B=B, x0=x0, P0=P0)

    # 仿真
    n_steps = 20
    true_states = []
    measurements = []
    estimates = []

    x_true = x0.copy()

    for t in range(n_steps):
        # 真实状态演化
        x_true = F @ x_true + np.random.multivariate_normal(np.zeros(2), Q)
        true_states.append(x_true.copy())

        # 测量
        z = H @ x_true + np.random.multivariate_normal(np.zeros(1), R)
        measurements.append(z[0])

        # 卡尔曼滤波
        kf.predict()
        kf.update(z)
        x_est, P_est = kf.get_state()
        estimates.append(x_est.copy())

    true_states = np.array(true_states)
    measurements = np.array(measurements)
    estimates = np.array(estimates)

    # 打印结果
    print(f"\n  {'步骤':>6} {'真实位置':>12} {'测量值':>12} {'估计值':>12} {'估计误差':>12}")
    print(f"  {'-'*6} {'-'*12} {'-'*12} {'-'*12} {'-'*12}")

    for t in range(n_steps):
        error = abs(true_states[t, 0] - estimates[t, 0])
        print(f"  {t:>6} {true_states[t, 0]:>12.4f} {measurements[t]:>12.4f} "
              f"{estimates[t, 0]:>12.4f} {error:>12.4f}")

    # 统计
    pos_errors = np.abs(true_states[:, 0] - estimates[:, 0])
    meas_errors = np.abs(true_states[:, 0] - measurements)

    print(f"\n  位置估计 RMSE: {np.sqrt(np.mean(pos_errors**2)):.4f}")
    print(f"  测量 RMSE:     {np.sqrt(np.mean(meas_errors**2)):.4f}")
    print(f"  卡尔曼滤波改善: {(1 - np.sqrt(np.mean(pos_errors**2)) / np.sqrt(np.mean(meas_errors**2))) * 100:.1f}%")

    # EKF 演示
    print("\n\n  EKF 演示（非线性运动模型）")
    print("  " + "-" * 40)

    # 非线性运动模型：匀速圆周运动
    def f_nonlinear(x, u=None):
        theta = x[2]
        v = 1.0
        dt = 1.0
        return np.array([
            x[0] + v * np.cos(theta) * dt,
            x[1] + v * np.sin(theta) * dt,
            x[2] + 0.1 * dt  # 角速度
        ])

    def h_nonlinear(x):
        return np.array([np.sqrt(x[0]**2 + x[1]**2)])  # 测量距离

    def F_jac(x, u=None):
        theta = x[2]
        v = 1.0
        dt = 1.0
        return np.array([
            [1, 0, -v * np.sin(theta) * dt],
            [0, 1, v * np.cos(theta) * dt],
            [0, 0, 1]
        ])

    def H_jac(x):
        r = np.sqrt(x[0]**2 + x[1]**2)
        return np.array([[x[0]/r, x[1]/r, 0]])

    ekf = ExtendedKalmanFilter(
        dim_x=3, dim_z=1,
        f=f_nonlinear, h=h_nonlinear,
        F_jacobian=F_jac, H_jacobian=H_jac,
        Q=np.diag([0.01, 0.01, 0.001]),
        R=np.array([[0.5]]),
        x0=np.array([1, 0, 0]),
        P0=np.eye(3)
    )

    n_steps = 20
    x_true = np.array([1.0, 0.0, 0.0])

    print(f"\n  {'步骤':>6} {'真实位置':>15} {'估计位置':>15} {'误差':>12}")
    print(f"  {'-'*6} {'-'*15} {'-'*15} {'-'*12}")

    for t in range(n_steps):
        x_true = f_nonlinear(x_true) + np.random.multivariate_normal(np.zeros(3), np.diag([0.01, 0.01, 0.001]))
        z = h_nonlinear(x_true) + np.random.normal(0, np.sqrt(0.5))

        ekf.predict()
        ekf.update(np.array([z]))
        x_est, _ = ekf.get_state()

        error = np.linalg.norm(x_true[:2] - x_est[:2])
        print(f"  {t:>6} ({x_true[0]:>5.2f}, {x_true[1]:>5.2f}) "
              f"({x_est[0]:>5.2f}, {x_est[1]:>5.2f}) {error:>12.4f}")


if __name__ == "__main__":
    demonstrate_kalman_filter()

第十五章：从零实现 EKF-SLAM

python 复制代码

"""
EKF-SLAM 的完整实现。
包含：2D 激光 SLAM 的基本框架。
"""

import numpy as np
from typing import Tuple, List


class EKFSLAM:
    """EKF-SLAM 实现。

    状态：[x, y, θ, m1_x, m1_y, m2_x, m2_y, ...]
    """

    def __init__(
        self,
        initial_pose: np.ndarray = None,
        Q_robot: np.ndarray = None,
        R_obs: np.ndarray = None,
        landmark_threshold: float = 5.0,
    ):
        """初始化 EKF-SLAM。

        Args:
            initial_pose: 初始位姿 [x, y, θ]
            Q_robot: 机器人运动噪声
            R_obs: 观测噪声
            landmark_threshold: 新地标检测阈值
        """
        self.x = initial_pose if initial_pose is not None else np.zeros(3)
        self.P = np.eye(3) * 0.1
        self.Q_robot = Q_robot if Q_robot is not None else np.diag([0.1, 0.1, 0.05])
        self.R_obs = R_obs if R_obs is not None else np.diag([0.5, 0.1])
        self.landmark_threshold = landmark_threshold

        self.n_landmarks = 0
        self.landmarks = {}  # id -> (index_in_state, position)

    def predict(self, u: np.ndarray):
        """预测步：机器人运动。

        Args:
            u: 控制输入 [Δs, Δθ]
        """
        delta_s, delta_theta = u
        theta = self.x[2]

        # 更新机器人状态
        self.x[0] += delta_s * np.cos(theta + delta_theta / 2)
        self.x[1] += delta_s * np.sin(theta + delta_theta / 2)
        self.x[2] += delta_theta

        # 雅可比矩阵（对机器人状态）
        F_robot = np.array([
            [1, 0, -delta_s * np.sin(theta + delta_theta / 2)],
            [0, 1, delta_s * np.cos(theta + delta_theta / 2)],
            [0, 0, 1]
        ])

        # 扩展到完整状态空间
        n = len(self.x)
        F = np.eye(n)
        F[:3, :3] = F_robot

        # 扩展噪声矩阵
        Q = np.zeros((n, n))
        Q[:3, :3] = self.Q_robot

        # 更新协方差
        self.P = F @ self.P @ F.T + Q

    def update(self, observations: List[Tuple[float, float, int]]):
        """更新步：处理观测。

        Args:
            observations: 观测列表 [(range, bearing, landmark_id), ...]
        """
        for obs in observations:
            z_range, z_bearing, landmark_id = obs
            z = np.array([z_range, z_bearing])

            if landmark_id in self.landmarks:
                # 已知地标：更新
                self._update_known_landmark(z, landmark_id)
            else:
                # 新地标：初始化
                self._initialize_new_landmark(z, landmark_id)

    def _update_known_landmark(self, z: np.ndarray, landmark_id: int):
        """更新已知地标。"""
        idx = self.landmarks[landmark_id][0]
        lx, ly = self.x[idx], self.x[idx + 1]

        # 预测观测
        dx = lx - self.x[0]
        dy = ly - self.x[1]
        r = np.sqrt(dx**2 + dy**2)
        b = np.arctan2(dy, dx) - self.x[2]

        z_pred = np.array([r, b])

        # 雅可比矩阵
        n = len(self.x)
        H = np.zeros((2, n))

        # 对机器人状态的导数
        H[0, 0] = -dx / r
        H[0, 1] = -dy / r
        H[1, 0] = dy / r**2
        H[1, 1] = -dx / r**2
        H[1, 2] = -1

        # 对地标状态的导数
        H[0, idx] = dx / r
        H[0, idx + 1] = dy / r
        H[1, idx] = -dy / r**2
        H[1, idx + 1] = dx / r**2

        # 卡尔曼更新
        S = H @ self.P @ H.T + self.R_obs
        K = self.P @ H.T @ np.linalg.inv(S)

        # 新息
        y = z - z_pred
        y[1] = self._normalize_angle(y[1])

        # 更新
        self.x = self.x + K @ y
        n = len(self.x)
        self.P = (np.eye(n) - K @ H) @ self.P

    def _initialize_new_landmark(self, z: np.ndarray, landmark_id: int):
        """初始化新地标。"""
        r, b = z

        # 计算地标位置
        lx = self.x[0] + r * np.cos(self.x[2] + b)
        ly = self.x[1] + r * np.sin(self.x[2] + b)

        # 扩展状态向量
        idx = len(self.x)
        self.x = np.append(self.x, [lx, ly])

        # 扩展协方差矩阵
        n = len(self.x)
        P_new = np.zeros((n, n))
        P_new[:n-2, :n-2] = self.P

        # 新地标的协方差（通过雅可比传播）
        G_x = np.array([
            [1, 0, -r * np.sin(self.x[2] + b)],
            [0, 1, r * np.cos(self.x[2] + b)]
        ])
        G_z = np.array([
            [np.cos(self.x[2] + b), -r * np.sin(self.x[2] + b)],
            [np.sin(self.x[2] + b), r * np.cos(self.x[2] + b)]
        ])

        P_new[n-2:n, n-2:n] = G_x @ self.P[:3, :3] @ G_x.T + G_z @ self.R_obs @ G_z.T
        P_new[n-2:n, :3] = G_x @ self.P[:3, :3]
        P_new[:3, n-2:n] = self.P[:3, :3] @ G_x.T

        self.P = P_new
        self.landmarks[landmark_id] = (idx, np.array([lx, ly]))
        self.n_landmarks += 1

    @staticmethod
    def _normalize_angle(angle: float) -> float:
        """将角度归一化到 [-π, π]。"""
        while angle > np.pi:
            angle -= 2 * np.pi
        while angle < -np.pi:
            angle += 2 * np.pi
        return angle

    def get_pose(self) -> np.ndarray:
        """获取机器人位姿。"""
        return self.x[:3].copy()

    def get_landmarks(self) -> dict:
        """获取地标位置。"""
        result = {}
        for lid, (idx, _) in self.landmarks.items():
            result[lid] = self.x[idx:idx+2].copy()
        return result


def demonstrate_ekf_slam():
    """演示 EKF-SLAM。"""
    np.random.seed(42)

    print("=" * 70)
    print("EKF-SLAM 演示")
    print("=" * 70)

    # 创建 SLAM 实例
    slam = EKFSLAM(
        initial_pose=np.array([0, 0, 0]),
        Q_robot=np.diag([0.05, 0.05, 0.02]),
        R_obs=np.diag([0.3, 0.05]),
    )

    # 真实地标位置
    true_landmarks = {
        0: np.array([5, 5]),
        1: np.array([10, 0]),
        2: np.array([5, -5]),
        3: np.array([0, -10]),
        4: np.array([-5, -5]),
    }

    # 仿真参数
    n_steps = 30
    dt = 1.0
    v = 1.0  # 速度
    omega = 0.1  # 角速度

    true_pose = np.array([0.0, 0.0, 0.0])

    print(f"\n  真实地标位置:")
    for lid, pos in true_landmarks.items():
        print(f"    地标 {lid}: ({pos[0]:.1f}, {pos[1]:.1f})")

    print(f"\n  {'步骤':>6} {'真实位姿':>20} {'估计位姿':>20} {'误差':>10}")
    print(f"  {'-'*6} {'-'*20} {'-'*20} {'-'*10}")

    for t in range(n_steps):
        # 真实运动
        theta = true_pose[2]
        true_pose[0] += v * np.cos(theta) * dt
        true_pose[1] += v * np.sin(theta) * dt
        true_pose[2] += omega * dt

        # 控制输入（带噪声）
        u = np.array([v * dt, omega * dt]) + np.random.randn(2) * 0.1

        # SLAM 预测
        slam.predict(u)

        # 观测（模拟激光雷达）
        observations = []
        for lid, lpos in true_landmarks.items():
            dx = lpos[0] - true_pose[0]
            dy = lpos[1] - true_pose[1]
            r = np.sqrt(dx**2 + dy**2)

            if r < 15:  # 观测范围
                b = np.arctan2(dy, dx) - true_pose[2]
                r_noisy = r + np.random.randn() * 0.3
                b_noisy = b + np.random.randn() * 0.05
                observations.append((r_noisy, b_noisy, lid))

        # SLAM 更新
        slam.update(observations)

        # 获取估计
        est_pose = slam.get_pose()
        error = np.linalg.norm(true_pose[:2] - est_pose[:2])

        if t % 5 == 0:
            print(f"  {t:>6} ({true_pose[0]:>6.2f}, {true_pose[1]:>6.2f}, {true_pose[2]:>5.2f}) "
                  f"({est_pose[0]:>6.2f}, {est_pose[1]:>6.2f}, {est_pose[2]:>5.2f}) {error:>10.4f}")

    # 最终地标估计
    print(f"\n  最终地标估计:")
    est_landmarks = slam.get_landmarks()
    for lid in sorted(est_landmarks.keys()):
        true_pos = true_landmarks[lid]
        est_pos = est_landmarks[lid]
        error = np.linalg.norm(true_pos - est_pos)
        print(f"    地标 {lid}: 真实 ({true_pos[0]:>5.1f}, {true_pos[1]:>5.1f}), "
              f"估计 ({est_pos[0]:>5.2f}, {est_pos[1]:>5.2f}), 误差 {error:.4f}")


if __name__ == "__main__":
    demonstrate_ekf_slam()

第十六章：从零实现简单的视觉里程计

python 复制代码

"""
简单视觉里程计的完整实现。
包含：特征提取、匹配、位姿估计、三角化。
"""

import numpy as np
from typing import Tuple, List


def detect_corners(image: np.ndarray, threshold: float = 0.01) -> List[Tuple[int, int]]:
    """简单的角点检测（基于梯度）。

    Args:
        image: 灰度图像 (H, W)
        threshold: 梯度阈值

    Returns:
        corners: 角点列表 [(x, y), ...]
    """
    H, W = image.shape

    # 计算梯度
    Ix = np.zeros_like(image)
    Iy = np.zeros_like(image)

    Ix[:, 1:-1] = image[:, 2:] - image[:, :-2]
    Iy[1:-1, :] = image[2:, :] - image[:-2, :]

    # Harris 角点检测
    Ixx = Ix * Ix
    Iyy = Iy * Iy
    Ixy = Ix * Iy

    # 窗口求和
    window_size = 3
    offset = window_size // 2

    corners = []
    for i in range(offset, H - offset):
        for j in range(offset, W - offset):
            Sxx = np.sum(Ixx[i-offset:i+offset+1, j-offset:j+offset+1])
            Syy = np.sum(Iyy[i-offset:i+offset+1, j-offset:j+offset+1])
            Sxy = np.sum(Ixy[i-offset:i+offset+1, j-offset:j+offset+1])

            # Harris 响应
            det = Sxx * Syy - Sxy**2
            trace = Sxx + Syy
            R = det - 0.04 * trace**2

            if R > threshold:
                corners.append((j, i))

    return corners


def compute_descriptor(image: np.ndarray, x: int, y: int, patch_size: int = 8) -> np.ndarray:
    """计算简单描述子（图像 patch 展平）。

    Args:
        image: 灰度图像
        x, y: 特征点坐标
        patch_size: patch 大小

    Returns:
        descriptor: 描述子向量
    """
    offset = patch_size // 2
    H, W = image.shape

    x_start = max(0, x - offset)
    x_end = min(W, x + offset)
    y_start = max(0, y - offset)
    y_end = min(H, y + offset)

    patch = image[y_start:y_end, x_start:x_end].astype(float)

    # 归一化
    if patch.std() > 0:
        patch = (patch - patch.mean()) / patch.std()

    # 展平并截断/填充到固定长度
    descriptor = patch.flatten()
    target_len = patch_size * patch_size

    if len(descriptor) < target_len:
        descriptor = np.pad(descriptor, (0, target_len - len(descriptor)))
    else:
        descriptor = descriptor[:target_len]

    return descriptor


def match_features(
    desc1: List[np.ndarray],
    desc2: List[np.ndarray],
    ratio_threshold: float = 0.8,
) -> List[Tuple[int, int]]:
    """特征匹配（最近邻比率测试）。

    Args:
        desc1: 图像 1 的描述子列表
        desc2: 图像 2 的描述子列表
        ratio_threshold: 比率阈值

    Returns:
        matches: 匹配列表 [(idx1, idx2), ...]
    """
    matches = []

    for i, d1 in enumerate(desc1):
        distances = [np.linalg.norm(d1 - d2) for d2 in desc2]

        if len(distances) < 2:
            continue

        # 排序
        sorted_indices = np.argsort(distances)

        # 比率测试
        if distances[sorted_indices[0]] < ratio_threshold * distances[sorted_indices[1]]:
            matches.append((i, sorted_indices[0]))

    return matches


def estimate_fundamental_matrix(
    pts1: np.ndarray,
    pts2: np.ndarray,
) -> np.ndarray:
    """估计基础矩阵（8 点算法的简化版）。

    Args:
        pts1: 匹配点 1 (N, 2)
        pts2: 匹配点 2 (N, 2)

    Returns:
        F: 基础矩阵 (3, 3)
    """
    n = pts1.shape[0]

    # 归一化
    mean1 = pts1.mean(axis=0)
    mean2 = pts2.mean(axis=0)
    pts1_centered = pts1 - mean1
    pts2_centered = pts2 - mean2

    scale1 = np.sqrt(2) / (np.mean(np.linalg.norm(pts1_centered, axis=1)) + 1e-8)
    scale2 = np.sqrt(2) / (np.mean(np.linalg.norm(pts2_centered, axis=1)) + 1e-8)

    T1 = np.array([[scale1, 0, -scale1 * mean1[0]],
                    [0, scale1, -scale1 * mean1[1]],
                    [0, 0, 1]])
    T2 = np.array([[scale2, 0, -scale2 * mean2[0]],
                    [0, scale2, -scale2 * mean2[1]],
                    [0, 0, 1]])

    pts1_norm = (T1 @ np.hstack([pts1, np.ones((n, 1))]).T).T[:, :2]
    pts2_norm = (T2 @ np.hstack([pts2, np.ones((n, 1))]).T).T[:, :2]

    # 构建方程组
    A = np.zeros((n, 9))
    for i in range(n):
        u1, v1 = pts1_norm[i]
        u2, v2 = pts2_norm[i]
        A[i] = [u2*u1, u2*v1, u2, v2*u1, v2*v1, v2, u1, v1, 1]

    # SVD 求解
    _, _, Vt = np.linalg.svd(A)
    F = Vt[-1].reshape(3, 3)

    # 强制秩 2 约束
    U, S, Vt = np.linalg.svd(F)
    S[2] = 0
    F = U @ np.diag(S) @ Vt

    # 反归一化
    F = T2.T @ F @ T1

    return F / F[2, 2]


def triangulate_points(
    pts1: np.ndarray,
    pts2: np.ndarray,
    P1: np.ndarray,
    P2: np.ndarray,
) -> np.ndarray:
    """三角化三维点。

    Args:
        pts1: 匹配点 1 (N, 2)
        pts2: 匹配点 2 (N, 2)
        P1: 投影矩阵 1 (3, 4)
        P2: 投影矩阵 2 (3, 4)

    Returns:
        points_3d: 三维点 (N, 3)
    """
    n = pts1.shape[0]
    points_3d = np.zeros((n, 3))

    for i in range(n):
        x1, y1 = pts1[i]
        x2, y2 = pts2[i]

        A = np.array([
            x1 * P1[2] - P1[0],
            y1 * P1[2] - P1[1],
            x2 * P2[2] - P2[0],
            y2 * P2[2] - P2[1],
        ])

        _, _, Vt = np.linalg.svd(A)
        X = Vt[-1]
        points_3d[i] = X[:3] / X[3]

    return points_3d


def demonstrate_visual_odometry():
    """演示视觉里程计。"""
    np.random.seed(42)

    print("=" * 70)
    print("简单视觉里程计演示")
    print("=" * 70)

    # 生成模拟图像（简单的 2D 点云）
    H, W = 100, 100
    n_points = 20

    # 3D 点
    points_3d = np.random.randn(n_points, 3) * 5
    points_3d[:, 2] += 10  # 确保在相机前方

    # 相机内参
    K = np.array([[100, 0, 50],
                  [0, 100, 50],
                  [0, 0, 1]])

    # 相机 1 位姿（单位矩阵）
    R1 = np.eye(3)
    t1 = np.zeros(3)
    P1 = K @ np.hstack([R1, t1.reshape(3, 1)])

    # 相机 2 位姿（小幅运动）
    angle = 0.1
    R2 = np.array([[np.cos(angle), 0, np.sin(angle)],
                    [0, 1, 0],
                    [-np.sin(angle), 0, np.cos(angle)]])
    t2 = np.array([1, 0, 0])
    P2 = K @ np.hstack([R2, t2.reshape(3, 1)])

    # 投影到图像
    pts1_homo = (P1 @ np.hstack([points_3d, np.ones((n_points, 1))]).T).T
    pts1 = pts1_homo[:, :2] / pts1_homo[:, 2:3]

    pts2_homo = (P2 @ np.hstack([points_3d, np.ones((n_points, 1))]).T).T
    pts2 = pts2_homo[:, :2] / pts2_homo[:, 2:3]

    # 添加噪声
    pts1 += np.random.randn(n_points, 2) * 0.5
    pts2 += np.random.randn(n_points, 2) * 0.5

    print(f"\n  3D 点数量: {n_points}")
    print(f"  相机运动: 旋转 {angle:.1f} rad, 平移 ({t2[0]:.1f}, {t2[1]:.1f}, {t2[2]:.1f})")

    # 估计基础矩阵
    F = estimate_fundamental_matrix(pts1, pts2)

    # 检查对极约束
    errors = []
    for i in range(n_points):
        p1 = np.append(pts1[i], 1)
        p2 = np.append(pts2[i], 1)
        error = abs(p2 @ F @ p1)
        errors.append(error)

    print(f"\n  对极约束平均误差: {np.mean(errors):.6f}")

    # 三角化
    points_3d_est = triangulate_points(pts1, pts2, P1, P2)

    # 计算重建误差
    # 归一化到相同尺度
    scale = np.linalg.norm(points_3d) / np.linalg.norm(points_3d_est)
    points_3d_est_scaled = points_3d_est * scale

    errors_3d = np.linalg.norm(points_3d - points_3d_est_scaled, axis=1)

    print(f"\n  三维重建误差:")
    print(f"    平均: {np.mean(errors_3d):.4f}")
    print(f"    最大: {np.max(errors_3d):.4f}")
    print(f"    最小: {np.min(errors_3d):.4f}")


if __name__ == "__main__":
    demonstrate_visual_odometry()

第十七章：完整 SLAM Pipeline 与精度对比

python 复制代码

"""
完整的 SLAM Pipeline。
对比滤波方法和优化方法。
"""

import numpy as np
from typing import Tuple, List


def run_slam_comparison():
    """运行 SLAM 方法对比。"""
    np.random.seed(42)

    print("=" * 70)
    print("SLAM 方法综合对比")
    print("=" * 70)

    # 设置
    n_landmarks = 10
    n_steps = 50

    # 真实地标
    true_landmarks = np.random.randn(n_landmarks, 2) * 10

    # 真实轨迹（圆形）
    radius = 15
    true_trajectory = np.zeros((n_steps, 3))

    for t in range(n_steps):
        angle = 2 * np.pi * t / n_steps
        true_trajectory[t] = [radius * np.cos(angle), radius * np.sin(angle), angle + np.pi/2]

    # 生成观测
    def generate_observations(pose, landmarks, max_range=20):
        observations = []
        for i, lm in enumerate(landmarks):
            dx = lm[0] - pose[0]
            dy = lm[1] - pose[1]
            r = np.sqrt(dx**2 + dy**2)
            if r < max_range:
                b = np.arctan2(dy, dx) - pose[2]
                r_noisy = r + np.random.randn() * 0.3
                b_noisy = b + np.random.randn() * 0.05
                observations.append((r_noisy, b_noisy, i))
        return observations

    # 方法 1：仅里程计（无 SLAM）
    def odometry_only(true_trajectory, noise_scale=0.1):
        estimated = np.zeros_like(true_trajectory)
        estimated[0] = true_trajectory[0]

        for t in range(1, len(true_trajectory)):
            # 里程计增量
            dx = true_trajectory[t, 0] - true_trajectory[t-1, 0]
            dy = true_trajectory[t, 1] - true_trajectory[t-1, 1]
            dtheta = true_trajectory[t, 2] - true_trajectory[t-1, 2]

            # 添加噪声
            dx += np.random.randn() * noise_scale
            dy += np.random.randn() * noise_scale
            dtheta += np.random.randn() * noise_scale * 0.1

            estimated[t, 0] = estimated[t-1, 0] + dx
            estimated[t, 1] = estimated[t-1, 1] + dy
            estimated[t, 2] = estimated[t-1, 2] + dtheta

        return estimated

    # 方法 2：简单滤波 SLAM
    def filter_slam(true_trajectory, true_landmarks):
        # 初始化
        pose = true_trajectory[0].copy()
        P = np.eye(3) * 0.1
        landmarks_est = {}
        Q = np.diag([0.05, 0.05, 0.02])
        R = np.diag([0.3, 0.05])

        trajectory = np.zeros_like(true_trajectory)
        trajectory[0] = pose

        for t in range(1, n_steps):
            # 预测
            dx = true_trajectory[t, 0] - true_trajectory[t-1, 0]
            dy = true_trajectory[t, 1] - true_trajectory[t-1, 1]
            dtheta = true_trajectory[t, 2] - true_trajectory[t-1, 2]

            theta = pose[2]
            pose[0] += dx * np.cos(theta) - dy * np.sin(theta)
            pose[1] += dx * np.sin(theta) + dy * np.cos(theta)
            pose[2] += dtheta

            P = P + Q

            # 观测更新
            obs = generate_observations(pose, true_landmarks)
            for r, b, lid in obs:
                if lid not in landmarks_est:
                    # 初始化地标
                    lx = pose[0] + r * np.cos(pose[2] + b)
                    ly = pose[1] + r * np.sin(pose[2] + b)
                    landmarks_est[lid] = np.array([lx, ly])
                else:
                    # 简单更新（近似）
                    lx = pose[0] + r * np.cos(pose[2] + b)
                    ly = pose[1] + r * np.sin(pose[2] + b)
                    landmarks_est[lid] = 0.8 * landmarks_est[lid] + 0.2 * np.array([lx, ly])

            trajectory[t] = pose.copy()

        return trajectory, landmarks_est

    # 方法 3：优化 SLAM（简化版------周期性修正）
    def optimization_slam(true_trajectory, true_landmarks):
        trajectory = odometry_only(true_trajectory, noise_scale=0.1)
        landmarks_est = {}

        # 每隔一段时间进行修正
        correction_interval = 10

        for t in range(n_steps):
            obs = generate_observations(trajectory[t], true_landmarks)

            for r, b, lid in obs:
                lx = trajectory[t, 0] + r * np.cos(trajectory[t, 2] + b)
                ly = trajectory[t, 1] + r * np.sin(trajectory[t, 2] + b)

                if lid not in landmarks_est:
                    landmarks_est[lid] = np.array([lx, ly])
                else:
                    landmarks_est[lid] = 0.9 * landmarks_est[lid] + 0.1 * np.array([lx, ly])

            # 周期性修正
            if t > 0 and t % correction_interval == 0 and len(landmarks_est) > 3:
                # 使用地标观测修正位姿
                for r, b, lid in obs:
                    if lid in landmarks_est:
                        lm = landmarks_est[lid]
                        # 修正位姿
                        expected_x = lm[0] - r * np.cos(trajectory[t, 2] + b)
                        expected_y = lm[1] - r * np.sin(trajectory[t, 2] + b)
                        trajectory[t, 0] = 0.9 * trajectory[t, 0] + 0.1 * expected_x
                        trajectory[t, 1] = 0.9 * trajectory[t, 1] + 0.1 * expected_y

        return trajectory, landmarks_est

    # 运行各方法
    print(f"\n  地标数量: {n_landmarks}, 步数: {n_steps}")
    print(f"  轨迹半径: {radius}")

    # 仅里程计
    traj_odom = odometry_only(true_trajectory)
    errors_odom = np.linalg.norm(true_trajectory[:, :2] - traj_odom[:, :2], axis=1)

    # 滤波 SLAM
    traj_filter, lm_filter = filter_slam(true_trajectory, true_landmarks)
    errors_filter = np.linalg.norm(true_trajectory[:, :2] - traj_filter[:, :2], axis=1)

    # 优化 SLAM
    traj_opt, lm_opt = optimization_slam(true_trajectory, true_landmarks)
    errors_opt = np.linalg.norm(true_trajectory[:, :2] - traj_opt[:, :2], axis=1)

    # 打印结果
    print(f"\n  {'方法':>15} {'平均误差':>12} {'最大误差':>12} {'最终误差':>12}")
    print(f"  {'-'*15} {'-'*12} {'-'*12} {'-'*12}")

    print(f"  {'仅里程计':>15} {np.mean(errors_odom):>12.4f} {np.max(errors_odom):>12.4f} {errors_odom[-1]:>12.4f}")
    print(f"  {'滤波 SLAM':>15} {np.mean(errors_filter):>12.4f} {np.max(errors_filter):>12.4f} {errors_filter[-1]:>12.4f}")
    print(f"  {'优化 SLAM':>15} {np.mean(errors_opt):>12.4f} {np.max(errors_opt):>12.4f} {errors_opt[-1]:>12.4f}")

    # 地标估计精度
    print(f"\n  地标估计精度:")
    print(f"  {'方法':>15} {'平均误差':>12}")
    print(f"  {'-'*15} {'-'*12}")

    for name, lm_est in [("滤波 SLAM", lm_filter), ("优化 SLAM", lm_opt)]:
        errors = []
        for lid, lm in lm_est.items():
            error = np.linalg.norm(lm - true_landmarks[lid])
            errors.append(error)
        if errors:
            print(f"  {name:>15} {np.mean(errors):>12.4f}")


if __name__ == "__main__":
    run_slam_comparison()

附录：关键公式汇总

A.1 贝叶斯推理

公式	表达式
贝叶斯定理	$p(x
预测步	$\bar{p}(x_t) = \int p(x_t
更新步	$p(x_t) = \eta \cdot p(z_t

A.2 卡尔曼滤波

公式	表达式
预测	$\hat{x}_{t
预测协方差	$P_{t
卡尔曼增益	$K_t = P_{t
更新	$\hat{x}_{t
更新协方差	$P_{t

A.3 EKF

公式	表达式
非线性预测	$\hat{x}_{t
雅可比	$F_t = \frac{\partial f}{\partial x}\Big
非线性更新	$\hat{x}_{t

A.4 视觉几何

公式	表达式
对极约束	p2TFp1=0\mathbf{p}_2^T F \mathbf{p}_1 = 0p2TFp1=0
本质矩阵	E=KTFKE = K^T F KE=KTFK
光流方程	Ixu+Iyv+It=0I_x u + I_y v + I_t = 0Ixu+Iyv+It=0

A.5 SLAM 优化

公式	表达式
MAP 估计	X∗=arg⁡min⁡X∑i∣zi−hi(X)∣Σi−12X^* = \arg\min_X \sum_i \|z_i - h_i(X)\|_{\Sigma_i^{-1}}^2X∗=argminX∑i∣zi−hi(X)∣Σi−12
高斯-牛顿	(JTΣ−1J)ΔX=JTΣ−1r(J^T \Sigma^{-1} J) \Delta X = J^T \Sigma^{-1} r(JTΣ−1J)ΔX=JTΣ−1r
Schur 补	Hxx−HxmHmm−1HmxH_{xx} - H_{xm} H_{mm}^{-1} H_{mx}Hxx−HxmHmm−1Hmx

参考文献

Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
Barfoot, T. (2017). State Estimation for Robotics. Cambridge University Press.
Durrant-Whyte, H., & Bailey, T. (2006). Simultaneous localization and mapping: Part I/II. IEEE Robotics & Automation Magazine.
Mur-Artal, R., Montiel, J., & Tardós, J. (2015). ORB-SLAM: A versatile and accurate monocular SLAM system. IEEE TRO.
Engel, J., Schöps, T., & Cremers, D. (2014). LSD-SLAM: Large-scale direct monocular SLAM. ECCV.
Kaess, M., et al. (2012). iSAM2: Incremental smoothing and mapping using the Bayes tree. IJRR.
Dellaert, F., & Factor, G. (2017). Factor graphs and GTSAM: A graphical model approach to sensor fusion. FUSION.
DeTone, D., Malisiewicz, T., & Rabinovich, A. (2018). SuperPoint: Self-supervised interest point detection and description. CVPR Workshops.
Teed, Z., & Deng, J. (2021). DROID-SLAM: Deep visual SLAM for monocular, stereo, and RGB-D cameras. NeurIPS.
Sucar, E., et al. (2021). iMAP: Implicit mapping and positioning in real-time. ICCV.

机器人状态估计与 SLAM—概率推理到 simultaneous Localization and Mapping

目录

第一部分：状态估计基础理论

第一章：绪论------机器人为什么需要状态估计

1.1 机器人状态估计的核心问题

1.1.1 什么是状态？

1.1.2 为什么状态估计是困难的？

1.2 传感器与测量模型

1.2.1 常见传感器

1.2.2 测量模型

1.3 运动模型

1.3.1 离散时间运动模型

第二章：概率论与贝叶斯推理基础

2.1 概率论基础

2.1.1 概率分布

2.1.2 贝叶斯定理

2.1.3 高斯分布

2.2 贝叶斯推理在机器人中的应用

2.2.1 定位问题

2.2.2 SLAM 问题

第三章：递归贝叶斯估计------滤波理论

3.1 贝叶斯滤波

3.1.1 马尔可夫假设

3.1.2 贝叶斯滤波的递推公式

3.1.3 贝叶斯滤波的物理含义

3.2 最大后验估计与最小均方误差估计

3.2.1 最大后验估计（MAP）

3.2.2 最小均方误差估计（MMSE）

3.2.3 协方差矩阵的传播

第二部分：卡尔曼滤波族

第四章：线性卡尔曼滤波

4.1 线性系统模型

4.1.1 状态空间模型

4.1.2 卡尔曼滤波的假设

4.2 卡尔曼滤波算法

4.2.1 预测步

4.2.2 更新步

4.2.3 卡尔曼增益的直觉

4.3 卡尔曼滤波的信息论解释

4.3.1 信息形式

4.3.2 信息增益

第五章：扩展卡尔曼滤波（EKF）

5.1 非线性系统

5.1.1 非线性模型

5.1.2 EKF 的核心思想

5.2 EKF 算法

5.2.1 预测步

5.2.2 更新步

5.2.3 EKF 的局限

5.3 迭代扩展卡尔曼滤波（IEKF）

5.3.1 核心思想

第六章：无迹卡尔曼滤波（UKF）与粒子滤波

6.1 无迹卡尔曼滤波（UKF）

6.1.1 核心思想

6.1.2 Sigma 点的生成

6.1.3 UKF 算法

6.1.4 UKF vs EKF

6.2 粒子滤波

6.2.1 核心思想

6.2.2 粒子滤波算法

6.2.3 粒子滤波的理论性质

第三部分：SLAM 问题的形式化

第七章：SLAM 问题的数学定义

7.1 SLAM 的形式化

7.1.1 问题定义

7.1.2 SLAM 的概率图模型

7.1.3 SLAM 的两个子问题

7.2 SLAM 的分类

第八章：EKF-SLAM

8.1 EKF-SLAM 的状态表示

8.1.1 状态向量

8.1.2 协方差矩阵

8.2 EKF-SLAM 算法

8.2.1 预测步

8.2.2 更新步

8.3 EKF-SLAM 的复杂度分析

第九章：基于图的 SLAM 与因子图优化

9.1 图 SLAM 的形式化

9.1.1 因子图

9.1.2 因子的类型