Part I · 第 4 章概率 · 4.8Part I · Ch. 4 Probability · 4.8

卡尔曼滤波与 EKFKalman Filter & EKF

轮式里程计会慢慢漂走，GPS 又抖又跳，单靠哪一个都不靠谱。卡尔曼滤波 (Kalman filter) 用一团高斯分布表示"我大概在哪"：预测时把这团信念挪过去、再吹胖一点；更新时把它和测量相乘，得到一团更瘦、更可信的信念。把线性模型换成雅可比矩阵，它就成了机器人状态估计里的老黄牛：扩展卡尔曼滤波 (EKF)。 Wheel odometry slowly drifts; GPS is noisy and jumpy; neither is good enough on its own. The Kalman filter keeps a Gaussian blob for "where I probably am": predict moves the blob and makes it fatter; update multiplies it with a measurement into a thinner, more trustworthy blob. Swap the linear model for Jacobians and you get the workhorse of robot state estimation: the extended Kalman filter (EKF).

约 35 分钟~35 min 前置：4.4 高斯分布 · 4.5 不确定性传播 · 4.7 贝叶斯滤波 · 3.3 雅可比（EKF 部分）Prereqs: 4.4 Gaussians · 4.5 Propagating Uncertainty · 4.7 Bayes Filter · 3.3 Jacobian (for the EKF) 5 个交互5 interactives

1为什么机器人需要它Why robots need this

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

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

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

A delivery robot drives around a campus. It has two sources of information: wheel encoders (odometry) say "I just moved 0.52 m forward and turned 3° left"; GPS reports "you are at (x, y)" once a second.

Odometry is accurate and smooth over short spans, but every small error accumulates: a wheel slips, the ground is bumpy, and ten minutes later the robot thinks it is in the next building. GPS never accumulates error, but each reading jumps around the truth by several meters, and it drops out indoors.

One is "trustworthy short-term, drifts long-term"; the other is "trustworthy long-term, jumpy short-term". The Kalman filter fuses them according to how much each deserves to be trusted, and for linear-Gaussian systems the result is optimal. You will find it in drone autopilots, self-driving localization, IMU + GPS fusion in your phone and SLAM front-ends. It is the exact implementation of the Bayes filter from Lesson 4.7 in a "Gaussian world": the belief is no longer a histogram but just two quantities, a mean and a covariance.

2直觉：一团会移动、会胖瘦的信念Intuition: a blob of belief that moves, grows and shrinks

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

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

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

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

A robot never knows exactly where it is. All it has is a belief: a Gaussian. The mean is "where I most likely am", the width (standard deviation σ) is "how unsure I am". Picture a pile of sand on a table.

The Kalman filter does just two things, over and over:

Predict: the robot moves according to its motion model, so the sand pile slides along. The motion itself is noisy, so the pile also spreads out: uncertainty grows.
Update: a measurement arrives. It is a sand pile of its own (sensor noise). Multiply the two piles and you get a narrower pile that sits between them, closer to whichever one was more certain.

"Closer to whom, and by how much" is set by a single number between 0 and 1: the Kalman gain K. K near 1 means "trust the measurement"; K near 0 means "trust my prediction". Let's feel one update with our hands first.

FIG 1两团高斯相乘：一次更新Multiplying two Gaussians: one update

青色是预测（先验），紫色是测量，黄色是两者相乘再归一化的结果（后验）。拖曲线底部的圆点移动均值，拖曲线腰部的圆点改变宽度。底部紫色虚线箭头是"新息"（测量与预测之差），黄色箭头是实际挪动的那一段，正好是新息的 K 倍。注意：黄色曲线总比两者都窄，哪怕两个读数互相矛盾。 Cyan is the prediction (prior), purple the measurement, amber their normalized product (posterior). Drag the dot at the base of a curve to move its mean, the dot on its flank to change its width. The dashed purple arrow at the bottom is the "innovation" (measurement minus prediction); the amber arrow is how far the estimate actually moves: exactly K times the innovation. Notice the amber curve is always narrower than both, even when the two readings disagree.

