想象你被蒙上眼睛，站在一座空无一物的仓库中央，任务是用一根拐杖找到所有承重柱并画出一张地图。你没有地图，也没有人告诉你起点坐标，只能做两件事：迈步（凭直觉判断自己走了多远、转了多少弯），以及用拐杖敲击（感知身边的柱子在多远、朝哪个方向）。你很快发现一个死结：要画柱子的位置，你得先知道自己在哪；要知道自己在哪，你又需要一张地图来参照。这就是 SLAM（同时定位与建图）的“先有鸡还是先有蛋”难题。EKF-SLAM 给出的解法优雅而朴素：把“我在哪”和“每根柱子在哪”写进同一本账本（状态向量），再配一张“误差表”（协方差矩阵）。你每迈一步，账本上的括注就变大一点；每敲到一根柱子，整本账本一起联动修订——发现自己偏了，柱子的坐标也会跟着修正。这本账本的记账规则，就是扩展卡尔曼滤波（EKF）的全部秘密。读完这篇文章，你会理解这套账本逻辑的每一步推导、它的局限性，以及为什么它只适合整理小仓库，而不是绘制整座城市。

岗位关联

职业价值：EKF-SLAM 是“感知与定位”方向的入门基石，理解其状态表示、观测模型与协方差逻辑，才能看懂现代激光/视觉 SLAM 的演进脉络，也是机器人算法岗位面试的高频核心考点。

目录
  1. 可视化演示
  2. 问题起源：先有鸡还是先有蛋
  3. 状态表示：一本合账的账本
  4. 运动模型：每一步都偷偷放大误差
  5. 观测模型：拐杖敲出的距离与方位
  6. 数据关联：这根柱子是哪一根
  7. EKF更新：整本账本一起联动修订
  8. 一致性挑战：越走越“自信”的陷阱
  9. 实践权衡：小仓库里的账本之王
  10. AI 应用场景
  11. Python 演示
  12. 常见误区
  13. 面试题

可视化演示

模拟机器人EKF-SLAM建图过程，展示路标观测与协方差不确定性演化及数据关联匹配。

本节配有交互动效（进入视口后自动加载，约 3KB）。

模式1：EKF-SLAM建图过程 机器人沿轨迹运动并观测路标 新路标加入状态向量后不确定性传播 x = [x_r, y_r, θ_r, m_1, ..., m_n] 状态向量包含位姿与所有路标位置。

问题起源：先有鸡还是先有蛋

蒙眼人站在仓库中央，第一反应是找参照物。可四周除了形状相似的柱子，没有任何坐标标记。他想要画地图，得先知道自己站在哪里；想知道自己站在哪里，又得先有一张地图来对照。这个死结完美复刻了 SLAM 的困境：机器人在未知环境中，定位需要地图，建图需要定位，两个问题互为前提，单独解任何一个都无解。

好在机器人的运动是一个时间过程，不必一次性解开死结。从起点出发，每一步都可以凭控制指令推算自己的位置变化；每遇到一个路标，又可以借观测反推自己与路标的关系。把“推算”和“修正”交替进行几百次，地图和位姿就会一同从模糊走向清晰——这就是把死结变成递推的思想。

递推求解的数学框架是贝叶斯滤波：用一个不断演化的状态描述机器人及地图，用两条方程刻画全部规律——一条描述状态如何随控制指令变化，一条描述传感器如何“看到”状态：

蒙眼人的破局点就在这里。他放弃“一次算准”的幻想，改为“走一步、敲一棒、更正一次”：运动方程把昨天的估计推到今天，观测方程把今天的感知拉回真相。死结没有消失，但在递推中被温柔地解开了。

