Part I · 第 4 章 概率 · 4.8

卡尔曼滤波与 EKF

轮式里程计会慢慢漂走，GPS 又抖又跳，单靠哪一个都不靠谱。卡尔曼滤波 (Kalman filter) 用一团高斯分布表示"我大概在哪"：预测时把这团信念挪过去、再吹胖一点；更新时把它和测量相乘，得到一团更瘦、更可信的信念。把线性模型换成雅可比矩阵，它就成了机器人状态估计里的老黄牛：扩展卡尔曼滤波 (EKF)。

约 35 分钟 前置：4.4 高斯分布 · 4.5 不确定性传播 · 4.7 贝叶斯滤波 · 3.3 雅可比（EKF 部分） 5 个交互

    1为什么机器人需要它

    一辆送货小车在园区里跑。它有两个信息来源：轮子上的编码器（里程计）告诉它"我刚才往前走了 0.52 米、左转了 3°"；GPS 每秒报一次"你在 (x, y)"。

    里程计短期内又准又平滑，可每一步的小误差都会累积：轮子打滑一下、地面颠一下，十分钟后它可能以为自己在隔壁楼里。GPS 不会累积误差，但每次读数都在真值附近乱跳好几米，进了楼还会丢信号。

    一个"短期可信、长期漂移"，一个"长期可信、短期乱跳"。卡尔曼滤波按各自的可信程度把两者融合起来；在线性、高斯的假设下，它给出的估计是最优的。无人机飞控、自动驾驶定位、手机里 IMU 与 GPS 的融合、SLAM 的前端，到处都有它。它就是 4.7 节贝叶斯滤波在"高斯世界"里的精确实现：信念不再是一张直方图，而只用两个量描述，均值和协方差。

    2直觉：一团会移动、会胖瘦的信念

    机器人从来不知道自己确切在哪，它只有一个信念 (belief)：一个高斯分布。均值是"最可能的位置"，宽度（标准差 σ）是"我有多拿不准"。不妨把它想成桌上的一堆沙子。

    卡尔曼滤波只做两件事，交替进行：

    • 预测 (predict)：机器人按运动模型往前走，沙堆跟着挪过去。运动本身有噪声，所以沙堆同时摊开一些：不确定性变大了。
    • 更新 (update)：来了一个测量，它自己也是一堆沙子（传感器噪声）。把两堆沙子相乘，得到一堆更窄的新沙堆，位置落在两者之间，偏向更有把握的那一方。

    "偏向谁、偏多少"由一个 0 到 1 之间的数决定：卡尔曼增益 (Kalman gain) K。K 越接近 1，越相信测量；越接近 0，越相信自己的预测。先来亲手体会一次更新。

    FIG 1两团高斯相乘：一次更新
    青色是预测（先验），紫色是测量，黄色是两者相乘再归一化的结果（后验）。拖曲线底部的圆点移动均值，拖曲线腰部的圆点改变宽度。底部紫色虚线箭头是"新息"（测量与预测之差），黄色箭头是实际挪动的那一段，正好是新息的 K 倍。注意：黄色曲线总比两者都窄，哪怕两个读数互相矛盾。
    • 让 K ≈ 0.5：预测和测量各占一半。这时两条曲线的宽度有什么关系？
    • 让后验几乎和测量重合（K > 0.95）。用一句话说出这对应什么样的传感器。

    把刚才看到的写成公式（这里 \(\mu^-, (\sigma^-)^2\) 是预测的均值与方差，\(z, \sigma_z^2\) 是测量及其噪声方差）：

    $$ \hl{K}{K} = \frac{\hl{prior}{\cx{(\sigma^-)^2}}}{\hl{prior}{\cx{(\sigma^-)^2}} + \hl{meas}{\cw{\sigma_z^2}}} $$ $$ \hl{post}{\cv{\mu}} = \hl{prior}{\cx{\mu^-}} + \hl{K}{K}\,\hl{innov}{(\cw{z} - \cx{\mu^-})}, \qquad \hl{post}{\cv{\sigma^2}} = (1 - \hl{K}{K})\,\hl{prior}{\cx{(\sigma^-)^2}} $$

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的曲线或箭头会被高亮。

    核心洞见

    卡尔曼增益就是一个信任比例：K = 我的不确定性 ÷（我的不确定性 + 传感器的不确定性）。谁更拿不准，谁就让步。更新后的方差 \((1-K)(\sigma^-)^2\) 一定比先验小：只要模型是对的，多一个测量永远不会让你更糊涂。

    3动手玩：追踪一辆小车

    现在让时间流动起来。一辆小车在轨道上来回跑：大体匀速，但会随机加减速，到了两端被缓冲器弹回。一个很烂的"GPS"每 0.2 秒给出一个位置读数（紫色），噪声标准差 1.2 米。滤波器的状态是位置和速度 \((p, v)\)，运动模型是"匀速"：\(p \leftarrow p + v\,\Delta t\)。

    你手里有两个旋钮，它们是滤波器自以为的噪声水平，不改变真实世界：

    • Q（过程噪声）：滤波器认为"匀速模型"有多不靠谱，用随机加速度 σₐ 表示。Q 大 = "小车随时可能变速，别太信预测"。
    • R（测量噪声）：滤波器认为 GPS 有多不靠谱，用 σ_z 表示。R 大 = "GPS 很烂，别太信测量"。

    先一步一步按 预测、更新，看信念的团怎么变胖、变瘦；然后打开自动播放，转动旋钮。

    FIG 2一维追踪实验室
    轨道
    位置 − 时间
    预设
    图例：不带颜色的实线与小车 = 真实位置；紫点 = GPS 读数（空心 = 没被用上）；黄线与黄带 = 估计值与 ±2σ 范围；青色 = 刚预测完、还没更新的信念。真实的随机加速度约 0.4 m/s²，但小车还会被缓冲器弹、被"巡航"拉回 1.5 m/s，匀速模型其实是错的。所以表现最好的 Q 往往比 0.4 大：Q 不只代表噪声，也替你的模型误差买单。
    试一试
    • 把滤波器调得迟钝：让位置的卡尔曼增益低于 0.08，并至少跑 20 次更新。小车被弹回时，黄线跟得上吗？
    • 把它调得神经质：增益高于 0.9，跑 20 次更新。黄线和紫点一样抖，滤波器等于没开。
    • 打败传感器：找一组 Q、R，在同一组设置下连续 50 次更新，让估计误差 (RMSE) 低于 GPS 误差的 70%。
    读懂旋钮

    对均值来说，真正起作用的是 Q 与 R 的比值：Q/R 小，滤波器相信模型，输出平滑但反应慢；Q/R 大，滤波器相信传感器，反应快但抖。把 Q、R 同时放大 10 倍，K 和黄线都不会变，只有 ±2σ 带会变宽。这说明 Q、R 的绝对大小决定的是滤波器"自称"有多准，这一点在第 7 节还会咬人。

    4第二个视角：相空间里的协方差椭圆

    FIG 2 只画了位置那一维，可滤波器心里装的是二维状态 \((p, v)\)。二维高斯的"团"是一个椭圆（4.4 节）：横轴是位置，纵轴是速度。

    预测那一步 \(p \leftarrow p + v\,\Delta t\)，写成矩阵就是 \(F = \begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}\)。这正是 1.3 节里的剪切：椭圆上速度大的那些"可能的我"跑得更远，速度小的落在后面，椭圆于是被拉歪，位置和速度变得相关。按 4.5 节的规则，协方差变成 \(FPF^\top\)，再加上 Q。

    更新那一步只测位置。测量在相空间里是一条竖直的条带：它说"p 大概在这儿"，对 v 只字未提。后验是椭圆与条带的交集。神奇的地方来了：因为椭圆是歪的，横向挤一挤，竖向也跟着变窄。速度从没被测过，却也变准了。

    FIG 3剪切、相关与"免费"的速度
    椭圆是 2σ 范围。青色 = 预测后的信念，黄色 = 更新后的信念，灰色虚线 = 上一步的样子。紫色条带是位置测量 \(z \pm 2\sigma_z\)，可以左右拖动 z；黄色虚线椭圆是"如果现在更新"会得到的结果。两边的粗线段是位置、速度各自的 ±2σ。先直接按"更新"试试：一开始椭圆没有倾斜，速度一点也不会变准。
    • 只用位置测量，把速度的标准差 σ_v 从 0.80 压到 0.40 以下。
    两个视角，一回事

    FIG 2 的位置 ±2σ 带，就是 FIG 3 椭圆在横轴上的"影子"。FIG 2 里滤波器明明只看到位置读数，却能估出速度、提前把团挪到小车要去的地方，靠的正是椭圆里的这份相关性。拖动 z 时留意：测量偏右，后验均值会斜着往右上方走，因为"小车比预想的更靠前"意味着"它大概也比预想的更快"。

    5再看数学

    提示：悬停下面公式中带底色的符号，FIG 3 中对应的椭圆、条带或箭头会高亮（FIG 2 右侧的实时公式也可以悬停）。

    一维：两个高斯相乘

    贝叶斯更新（4.7 节）说：后验 ∝ 似然 × 先验。两个都是高斯，相乘后指数部分仍然是 x 的二次式，所以结果还是高斯：

    $$ p(x \mid z) \;\propto\; \exp\!\Big(-\frac{(x-\mu^-)^2}{2(\sigma^-)^2}\Big)\cdot \exp\!\Big(-\frac{(z-x)^2}{2\sigma_z^2}\Big). $$

    比较 \(x^2\) 和 \(x\) 的系数，得到两条非常好记的规则：精度（方差的倒数）相加，均值是按精度加权的平均：

    $$ \frac{1}{\sigma^2} = \frac{1}{(\sigma^-)^2} + \frac{1}{\sigma_z^2}, \qquad \mu = \sigma^2\Big(\frac{\mu^-}{(\sigma^-)^2} + \frac{z}{\sigma_z^2}\Big). $$

    令 \(K = (\sigma^-)^2 / ((\sigma^-)^2 + \sigma_z^2)\)，整理一下就是第 2 节的形式 \(\mu = \mu^- + K(z-\mu^-)\)，\(\sigma^2 = (1-K)(\sigma^-)^2\)。两种写法完全等价，"增益 + 新息"的写法可以直接推广到多维。

    一般形式：卡尔曼滤波

    系统的状态 \(\x\) 按线性模型演化，传感器线性地观测它，两处都有零均值高斯噪声：

    $$ \x_k = F\,\x_{k-1} + B\,\mathbf u_k + \mathbf w_k,\;\; \mathbf w_k\sim\mathcal N(\mathbf 0, Q); \qquad \mathbf z_k = H\,\x_k + \boldsymbol\epsilon_k,\;\; \boldsymbol\epsilon_k \sim \mathcal N(\mathbf 0, R). $$

    信念是 \(\mathcal N(\hat{\x}, P)\)。每个时刻做两步：第一行是预测，后两行是更新。

    $$ \hat{\x}^- = F\hat{\x} + B\mathbf u, \qquad \hl{prior}{\cx{P^-}} = \hl{FPF}{F P F^\top} + \hl{Q}{Q} $$ $$ \hl{K}{K} = P^- H^\top \big(\hl{S}{H P^- H^\top + \hl{meas}{\cw{R}}}\big)^{-1} $$ $$ \hat{\x} = \hat{\x}^- + \hl{K}{K}\,\hl{innov}{(\cw{\mathbf z} - H\hat{\x}^-)}, \qquad \hl{post}{\cv{P}} = (I - K H)\, P^- $$
    符号含义在图里
    \(\hat{\x}, P\)信念：均值与协方差黄色的团 / 椭圆
    \(F\)运动模型：状态怎样演化FIG 3 里的剪切
    \(Q\)过程噪声：模型有多不准预测时团变胖
    \(H\)测量模型：状态 → 读数只测位置时 \(H = [1\;\,0]\)
    \(R\)测量噪声紫色条带的宽度
    \(\mathbf z - H\hat{\x}^-\)新息 (innovation)：测量带来的"意外"紫色虚线箭头
    \(S = HP^-H^\top + R\)新息的协方差：预期的意外有多大预测团与测量团宽度的合成
    \(K\)卡尔曼增益：信任比例FIG 1 的信任条
    一维只是特例

    令 \(F = H = 1\)，\(P^- = (\sigma^-)^2\)，\(R = \sigma_z^2\)，上面的 \(K = P^-/(P^- + R)\) 就退化成 FIG 1 的公式。多维时 \(K\) 是一个矩阵：它不仅决定"信多少"，还决定一个位置上的意外该怎样分摊到速度等没被测到的分量上，FIG 3 里斜着走的那支箭头就是它干的。

    扩展卡尔曼滤波 (EKF)

    真实机器人几乎都不是线性的：小车朝 \(\theta\) 方向开，位置的变化里有 \(\cos\theta\)；激光测到路标的距离里有平方根，方位里有 \(\operatorname{atan2}\)。模型变成

    $$ \x_k = f(\x_{k-1}, \mathbf u_k) + \mathbf w_k, \qquad \mathbf z_k = h(\x_k) + \boldsymbol\epsilon_k. $$

    EKF 的思路就是 3.3 节的雅可比：在当前估计附近，把 \(f\) 和 \(h\) 看成线性的。均值照常走真正的非线性函数，协方差则用雅可比矩阵传播（4.5 节的 \(J\Sigma J^\top\)）：

    $$ \hat{\x}^- = f(\hat{\x}, \mathbf u),\quad F = \frac{\partial f}{\partial \x}\Big|_{\hat{\x},\,\mathbf u}; \qquad \hl{innov}{\mathbf z - h(\hat{\x}^-)},\quad \hl{H}{H = \frac{\partial h}{\partial \x}\Big|_{\hat{\x}^-}} $$

    其余公式和线性 KF 一字不差。以第 6 节的小车为例，状态 \(\x = (x, y, \theta)\)，控制量是里程计给出的线速度 \(v\) 与角速度 \(\omega\)，传感器测量到路标 \(\mathbf m\) 的距离与方位。记 \(\Delta x = m_x - x\)，\(\Delta y = m_y - y\)，\(r = \sqrt{\Delta x^2 + \Delta y^2}\)：

    $$ f = \begin{bmatrix} x + v\Delta t\cos\theta \\ y + v\Delta t\sin\theta \\ \theta + \omega\Delta t \end{bmatrix},\qquad F = \begin{bmatrix} 1 & 0 & -v\Delta t\sin\theta \\ 0 & 1 & v\Delta t\cos\theta \\ 0 & 0 & 1 \end{bmatrix} $$ $$ h = \begin{bmatrix} r \\ \operatorname{atan2}(\Delta y, \Delta x) - \theta \end{bmatrix},\qquad \hl{H}{H} = \begin{bmatrix} -\Delta x / r & -\Delta y / r & 0 \\ \Delta y / r^2 & -\Delta x / r^2 & -1 \end{bmatrix} $$

    读一读 \(F\) 的第三列：朝向的一点误差，会按 \(v\Delta t\) 的比例变成侧向的位置误差。这就是为什么里程计的航向一偏，位置椭圆会沿着垂直于行驶方向的方向迅速变长。

    6机器人中的它：用路标做 EKF 定位

    一台小车沿着椭圆形路线兜圈。它用里程计做预测，用激光测到附近路标的距离和方位做更新（假设它认得每个路标是谁，也就是数据关联已知）。路标都在地图上，位置是已知的。

    看椭圆：在没有路标的那一侧，只有预测、没有更新，椭圆会越长越大，而且是横着长（航向误差变成侧向误差）。一看到路标，它就猛地缩回去。红色虚线是纯里程计（航位推算 dead reckoning），它永远不会被纠正。

    FIG 4EKF 定位：路标、椭圆与雅可比
    最近一次更新的线性化（在 \(\hat{\x}^-\) 处求值）：
    点击路标开关它（空心 = 关闭），点击空白处添加新路标。实线 = 真实轨迹，红色虚线 = 纯里程计，黄色 = EKF 估计与它的 2σ 位置椭圆、±2σ 航向扇形。青色虚线圆是传感器量程，紫线是这一刻测到的路标。
    • 关掉所有路标（或把量程调到 0），等椭圆长到超过 1.5 米（看右侧读数），再打开路标，看它缩回 0.35 米以内。
    真实系统里

    ROS 的 robot_localization 包用 EKF 把轮式里程计、IMU 和 GPS 融合成一个位姿；PX4 飞控的 EKF2 同时估计无人机的姿态、速度、位置和传感器零偏；视觉惯性里程计里的 MSCKF 也是卡尔曼滤波的一个变种。对付三维旋转时，工程上常用"误差状态"卡尔曼滤波 (error-state KF)：把小的旋转误差放在李代数里（第 5 章）当作向量来滤，就绕开了旋转矩阵不能直接相加的麻烦。

    7常见误区

    误区 1：Q 和 R 随便调，反正能跑

    R 描述的是传感器，可以测出来：让机器人静止，录一段读数，算方差就行。Q 描述的是"模型有多错"，更难确定，通常要调。两者含义不同，不要把 R 调小来"让滤波器跟得更快"，那是在对滤波器撒谎；跟不上说明模型不够好，应该加大 Q，或者换更好的模型。

    误区 2：P 越小越好

    P 是滤波器自称的不确定性，不是真实误差。Q 设得太小，P 会一路缩到几乎为零，K 也跟着变成零，滤波器从此对测量充耳不闻。真实世界稍一变化，估计就一去不回，而 ±2σ 带还信誓旦旦地很窄。这叫发散 (divergence)。一个好滤波器应该"诚实"：真值大约 95% 的时间落在 ±2σ 带里。下面的 FIG 5 让你亲手把滤波器调到撒谎。

    FIG 5诚实的滤波器与撒谎的滤波器
    同一段数据：小车匀速前进，在第 20 秒急刹并掉头（竖直虚线）。滑块只改变滤波器的假设，结果即时重算。真值跑出 ±2σ 带的时刻用红点标出；"覆盖率"是真值落在带内的时间比例。
    • 让滤波器过度自信：把覆盖率压到 50% 以下。然后想一想：只看黄线和黄带，你能发现它在撒谎吗？
    误区 3：角度直接相减

    预测方位是 −179°，测到 179°。直接相减得到新息 358°，滤波器会以为差了将近一圈，把机器人猛地转回去；正确答案是 −2°。凡是涉及角度的新息和状态，都要归一化到 (−180°, 180°]。FIG 4 每一次更新都做了这件事，否则小车绕到左边时就会抽风。

    误区 4：EKF 的线性化总是够好

    雅可比只在估计点附近可信。如果不确定性比函数"弯曲"的尺度还大（比如离一个很近的路标测方位，或者初始位姿完全不知道），线性化就会出错，滤波器可能变得过度自信甚至发散。对策：给 Q 留点余量，用迭代 EKF 重新线性化，或者换成无迹卡尔曼滤波 (UKF)。另外，数值误差会让 P 失去对称正定，实际代码常用 Joseph 形式 \((I-KH)P^-(I-KH)^\top + KRK^\top\) 来更新 P。

    误区 5：一个高斯什么都能表示

    卡尔曼滤波的信念永远是单峰的。一条走廊里有两扇一模一样的门，机器人看到一扇门时，真实的信念应该有两个峰；"被绑架"的机器人（突然被搬到别处）则需要在整张地图上撒开的信念。一个高斯只能取中间值或者二选一，这两种都是错的。这种时候要用 4.10 节的粒子滤波。

    8小练习

    1. 传感器坏了，读数完全是噪声，相当于 \(R \to \infty\)。卡尔曼增益会怎样？
    \(K = P^-/(P^- + R)\)，分母趋于无穷，K → 0。新息乘以 0，测量对估计没有任何影响，这正是我们想要的。
    2. 预测：位置 10 m，σ = 2 m。测量：14 m，σ_z = 2 m。融合之后是？
    \(K = 4/(4+4) = 0.5\)，\(\mu = 10 + 0.5\cdot 4 = 12\)，\(\sigma^2 = (1-0.5)\cdot 4 = 2\)，\(\sigma = \sqrt 2 \approx 1.41\)。两个同样可信的读数，融合后的方差减半，比任何一个都准。
    3. FIG 3 中只测了位置，速度的不确定性却也变小了。根本原因是？
    \(H = [1\;\,0]\) 确实只看位置。速度的增益是 \(K_v = P^-_{vp}/S\)，正比于 p 与 v 的协方差。没有相关（椭圆不倾斜）时 \(K_v = 0\)，速度什么也学不到；剪切 \(F\) 制造了相关性。
    4. EKF 预测某路标的方位是 −179°，实际测到 179°。正确的方位新息是多少？
    \(179° - (-179°) = 358°\)，归一化到 (−180°, 180°] 后是 \(358° - 360° = -2°\)。−179° 也就是 181°，测量比预测少 2°，所以是 −2°，不是 +2°。

    9延伸阅读

    • S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 3 — 高斯滤波器：KF、EKF、UKF 与信息滤波，以及第 7 章的 EKF 定位
    • R. Labbe, Kalman and Bayesian Filters in Python — 免费的交互式 Jupyter 教材，从 g-h 滤波一路讲到 EKF 与 UKF
    • G. Welch & G. Bishop, An Introduction to the Kalman Filter, UNC-Chapel Hill TR 95-041 — 简短经典的入门讲义，本节的 x̂⁻、P⁻ 记号即来自这里
    • T. Babb, How a Kalman filter works, in pictures — 用图讲清"两个高斯相乘"的经典博文
    • R. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems", Journal of Basic Engineering, 1960 — 原始论文