让 K ≈ 0.5：预测和测量各占一半。这时两条曲线的宽度有什么关系？Make K ≈ 0.5: prediction and measurement get equal say. What must be true of the two widths?
让后验几乎和测量重合（K > 0.95）。用一句话说出这对应什么样的传感器。Make the posterior almost identical to the measurement (K > 0.95). In one sentence, what kind of sensor is this?

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

Here is what you just saw, as formulas ($\mu^-, (\sigma^-)^2$ are the predicted mean and variance; $z, \sigma_z^2$ are the measurement and its noise variance):

$$ \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 中对应的曲线或箭头会被高亮。Tip: hover the shaded symbols in the formula; the matching curve or arrow in FIG 1 lights up.

核心洞见Key insight

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

The Kalman gain is a trust ratio: K = my uncertainty ÷ (my uncertainty + the sensor's uncertainty). Whoever is less sure gives way. The updated variance $(1-K)(\sigma^-)^2$ is always smaller than the prior: as long as the model is right, one more measurement never makes you more confused.

3动手玩：追踪一辆小车Play: tracking a cart

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

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

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

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

Now let time run. A cart shuttles back and forth on a track: roughly constant speed, with random speed-ups and slow-downs, bounced back by bumpers at both ends. A terrible "GPS" reports its position every 0.2 s (purple) with 1.2 m of noise. The filter's state is position and velocity $(p, v)$, and its motion model is "constant velocity": $p \leftarrow p + v\,\Delta t$.

You have two knobs. They are the noise levels the filter believes in; they do not change the real world:

Q (process noise): how wrong the filter thinks its constant-velocity model is, expressed as a random acceleration σₐ. Large Q = "the cart may change speed any time; don't over-trust the prediction".
R (measurement noise): how bad the filter thinks the GPS is, expressed as σ_z. Large R = "the GPS is awful; don't over-trust the measurement".

First press Predict and Update one at a time and watch the blob grow and shrink; then turn on auto-play and play with the knobs.

FIG 2一维追踪实验室1D tracking lab

轨道TRACK

位置 − 时间POSITION vs TIME

预设Presets

图例：不带颜色的实线与小车 = 真实位置；紫点 = GPS 读数（空心 = 没被用上）；黄线与黄带 = 估计值与 ±2σ 范围；青色 = 刚预测完、还没更新的信念。真实的随机加速度约 0.4 m/s²，但小车还会被缓冲器弹、被"巡航"拉回 1.5 m/s，匀速模型其实是错的。所以表现最好的 Q 往往比 0.4 大：Q 不只代表噪声，也替你的模型误差买单。 Legend: the plain (uncoloured) solid line and the cart = true position; purple dots = GPS readings (hollow = never used); amber line and band = estimate and ±2σ; cyan = a belief that has been predicted but not yet updated. The true random acceleration is about 0.4 m/s², but the cart is also bounced by bumpers and pulled back to a 1.5 m/s "cruise", so the constant-velocity model is actually wrong. That is why the best Q is usually larger than 0.4: Q pays not only for noise but also for your model's mistakes.

试一试Try this

把滤波器调得迟钝：让位置的卡尔曼增益低于 0.08，并至少跑 20 次更新。小车被弹回时，黄线跟得上吗？Make the filter sluggish: get the position gain below 0.08 and run at least 20 updates. When the cart bounces, does the amber line keep up?
把它调得神经质：增益高于 0.9，跑 20 次更新。黄线和紫点一样抖，滤波器等于没开。Make it jumpy: gain above 0.9 for 20 updates. The amber line now shakes as much as the purple dots; the filter might as well be off.
打败传感器：找一组 Q、R，在同一组设置下连续 50 次更新，让估计误差 (RMSE) 低于 GPS 误差的 70%。Beat the sensor: find Q and R such that, over 50 updates with the same settings, the estimate's RMSE is below 70% of the GPS's.

读懂旋钮Reading the knobs

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

For the mean, what really matters is the ratio Q/R: small Q/R trusts the model (smooth but slow), large Q/R trusts the sensor (fast but jittery). Scale Q and R together by 10 and neither K nor the amber line changes; only the ±2σ band widens. So the absolute size of Q and R sets how accurate the filter claims to be, and that will bite us in section 7.

4第二个视角：相空间里的协方差椭圆A second view: the covariance ellipse in phase space

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 2 only drew the position axis, but the filter actually carries a two-dimensional state $(p, v)$. A 2D Gaussian blob is an ellipse (Lesson 4.4): position across, velocity up.

The predict step $p \leftarrow p + v\,\Delta t$ is the matrix $F = \begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}$: exactly the shear from Lesson 1.3. Possible versions of me with high velocity run further ahead, slow ones fall behind, so the ellipse tilts and position and velocity become correlated. By the rule from Lesson 4.5 the covariance becomes $FPF^\top$, plus Q.