{xk=f(xk−1,uk)+wkzk=h(xk)+vk\displaystyle\begin{cases}\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k \\[8pt]\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k\end{cases}
第一条是“运动方程”，第二条是“观测方程”。$f$ 与 $h$ 都是非线性函数，EKF 中的 "E"（Extended）正是为处理这种非线性而引入的线性化手段。
  1. 状态演化方程$\mathbf{x}_{k-1}$ 是上一时刻“机器人+地图”的完整状态，经过运动函数 $f$ 与当前控制输入 $\mathbf{u}_k$ 的驱动，演化为 $\mathbf{x}_k$
    xk=f(xk−1,uk)+wk\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k
  2. 控制输入$\mathbf{u}_k$ 是机器人收到的速度、转向等指令，相当于你决定迈出的每一步的方向与大小
  3. 过程噪声$\mathbf{w}_k$ 是执行指令时的随机误差，代表“你以为走了 1 米，实际走了 0.95 米或 1.05 米”
  4. 观测方程$\mathbf{z}_k$ 是传感器读数，由观测函数 $h$ 将状态映射为“拐杖敲出的距离和方位”，再叠加测量噪声 $\mathbf{v}_k$

状态表示：一本合账的账本

蒙眼人摸索一阵后，最怕记忆混乱。聪明的做法是随身带一本账本：第一行记“我现在大概在哪、面朝哪”，后面每一行记“1 号柱大概在哪”“2 号柱大概在哪”，每行还附一个括注——“我对这个数有多确定”。EKF-SLAM 的状态表示，与这本账本如出一辙。

在数学上，账本内容就是一个增广状态向量：前面 3 个分量是机器人位姿，后面每 2 个分量是一个路标的坐标。路标数量记为 $n$，于是状态维度是 $3 + 2n$，地图上每多发现一根柱子，账本就多两行：

除了均值，账本还配了一张“误差表”——协方差矩阵 $\mathbf{P}$。对角线记录每个数字的方差（括注大小），非对角线记录两两之间的相关性。相关性是全书最关键的设定：正因为“我的位置”与“1 号柱的位置”高度相关，当我发现自己偏左 2 米时，才能顺手把 1 号柱也往左挪 2 米。

这本合账让“我在哪”和“柱子在哪”不再是两本独立的笔记，而是同一本账的两个科目。任何一个科目被修订，另一个自动联动——这便是 EKF-SLAM 一切魔法的源头。

x=[x,  y,  θ⏟机器人位姿,  m1x,  m1y,  m2x,  m2y,  …,  mnx,  mny⏟全部 n 个路标的坐标]T\displaystyle\mathbf{x} = [\underbrace{x,\; y,\; \theta}_{\text{机器人位姿}},\; \underbrace{m_{1x},\; m_{1y},\; m_{2x},\; m_{2y},\; \ldots,\; m_{nx},\; m_{ny}}_{\text{全部 } n \text{ 个路标的坐标}}]^{T}
$x$、$y$ 是机器人位置，$\theta$ 是朝向角；每个路标用 $(m_{ix}, m_{iy})$ 表示其在同一世界坐标系下的坐标。所有数字拼成一个列向量，就是“账本”的正文。
  1. 位姿三项$x$、$y$ 是机器人在平面坐标系中的位置，$\theta$ 是朝向角，三者共同回答“我在哪、面朝哪”
    xpose=[x,  y,  θ]\mathbf{x}_{pose} = [x,\; y,\; \theta]
  2. 路标坐标对第 $i$ 个路标用 $(m_{ix}, m_{iy})$ 表示其位置，$i = 1, 2, \ldots, n$
    mi=[mix,  miy]\mathbf{m}_i = [m_{ix},\; m_{iy}]
  3. 增广拼接位姿与全部路标拼成一个 $3 + 2n$ 维列向量，决定了账本的“行数”与协方差矩阵的尺寸

运动模型：每一步都偷偷放大误差

蒙眼走路时，你对自己位置的判断会越来越含糊。走第一步，你还比较确定；走到第十步，你已经分不清是歪向左还是偏向右。每迈一步，新的误差叠加进来，账本上“我在哪”那行的括注就变大一点，这就是运动模型要描述的“不确定性增长”。

运动模型回答“不敲柱子时，我该怎样推算下一个状态”。给定控制输入 $\mathbf{u}_k$（前进速度、转向角）和时间步 $\Delta t$，预测状态由上一步状态经函数 $f$ 映射而来；同时，协方差也要被推进一步，记法 $k|k-1$ 表示“用 $k-1$ 时刻的信息预测 $k$ 时刻”：

蒙眼人每迈一步，账本上的括注就涨一圈。这个过程不需要任何外界信息，纯粹是对自己判断的诚实评估：不知道路在哪里，就坦然承认越来越不确定。

x^k∣k−1=f(x^k−1∣k−1,  uk)\displaystyle\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1},\; \mathbf{u}_k)
Pk∣k−1=FkPk−1∣k−1FkT+Qk\displaystyle\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^{T} + \mathbf{Q}_k
第一条是状态的先验预测，第二条是协方差的先验传播。$\mathbf{F}_k$ 是运动函数的雅可比矩阵，$\mathbf{Q}_k$ 是过程噪声协方差。预测步永远不会让账本变得更确定。
  1. 状态预测$\hat{\mathbf{x}}_{k|k-1}$ 是只凭运动模型得到的先验估计，没有利用任何传感器信息
    x^k∣k−1=f(x^k−1∣k−1,  uk)\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1},\; \mathbf{u}_k)
  2. 雅可比矩阵$\mathbf{F}_k$ 是 $f$ 在当前估计点的导数，回答“某个分量动一点，其他分量跟着动多少”，是线性化的关键
    Fk=∂f∂x∣x^k−1∣k−1\mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1|k-1}}
  3. 协方差传播$\mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^{T}$ 把旧的不确定度“摊”到新状态上，类比内心那条误差括注随步伐被拉伸
    Pk∣k−1=FkPk−1∣k−1FkT+Qk\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^{T} + \mathbf{Q}_k
  4. 过程噪声叠加$\mathbf{Q}_k$ 是本步新引入的随机误差，它保证协方差只会增大不会减小——没观测到任何柱子前，自信不该增强

观测模型：拐杖敲出的距离与方位

蒙眼人用拐杖敲到一根柱子时，得到两个数字：柱子在多远（距离），以及在我哪个方向（方位角）。但拐杖会有一点点滑动，耳朵判断方位也有偏差——这两个数字天生带着噪声。观测模型要描述的，正是这条“带噪声的感官通道”。

观测模型把状态（机器人位姿 + 路标坐标）映射为传感器读数。对经典的距离-方位传感器，映射是非线性的：距离需要开根号，方位需要反三角函数：

更新步还需要观测函数对状态的导数——观测雅可比 $\mathbf{H}_k$。它回答：“若我把机器人位置或路标位置挪动一点点，距离和方位会变多少？”这个矩阵是“敲击”与“账本”之间的桥梁，也是 EKF 名字里 "Extended" 的落点。

拐杖敲到的两个数虽然带噪声，但账本记得“敲击与我、与柱子的关系”，于是这一下敲击就足以同时修正两行账——这正是下一步要讲的关键机制。

zk=[(mx−x)2+(my−y)2atan2(my−y,  mx−x)−θ]⏟非线性观测函数 h(x)+vk\displaystyle\mathbf{z}_k = \underbrace{\begin{bmatrix} \sqrt{(m_x - x)^2 + (m_y - y)^2} \\[6pt] \text{atan2}(m_y - y,\; m_x - x) - \theta \end{bmatrix}}_{\text{非线性观测函数 } h(\mathbf{x})} + \mathbf{v}_k
观测噪声 $\mathbf{v}_k$ 的协方差记为 $\mathbf{R}_k$。激光雷达、视觉特征点、UWB 锚点都可以套进这个“距离+方位”框架，只是 $\mathbf{R}_k$ 的大小不同。
  1. 距离分量$\sqrt{(m_x - x)^2 + (m_y - y)^2}$ 计算机器人到路标的直线距离，对应拐杖敲到柱子的长度
    r=(mx−x)2+(my−y)2r = \sqrt{(m_x - x)^2 + (m_y - y)^2}
  2. 方位角分量$\text{atan2}(m_y - y,\; m_x - x)$ 先算路标在世界系下的方向角，再减去机器人朝向 $\theta$，得到“柱子在我身体坐标系的哪一侧”
    ϕ=atan2(my−y,  mx−x)−θ\phi = \text{atan2}(m_y - y,\; m_x - x) - \theta
  3. 观测噪声$\mathbf{v}_k$ 是传感器读数与真值的偏差，其大小由 $\mathbf{R}_k$ 刻画——拐杖越钝、手越抖，噪声越大
    vk∼N(0,  Rk)\mathbf{v}_k \sim \mathcal{N}(0,\; \mathbf{R}_k)

数据关联：这根柱子是哪一根

仓库里十几根柱子长得一模一样。蒙眼人敲到一根，必须先回答：这是 1 号柱，还是新发现的一根？如果认错了，把 3 号柱的观测记到 5 号柱的账本里，整本账都会乱套。数据关联就是“对号入座”。

传感器一次可能输出多个观测，每个观测需要与已有路标（或“新路标”）配对。最经典的方法是最近邻：用马氏距离衡量观测与每个路标预测之间的差异，落在门控阈值内的最近者胜出：

关联错误的危害极大：滤波器会把错误路标的高置信修正注入状态，且几乎无法靠后续观测自动纠正。因此工程中常采用“门控 + 最近邻 + 拒绝可疑观测”的组合策略，宁可漏配，不可错配。

蒙眼人之所以能维持一张可信地图，不是因为他认路神准，而是每敲一根柱子都先愣一下：“你是哪根？”认不准时，宁可多敲几次，也不随便记账。