The update measures position only. In phase space, that measurement is a vertical strip: it says "p is about here" and nothing about v. The posterior is where the ellipse and the strip overlap. Here is the magic: because the ellipse is tilted, squeezing it sideways also makes it shorter. Velocity was never measured, yet it got more accurate.

FIG 3剪切、相关与"免费"的速度Shear, correlation and "free" velocity

椭圆是 2σ 范围。青色 = 预测后的信念，黄色 = 更新后的信念，灰色虚线 = 上一步的样子。紫色条带是位置测量 $z \pm 2\sigma_z$，可以左右拖动 z；黄色虚线椭圆是"如果现在更新"会得到的结果。两边的粗线段是位置、速度各自的 ±2σ。先直接按"更新"试试：一开始椭圆没有倾斜，速度一点也不会变准。 Ellipses are 2σ. Cyan = belief after predict, amber = belief after update, dashed grey = the previous step. The purple strip is the position measurement $z \pm 2\sigma_z$; drag z sideways. The dashed amber ellipse previews what an update would give right now. The thick bars on the edges are the ±2σ of position and of velocity alone. Try pressing "Update" first: the ellipse starts untilted, so velocity does not improve at all.

只用位置测量，把速度的标准差 σ_v 从 0.80 压到 0.40 以下。Using only position measurements, push the velocity standard deviation σ_v from 0.80 below 0.40.

两个视角，一回事Two views, one filter

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

The ±2σ position band in FIG 2 is the "shadow" of FIG 3's ellipse on the horizontal axis. FIG 2's filter only ever sees positions, yet it estimates velocity and moves the blob to where the cart is heading, thanks to this correlation inside the ellipse. While dragging z, notice: a measurement to the right moves the posterior mean diagonally up and right, because "the cart is further ahead than I thought" implies "it is probably also faster than I thought".

5再看数学The math

提示：悬停下面公式中带底色的符号，FIG 3 中对应的椭圆、条带或箭头会高亮（FIG 2 右侧的实时公式也可以悬停）。Tip: hover the shaded symbols below and the matching ellipse, strip or arrow in FIG 3 lights up (the live formulas beside FIG 2 are hoverable too).

一维：两个高斯相乘1D: multiplying two Gaussians

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

The Bayes update (Lesson 4.7) says posterior ∝ likelihood × prior. Both are Gaussians, and the product's exponent is still quadratic in x, so the result is again a Gaussian:

$$ 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$ 的系数，得到两条非常好记的规则：精度（方差的倒数）相加，均值是按精度加权的平均：

Matching the coefficients of $x^2$ and $x$ gives two easy-to-remember rules: precisions (inverse variances) add, and the mean is a precision-weighted average:

$$ \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$。两种写法完全等价，"增益 + 新息"的写法可以直接推广到多维。

With $K = (\sigma^-)^2 / ((\sigma^-)^2 + \sigma_z^2)$, a little algebra turns this into the form from section 2: $\mu = \mu^- + K(z-\mu^-)$ and $\sigma^2 = (1-K)(\sigma^-)^2$. The two forms are identical; the "gain + innovation" form is the one that generalizes to many dimensions.

一般形式：卡尔曼滤波The general Kalman filter

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

The state $\x$ evolves by a linear model and a sensor observes it linearly, both with zero-mean Gaussian noise:

$$ \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)$。每个时刻做两步：第一行是预测，后两行是更新。

The belief is $\mathcal N(\hat{\x}, P)$. Every time step does two things: the first line is the predict step, the last two are the update.

$$ \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^- $$

符号Symbol	含义Meaning	在图里In the figures
$\hat{\x}, P$	信念：均值与协方差Belief: mean and covariance	黄色的团 / 椭圆The amber blob / ellipse
$F$	运动模型：状态怎样演化Motion model: how the state evolves	FIG 3 里的剪切The shear in FIG 3
$Q$	过程噪声：模型有多不准Process noise: how wrong the model is	预测时团变胖The blob fattening on predict
$H$	测量模型：状态 → 读数Measurement model: state → reading	只测位置时 $H = [1\;\,0]$Position only: $H = [1\;\,0]$
$R$	测量噪声Measurement noise	紫色条带的宽度Width of the purple strip
$\mathbf z - H\hat{\x}^-$	新息 (innovation)：测量带来的"意外"Innovation: the measurement's "surprise"	紫色虚线箭头The dashed purple arrow
$S = HP^-H^\top + R$	新息的协方差：预期的意外有多大Innovation covariance: how big a surprise to expect	预测团与测量团宽度的合成Prediction width and sensor width combined
$K$	卡尔曼增益：信任比例Kalman gain: the trust ratio	FIG 1 的信任条The trust bar in FIG 1

一维只是特例1D is just a special case

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

Set $F = H = 1$, $P^- = (\sigma^-)^2$, $R = \sigma_z^2$ and $K = P^-/(P^- + R)$ collapses to FIG 1's formula. In many dimensions $K$ is a matrix: it decides not only how much to trust, but also how a surprise in position should be shared out to unmeasured components like velocity. The diagonal arrow in FIG 3 is its doing.

扩展卡尔曼滤波 (EKF)The extended Kalman filter (EKF)

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

Real robots are almost never linear: a car heading at $\theta$ moves by $\cos\theta$; a range to a landmark has a square root, a bearing has an $\operatorname{atan2}$. The model becomes

$$ \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$）：

The EKF's idea is the Jacobian from Lesson 3.3: near the current estimate, treat $f$ and $h$ as linear. The mean goes through the true nonlinear functions; the covariance is propagated with Jacobians (the $J\Sigma J^\top$ of Lesson 4.5):

$$ \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}$：

Every other formula is the linear KF, letter for letter. For the robot of section 6 the state is $\x = (x, y, \theta)$, the controls are the odometry's linear and angular velocities $v, \omega$, and the sensor measures the range and bearing to a landmark $\mathbf m$. With $\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$ 的比例变成侧向的位置误差。这就是为什么里程计的航向一偏，位置椭圆会沿着垂直于行驶方向的方向迅速变长。

Read the third column of $F$: a small heading error turns into a sideways position error in proportion to $v\Delta t$. That is why, once the odometry heading is off, the position ellipse quickly stretches perpendicular to the direction of travel.

6机器人中的它：用路标做 EKF 定位In robotics: EKF localization with landmarks

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

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

A robot drives laps around an elliptical route. It predicts with odometry and updates with laser range-bearing measurements of nearby landmarks (assume it knows which landmark is which, i.e. known data association). The landmarks are on the map at known positions.

Watch the ellipse: on the side with no landmarks there is only predict and no update, so it keeps growing, and it grows sideways (heading error becomes lateral error). As soon as a landmark comes into view it snaps back. The dashed red line is pure odometry (dead reckoning), which is never corrected.

FIG 4EKF 定位：路标、椭圆与雅可比EKF localization: landmarks, ellipses and Jacobians

最近一次更新的线性化（在 $\hat{\x}^-$ 处求值）：Latest update's linearization (evaluated at $\hat{\x}^-$):

点击路标开关它（空心 = 关闭），点击空白处添加新路标。实线 = 真实轨迹，红色虚线 = 纯里程计，黄色 = EKF 估计与它的 2σ 位置椭圆、±2σ 航向扇形。青色虚线圆是传感器量程，紫线是这一刻测到的路标。 Click a landmark to toggle it (hollow = off); click empty space to add one. Solid line = true path, dashed red = pure odometry, amber = EKF estimate with its 2σ position ellipse and ±2σ heading wedge. The dashed cyan circle is the sensor range; purple lines are the landmarks being measured right now.

关掉所有路标（或把量程调到 0），等椭圆长到超过 1.5 米（看右侧读数），再打开路标，看它缩回 0.35 米以内。Switch off all landmarks (or set the range to 0), wait until the ellipse exceeds 1.5 m (see the readout), then switch them back on and watch it shrink below 0.35 m.