\displaystyled_{ij}^{2} = (\mathbf{z}_i - \hat{\mathbf{z}}_j)^{T} \mathbf{S}_j^{-1} (\mathbf{z}_i - \hat{\mathbf{z}}_j) < \chi_{\text{dof},\alpha}^{2}
$d_{ij}^{2}$ 是观测 $i$ 与路标 $j$ 之间的马氏距离平方。与欧氏距离不同，它用协方差 $\mathbf{S}_j$ 做了“单位归一”——噪声大的方向自动降权。
  1. 新息向量$\mathbf{z}_i - \hat{\mathbf{z}}_j$ 是第 $i$ 个观测与第 $j$ 个路标预测观测之差，代表“现实与预期的偏离”
    νij=zi−z^j\boldsymbol{\nu}_{ij} = \mathbf{z}_i - \hat{\mathbf{z}}_j
  2. 新息协方差$\mathbf{S}_j = \mathbf{H}_j \mathbf{P} \mathbf{H}_j^{T} + \mathbf{R}$ 刻画这个偏离在多大范围内属于“正常噪声”
    Sj=HjPHjT+R\mathbf{S}_j = \mathbf{H}_j \mathbf{P} \mathbf{H}_j^{T} + \mathbf{R}
  3. 马氏距离用 $\mathbf{S}_j^{-1}$ 加权后再平方，本质是“以不确定度为尺子量距离”
    dij2=νijTSj−1νijd_{ij}^{2} = \boldsymbol{\nu}_{ij}^{T} \mathbf{S}_j^{-1} \boldsymbol{\nu}_{ij}
  4. 门控判定与卡方分布分位数 $\chi_{\text{dof},\alpha}^{2}$ 比较，小于阈值才接受配对；否则视为新路标或直接丢弃
    dij2<χdof,α2d_{ij}^{2} < \chi_{\text{dof},\alpha}^{2}

EKF更新：整本账本一起联动修订

蒙眼人敲到柱子，发现“咦，离我只有 2 米，账本上却写着 3 米”。这一米的差距，既可能来自“我在哪”记错了，也可能来自“柱子在哪”记错了，还可能纯粹是拐杖量短了。EKF 的做法是：按三者不确定度的比例分摊修正——越不确定的一方，分到的修正越多。

这就是扩展卡尔曼滤波的更新步：先用预测观测与真实观测做差（新息），再乘上卡尔曼增益回填状态。关键在于 $\mathbf{K}_k$ 不是按维度和路标分别计算的，而是由整个协方差矩阵决定，因而一次更新会同时修正位姿与所有路标：

关键在第 3 步的分摊：因为你与 1 号柱的位置高度相关，你发现“我偏了”的同时，柱子的坐标也必须一起挪。整本账不是一行一行改，而是联动修订——这正是 EKF-SLAM 被称为“联合估计”的原因。

Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1\displaystyle\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^{T} \big(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{T} + \mathbf{R}_k\big)^{-1}
x^k∣k=x^k∣k−1+Kk(zk−h(x^k∣k−1))\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \big(\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})\big)
Pk∣k=(I−KkHk)Pk∣k−1\mathbf{P}_{k|k} = \big(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\big) \mathbf{P}_{k|k-1}
第一步算增益，第二步修状态，第三步缩协方差。三行连招完成一次“观测-修正”闭环。$\mathbf{I}$ 是单位矩阵。
  1. 增益计算$\mathbf{K}_k$ 由预测不确定度 $\mathbf{P}_{k|k-1}$ 与观测不确定度 $\mathbf{R}_k$ 的比值决定；预测越不自信、观测越可靠，$\mathbf{K}_k$ 越大
    Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^{T} \big(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^{T} + \mathbf{R}_k\big)^{-1}
  2. 新息回填$\boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$ 是“现实减去预期”，正负号决定修正方向，大小决定修正幅度
    νk=zk−h(x^k∣k−1)\boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})
  3. 状态修正$\hat{\mathbf{x}}_{k|k}$ 是修正后的后验估计；由于 $\mathbf{K}_k$ 内含交叉协方差，这一步会让位姿与所有路标联动移动
    x^k∣k=x^k∣k−1+Kkνk\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \boldsymbol{\nu}_k
  4. 协方差收缩$(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)$ 作为收缩因子使 $\mathbf{P}_{k|k}$ 变小——观测之后，账本的括注更窄了
    Pk∣k=(I−KkHk)Pk∣k−1\mathbf{P}_{k|k} = \big(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\big) \mathbf{P}_{k|k-1}

一致性挑战：越走越“自信”的陷阱

蒙眼人反复敲同一根柱子很多次，账本上的括注越写越小，他越来越确信自己的地图。但这份自信可能是一场幻觉——因为他是用“直线”在脑海里拟合每段弧形轨迹的。误差虽小，却一直在偷偷累积，而账本却毫不知情。

一致性（consistency）衡量滤波器给出的不确定度是否与实际误差相符。EKF-SLAM 的核心隐患在于：用一阶泰勒展开把非线性函数线性化，丢弃了高阶项，导致协方差被系统性低估：

蒙眼人最大的危险不是“不确定”，而是“假确定”。他把走廊当直线走了 50 米，账本还自信地记着“误差 0.1 米”，直到一头撞上墙。经典 EKF-SLAM 在中小规模环境表现良好，但地图一大、非线性一强，这份虚假自信就会吞噬整个地图。