真实系统里In real systems

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

ROS's robot_localization package fuses wheel odometry, IMU and GPS into one pose with an EKF; the PX4 autopilot's EKF2 estimates a drone's attitude, velocity, position and sensor biases all at once; the MSCKF used in visual-inertial odometry is a Kalman filter variant too. For 3D rotations, practitioners usually run an "error-state" Kalman filter: small rotation errors are kept in the Lie algebra (Chapter 5) and filtered as plain vectors, sidestepping the fact that rotation matrices cannot simply be added.

7常见误区Common pitfalls

误区 1：Q 和 R 随便调，反正能跑Pitfall 1: tune Q and R until it "looks fine"

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

R describes the sensor and can be measured: keep the robot still, log readings, compute the variance. Q describes "how wrong the model is" and is harder to pin down; it usually needs tuning. They mean different things, so do not shrink R just "to make the filter react faster"; that lies to the filter. If it lags, the model is not good enough: raise Q, or use a better model.

误区 2：P 越小越好Pitfall 2: the smaller P, the better

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

P is the uncertainty the filter claims, not the actual error. Make Q too small and P shrinks toward zero, K follows it to zero, and the filter stops listening to measurements altogether. The moment the real world changes, the estimate runs away while the ±2σ band still swears it is tight. This is divergence. A good filter is "honest": the truth falls inside ±2σ about 95% of the time. FIG 5 below lets you make a filter lie.

FIG 5诚实的滤波器与撒谎的滤波器Honest filters and lying filters

同一段数据：小车匀速前进，在第 20 秒急刹并掉头（竖直虚线）。滑块只改变滤波器的假设，结果即时重算。真值跑出 ±2σ 带的时刻用红点标出；"覆盖率"是真值落在带内的时间比例。 The same data every time: the cart cruises, then at t = 20 s brakes hard and reverses (vertical dashed line). The sliders change only the filter's assumptions; everything is recomputed instantly. Moments when the truth escapes the ±2σ band are marked with red dots; "coverage" is the fraction of time the truth is inside the band.

让滤波器过度自信：把覆盖率压到 50% 以下。然后想一想：只看黄线和黄带，你能发现它在撒谎吗？Make the filter overconfident: get coverage below 50%. Then ask: looking only at the amber line and band, could you tell it is lying?

误区 3：角度直接相减Pitfall 3: subtracting angles directly

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

The predicted bearing is −179°, the measured one 179°. Subtract naively and the innovation is 358°: the filter thinks it is off by almost a full turn and yanks the robot around. The right answer is −2°. Every angle in an innovation or a state must be wrapped to (−180°, 180°]. FIG 4 does this on every update; without it the robot would go haywire each time it passes the left end of the loop.

误区 4：EKF 的线性化总是够好Pitfall 4: the EKF's linearization is always good enough

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

A Jacobian is only trustworthy near the point where it is evaluated. If the uncertainty is larger than the scale on which the function bends (bearings to a very close landmark, or a completely unknown initial pose), the linearization goes wrong and the filter can become overconfident or diverge. Remedies: leave margin in Q, relinearize with an iterated EKF, or switch to the unscented Kalman filter (UKF). Also, round-off can make P lose its symmetry and positive-definiteness; real code often updates P in Joseph form $(I-KH)P^-(I-KH)^\top + KRK^\top$.

误区 5：一个高斯什么都能表示Pitfall 5: one Gaussian can represent anything

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

A Kalman filter's belief is always unimodal. In a corridor with two identical doors, seeing a door should give a belief with two peaks; a "kidnapped" robot (suddenly carried elsewhere) needs a belief spread over the whole map. A single Gaussian can only average the options or pick one, and both are wrong. That is the job of the particle filter in Lesson 4.10.

8小练习Check yourself

1. 传感器坏了，读数完全是噪声，相当于 $R \to \infty$。卡尔曼增益会怎样？1. The sensor breaks and outputs pure noise, i.e. $R \to \infty$. What happens to the Kalman gain?