\displaystyleh(\mathbf{x}) \approx h(\hat{\mathbf{x}}) + \mathbf{H}\,(\mathbf{x} - \hat{\mathbf{x}}) \quad\Longrightarrow\quad \mathbf{P}_{\text{EKF}} < \mathbf{P}_{\text{true}}
$\mathbf{P}_{\text{EKF}} < \mathbf{P}_{\text{true}}$ 表示滤波器认为自己非常确定，但真实误差并没有那么小。这份“虚假自信”会让后续增益严重偏向模型、轻视观测，最终地图发散。
  1. 一阶泰勒展开$h$ 只保留常数项与一阶导数项 $\mathbf{H}(\mathbf{x} - \hat{\mathbf{x}})$，相当于用切线代替曲线
    h(x)≈h(x^)+H (x−x^)h(\mathbf{x}) \approx h(\hat{\mathbf{x}}) + \mathbf{H}\,(\mathbf{x} - \hat{\mathbf{x}})
  2. 截断高阶项二阶及以上项被丢弃，误以为“切线等于曲线”；在近距离路标、大转角等强非线性处，误差尤其明显
    截断项：12(x−x^)T∇2h (x−x^)+⋯\text{截断项：} \tfrac{1}{2}(\mathbf{x} - \hat{\mathbf{x}})^{T} \nabla^{2} h\,(\mathbf{x} - \hat{\mathbf{x}}) + \cdots
  3. 协方差低估线性化误差以“未知”的方式混入递推，使滤波器协方差系统性小于真实误差
    PEKF<Ptrue\mathbf{P}_{\text{EKF}} < \mathbf{P}_{\text{true}}
  4. 反馈恶化被低估的协方差导致 $\mathbf{K}_k$ 偏小，后续观测被“无视”，状态逐步漂移而滤波器毫不自知
    Kk→偏小  ⇒  x^k∣k→偏旧\mathbf{K}_k \to \text{偏小} \;\Rightarrow\; \hat{\mathbf{x}}_{k|k} \to \text{偏旧}

实践权衡：小仓库里的账本之王

蒙眼整理一个 100 平方米的小仓库，账本完全够用；但如果让他蒙着眼跑遍一座城市，每根电线杆都记进账本，别说走路，光翻页就会把他累垮。EKF-SLAM 的适用边界，就是账本翻页的成本。

EKF-SLAM 每次更新都要操作整个协方差矩阵。状态维度 $3 + 2n$ 意味着协方差矩阵是 $(3+2n) \times (3+2n)$ 的方阵，一次观测更新涉及矩阵求逆，整体复杂度为：

工程上的典型做法是：在几十个路标的小场景（扫地机器人、小型仓储 AGV）直接用 EKF-SLAM；当地图规模变大，改用 FastSLAM（粒子滤波分摊路标）、图优化（GraphSLAM / ORB-SLAM 等）方案。EKF-SLAM 的价值不在于“最能打”，而在于它是最直观、可解释性最强的基线算法——理解它，才能理解后来者究竟解决了什么问题。

小仓库里，EKF-SLAM 是那个账本记得又快又准的蒙眼管理员；一旦换到大城市，他依然认真，但翻页的沙沙声已经盖过了脚步声。选哪种算法，本质是选一本配得上场景大小的账本。

\displaystyleT_k = O(n^{2})
$n$ 是路标数量。路标从 50 个涨到 500 个，单次更新的计算量放大约 100 倍——这是 EKF-SLAM 只适合小规模场景的根本原因。
  1. 路标规模$n$ 为地图中路标总数，每个路标贡献 2 个状态维度
    状态维度=3+2n\text{状态维度} = 3 + 2n
  2. 复杂度来源协方差传播与更新需要操作 $(3+2n) \times (3+2n)$ 数量级的矩阵元素，并伴随矩阵求逆
    P∈R(3+2n)×(3+2n)\mathbf{P} \in \mathbb{R}^{(3+2n) \times (3+2n)}
  3. 平方增长$O(n^2)$ 意味着路标翻倍、耗时约翻 4 倍，这就是“翻账本”的代价
    T(2n)≈4⋅T(n)T(2n) \approx 4 \cdot T(n)

AI 应用场景

扫地机器人的“云端换新房”建图

阶段：感知与定位（在线建图）

解决的问题：传统预置地图方案在用户搬家或家具大挪移后迅速失效，机器人“迷路”且无法自愈。EKF-SLAM 让机器人在清扫过程中同步更新地图，路标变化时账本自动联动修订，无需人工重新测绘。

具体案例：一台搭载单线激光雷达的扫地机器人进入陌生客厅（约 60 平方米），先绕场一圈建立 20 余个墙角和家具腿组成的路标地图，随即开始“弓”字形清扫。清扫中它一边用里程计推算自身位置（运动模型），一边用雷达敲击路标（观测模型），位姿估计误差稳定在 10 厘米以内。

楼宇配送机器人的小场景导航

阶段：感知 → 规划 → 控制

解决的问题：酒店、写字楼内部走廊环境路标有限且结构稳定，正是 EKF-SLAM 计算复杂度的“舒适区”。传统全局地图方案部署成本高，EKF-SLAM 无需预先建模即可投入使用。

具体案例：某园区酒店配送机器人负责 800 平方米大堂与走廊的送物任务，借助 20 余个人工地标点（电梯口、前台、转角）完成建图与定位。每次巡航 15 分钟无需人工重定位，遇到临时摆放的行李车时，通过马氏距离门控及时拒绝干扰观测，避免数据关联错误。

自动驾驶小车的传感器融合验证

阶段：多传感器融合（算法基线）

解决的问题：在开发更复杂的图优化 SLAM 之前，团队需要一个可解释、易调参的基线算法来验证 IMU、里程计与激光雷达的融合效果。EKF-SLAM 提供了清晰的“预测-更新”接口，方便逐个传感器接入。

具体案例：高校自动驾驶小车的课程实验中，团队先用 EKF-SLAM 在半径 100 米的封闭园区完成建图，观察协方差收敛曲线与数据关联正确率，验证传感器噪声参数是否标定准确；确认基线可靠后，再切换到图优化方案处理更大范围场景。

Python 演示

python81 行
import numpy as npimport math # 三个真实路标的位置(米)LM_TRUE = np.array([[4.0, 2.0], [8.0, 6.0], [2.0, 7.0]])N = len(LM_TRUE)DIM = 3 + 2 * N  # 状态维度: 机器人(x,y,theta) + N个路标(x,y) # 初始化状态: 位姿在原点, 路标初值带先验偏差state = np.zeros(DIM)state[3:] = LM_TRUE.flatten() + np.random.normal(0, 1.2, 2 * N)cov = np.eye(DIM) * 0.01cov[3:, 3:] = np.eye(2 * N) * 3.0  # 路标初始不确定度较大 def move(st, u, dt):    # 运动模型: 前进v、转向w, 更新(x,y,theta)    v, w = u    st = st.copy()    st[0] += v * dt * math.cos(st[2])    st[1] += v * dt * math.sin(st[2])    st[2] += w * dt    return st def F_jacobian(st, u, dt):    # 运动雅可比: 状态分量之间的相互影响    v, w = u    F = np.eye(DIM)    F[0, 2] = -v * dt * math.sin(st[2])    F[1, 2] = v * dt * math.cos(st[2])    return F def sense(st, idx):    # 观测模型: 测量到第idx个路标的距离和方位    mx, my = st[3 + 2 * idx], st[4 + 2 * idx]    r = math.hypot(mx - st[0], my - st[1])    bearing = math.atan2(my - st[1], mx - st[0]) - st[2]    return np.array([r, bearing]) def H_jacobian(st, idx):    # 观测雅可比: 距离/方位对状态各分量的偏导    mx, my = st[3 + 2 * idx], st[4 + 2 * idx]    dx, dy = mx - st[0], my - st[1]    r2 = dx * dx + dy * dy    r = math.sqrt(r2)    H = np.zeros((2, DIM))    H[0, 0] = -dx / r;  H[0, 1] = -dy / r    H[0, 3 + 2 * idx] = dx / r;  H[0, 4 + 2 * idx] = dy / r    H[1, 0] = dy / r2;  H[1, 1] = -dx / r2;  H[1, 2] = -1.0    H[1, 3 + 2 * idx] = -dy / r2;  H[1, 4 + 2 * idx] = dx / r2    return H Q = np.zeros((DIM, DIM))Q[:3, :3] = np.diag([0.02, 0.02, 0.01])  # 运动过程噪声R = np.diag([0.05, 0.03])                # 观测噪声 truth = np.zeros(3)dt = 0.5np.random.seed(42) for k in range(40):    u = np.array([1.0, 0.2])  # 前进1m/s, 转向0.2rad/s    truth = move(truth, u, dt)    # 预测步: 状态推进 + 协方差传播    F = F_jacobian(state, u, dt)    state = move(state, u, dt)    cov = F @ cov @ F.T + Q    # 更新步: 依次观测每个路标(数据关联已知)    for idx in range(N):        z = sense(truth, idx) + np.random.normal(0, 0.1, 2)        z_pred = sense(state, idx)        H = H_jacobian(state, idx)        S = H @ cov @ H.T + R        K = cov @ H.T @ np.linalg.inv(S)        innov = z - z_pred        innov[1] = math.atan2(math.sin(innov[1]), math.cos(innov[1]))        state = state + K @ innov        cov = (np.eye(DIM) - K @ H) @ cov    if k % 10 == 0:        pose_err = np.linalg.norm(state[:3] - truth)        lm_err = np.linalg.norm(state[3:] - LM_TRUE.flatten())        print(f"step {k:2d}: pose_err={pose_err:.3f}m  landmark_err={lm_err:.3f}m")
目的：用 60 行代码实现一个微型 EKF-SLAM，模拟机器人边运动边观测三个路标，观察位姿与路标估计误差随“预测-更新”迭代逐步收敛。
  1. state[3:] = LM_TRUE.flatten() + np.random.normal(0, 1.2, 2 * N)给路标初值加先验偏差，模拟“地图还没建好”的初始状态
  2. F[0, 2] = -v * dt * math.sin(st[2])运动雅可比的关键项：朝向角变化如何影响 x 方向位移，线性化的落点
  3. cov = F @ cov @ F.T + Q预测步的协方差传播，账本括注被拉伸并叠加上新噪声
  4. z = sense(truth, idx) + np.random.normal(0, 0.1, 2)在真实量测上加噪声，模拟“拐杖量测天生带误差”
  5. K = cov @ H.T @ np.linalg.inv(S)卡尔曼增益：按预测与观测的不确定度之比分配信任
  6. state = state + K @ innov一步修正，让位姿与全部路标联动移动
  7. cov = (np.eye(DIM) - K @ H) @ cov协方差收缩，观测之后账本更加确定