$K = P^-/(P^- + R)$，分母趋于无穷，K → 0。新息乘以 0，测量对估计没有任何影响，这正是我们想要的。$K = P^-/(P^- + R)$: the denominator blows up, so K → 0. The innovation is multiplied by zero and the measurement has no effect, which is exactly what we want.

2. 预测：位置 10 m，σ = 2 m。测量：14 m，σ_z = 2 m。融合之后是？2. Prediction: 10 m with σ = 2 m. Measurement: 14 m with σ_z = 2 m. What is the fused belief?

$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$。两个同样可信的读数，融合后的方差减半，比任何一个都准。$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$. Two equally good readings halve the variance: the fusion beats either one alone.

3. FIG 3 中只测了位置，速度的不确定性却也变小了。根本原因是？3. In FIG 3 only position is measured, yet velocity uncertainty shrinks too. Why?

$H = [1\;\,0]$ 确实只看位置。速度的增益是 $K_v = P^-_{vp}/S$，正比于 p 与 v 的协方差。没有相关（椭圆不倾斜）时 $K_v = 0$，速度什么也学不到；剪切 $F$ 制造了相关性。$H = [1\;\,0]$ really only sees position. The velocity gain is $K_v = P^-_{vp}/S$, proportional to the p–v covariance. Without correlation (an untilted ellipse) $K_v = 0$ and velocity learns nothing; the shear $F$ creates the correlation.

4. EKF 预测某路标的方位是 −179°，实际测到 179°。正确的方位新息是多少？4. An EKF predicts a landmark bearing of −179° and measures 179°. What is the correct bearing innovation?

$179° - (-179°) = 358°$，归一化到 (−180°, 180°] 后是 $358° - 360° = -2°$。−179° 也就是 181°，测量比预测少 2°，所以是 −2°，不是 +2°。$179° - (-179°) = 358°$, which wraps into (−180°, 180°] as $358° - 360° = -2°$. −179° is the same as 181°, and the measurement is 2° less than that, so −2°, not +2°.

9延伸阅读Further reading

S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 3 — 高斯滤波器：KF、EKF、UKF 与信息滤波，以及第 7 章的 EKF 定位Gaussian filters: KF, EKF, UKF and the information filter; Ch. 7 covers EKF localization
R. Labbe, Kalman and Bayesian Filters in Python — 免费的交互式 Jupyter 教材，从 g-h 滤波一路讲到 EKF 与 UKFA free, interactive Jupyter book, from the g-h filter all the way to the EKF and UKF
G. Welch & G. Bishop, An Introduction to the Kalman Filter, UNC-Chapel Hill TR 95-041 — 简短经典的入门讲义，本节的 x̂⁻、P⁻ 记号即来自这里A short classic tutorial; the x̂⁻, P⁻ notation used here follows it
T. Babb, How a Kalman filter works, in pictures — 用图讲清"两个高斯相乘"的经典博文The classic blog post explaining "multiplying two Gaussians" with pictures
R. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems", Journal of Basic Engineering, 1960 — 原始论文The original paper

符号Symbol	含义Meaning	在图里In the figures
\(\hat{\x}, P\)	信念：均值与协方差Belief: mean and covariance	黄色的团 / 椭圆The amber blob / ellipse
\(F\)	运动模型：状态怎样演化Motion model: how the state evolves	FIG 3 里的剪切The shear in FIG 3
\(Q\)	过程噪声：模型有多不准Process noise: how wrong the model is	预测时团变胖The blob fattening on predict
\(H\)	测量模型：状态 → 读数Measurement model: state → reading	只测位置时 \(H = [1\;\,0]\)Position only: \(H = [1\;\,0]\)
\(R\)	测量噪声Measurement noise	紫色条带的宽度Width of the purple strip
\(\mathbf z - H\hat{\x}^-\)	新息 (innovation)：测量带来的"意外"Innovation: the measurement's "surprise"	紫色虚线箭头The dashed purple arrow
\(S = HP^-H^\top + R\)	新息的协方差：预期的意外有多大Innovation covariance: how big a surprise to expect	预测团与测量团宽度的合成Prediction width and sensor width combined
\(K\)	卡尔曼增益：信任比例Kalman gain: the trust ratio	FIG 1 的信任条The trust bar in FIG 1