常见误区

误区我觉得 SLAM 很简单，不就是跑两个独立的卡尔曼滤波器吗？一个估计机器人位姿，一个估计路标位置，最后拼起来就是 EKF-SLAM 了。

正解这正是最常见的误解。EKF-SLAM 的精髓是把位姿与所有路标放进同一个状态向量，用协方差矩阵的交叉项记录“它们之间的相关关系”。正因为相关，观测路标时才能同时修正位姿。如果分开估计，位姿误差会直接“烧”进地图，地图误差又反过来污染位姿，系统必然发散。

为何常见：直觉上“分而治之”很自然，且很多入门资料只讲一维卡尔曼滤波，让人以为多估几个量就是并列跑几个滤波器。

误区匹配错了就错了呗，反正后面还有新的观测，误差慢慢就会被滤平，不用太担心。

正解错误关联会把“其实是新路标”的观测当成“旧路标”去修正，等于把自己的账记到别人头上。卡尔曼增益越大，一次性修正幅度越大，地图可能瞬间被拉歪，且之后很难自我修复。数据关联错误是 EKF-SLAM 发散的常见元凶，工程中必须加马氏距离门控，甚至拒绝可疑观测。

为何常见：卡尔曼滤波“均值会收敛”的直觉太强，忽略了关联是离散决策而非连续量，无法靠后续平均抹平。

误区机器人转的角度不大，泰勒展开取一阶就够了，丢掉的高阶项肯定没什么影响。

正解一阶线性化在每个时刻都会截断高阶项，而 SLAM 是递推系统，误差会沿时间累积。更麻烦的是，线性化还会让滤波器系统性低估协方差，产生“虚假自信”——地图已经漂移，报表却显示方差极小。这就是 EKF-SLAM 不一致问题的根源。

为何常见：微积分里一阶近似在局部确实好用，但递推系统里局部误差会全局累积，初学者容易忽视这一点。

误区地图里放 1000 个路标，信息量充足，精度肯定会更高，为什么不呢？

正解每个路标给状态向量加 2 维，协方差矩阵按平方增长，每次更新要处理 $O(n^2)$ 的元素。路标从 100 涨到 1000，计算量放大约 100 倍；且大规模下非线性误差累积更快，一致性更差。EKF-SLAM 适合几十个路标的小场景，大规模建图应改用图优化或粒子滤波方案。

为何常见：“信息越多越好”的直觉在感知领域普遍成立，但 SLAM 受计算复杂度与一致性双重约束，需要权衡取舍。

误区反正均值是高斯分布的峰值，协方差大点小点，最终估出的路标位置不会有变化。

正解协方差直接决定卡尔曼增益 $\mathbf{K}_k$ 的分配：预测不确定度大、观测更可靠，$\mathbf{K}_k$ 就大，测量被充分采纳；反之 $\mathbf{K}_k$ 就小。协方差一旦估计错误，修正量随即错误，均值估计必然跟着偏。协方差与均值是“账本”的一体两面，缺一不可。

为何常见：讲解高斯分布时总强调“均值是中心”，协方差被当成配角，忽略了递推滤波中两者相互驱动、相互制约。

面试题

Q1. EKF-SLAM 的核心难题是什么？

单选题 · easy

  • A.传感器采购成本过高
  • B.定位与建图互相依赖← 正确答案
  • C.地图数据存储代价大
  • D.机器人移动速度过快

答案：B。SLAM 的核心难点正是定位与建图互为前提：定位需要地图作参照，建图需要位置来标定。传感器价格、数据存储、移动速度都是工程问题而非本质难题。EKF-SLAM 的整套递推框架正是围绕打破这个死结设计的。

Q2. EKF-SLAM 的状态向量通常包含什么？

单选题 · easy

  • A.只保存机器人的位置
  • B.只保存路标的三维坐标
  • C.机器人位姿与所有路标坐标← 正确答案
  • D.地面纹理图像与高度数据

答案：C。EKF-SLAM 把机器人位姿 $(x, y, \theta)$ 与所有路标坐标放进同一个增广状态向量，这是“联调修正”的前提。只保存机器人或只保存路标都丢失了另一半信息，地面纹理不在状态向量中。

Q3. 运动模型完成预测后，协方差如何变化？

单选题 · mid

  • A.协方差保持不变
  • B.协方差逐渐变大← 正确答案
  • C.协方差逐渐变小
  • D.协方差直接清零

答案：B。预测步中，旧不确定度经 $\mathbf{F}_k$ 传播后只会被拉伸，同时叠加过程噪声 $\mathbf{Q}_k$，所以协方差只会变大。这正对应“蒙眼越走越不确定”的直觉；只有观测更新才会让协方差缩小。

Q4. 数据关联选用马氏距离的优势是什么？

单选题 · mid

  • A.能计算各维度的差异
  • B.引入不确定度的加权← 正确答案
  • C.不需要路标坐标信息
  • D.计算结果永远是整数

答案：B。马氏距离用新息协方差做加权，等于“以不确定度为尺子量差异”，噪声大的方向自动降权。欧氏距离把各维度等同看待，结果也不是整数；马氏距离仍需要路标坐标信息，因此 A、C、D 均不正确。

Q5. 卡尔曼增益 K 在更新中的作用是什么？

单选题 · mid

  • A.决定路标的显示颜色
  • B.权衡预测与观测的权重← 正确答案
  • C.控制机器人的电机转速
  • D.压缩地图数据的体积

答案：B。$\mathbf{K}_k$ 由预测不确定度与观测不确定度的比值决定，本质是给“预测”和“观测”分配信任权重。它属于滤波器的数学核心，与显示颜色、电机控制、数据压缩无关。

Q6. EKF-SLAM 一致性变差的主要原因是什么？

单选题 · hard

  • A.传感器分辨率过高
  • B.雅可比线性化误差累积← 正确答案
  • C.路标数量设置太少
  • D.地图特征提取强度不够

答案：B。EKF 用一阶泰勒展开线性化非线性函数，截断高阶项造成误差沿递推累积，并系统性低估协方差，导致“过度自信”，这是一致性变差的根源。传感器精度、路标数量、特征提取都不是算法级的根本原因。

Q7. EKF-SLAM 的协方差更新复杂度是多少？

单选题 · hard

  • A.线性复杂度 O(n)
  • B.平方复杂度 O(n²)← 正确答案
  • C.对数复杂度 O(logn)
  • D.常数复杂度 O(1)

答案：B。状态维度为 $3 + 2n$，协方差矩阵尺寸是 $(3+2n)^2$，每次更新涉及矩阵乘法和求逆，整体复杂度为 $O(n^2)$。路标数翻倍，计算量约翻 4 倍，这是 EKF-SLAM 只适用于小规模场景的直接原因。

Q8. 在“距离+方位”观测模型中，可直接感知的信息有哪些？

多选题 · mid

  • A.机器人到路标距离← 正确答案
  • B.路标相对机器人方位角← 正确答案
  • C.路标的全局坐标位置
  • D.路标的表面纹理颜色

答案：AB。距离-方位观测模型直接测量的是到路标的距离 $r$ 和相对方位角 $\phi$。路标全局坐标是通过滤波递推估计出来的，属于输出而非观测；纹理颜色不在该观测框架内。

Q9. 关于 EKF-SLAM 的一致性问题，下列说法正确的是？

多选题 · extreme

  • A.线性化截断误差是主要根源← 正确答案
  • B.协方差被低估导致过度自信← 正确答案
  • C.重复观测同一路标能根治发散
  • D.换成欧氏距离关联即可解决

答案：AB。一致性问题源于一阶线性化截断导致的协方差低估，进而表现为过度自信，A、B 正确。重复观测同一路标无法消除线性化误差，C 错误；数据关联采用何种距离解决的是配对问题，与一致性无关，D 错误。

Q10. 数据关联错误可能造成哪些后果？

多选题 · easy

  • A.地图中出现虚假路标← 正确答案
  • B.滤波器估计快速发散← 正确答案
  • C.机器人位姿估计偏移← 正确答案
  • D.计算速度大幅提升

答案：ABC。错误关联会把观测记到错误路标名下，导致地图中混入虚假路标、位姿修正被带偏，严重时滤波器发散。错误关联只会让系统更慢地收敛甚至崩溃，绝无可能提升计算速度，D 是干扰项。