Part I · 第 4 章概率 · 4.5

不确定性传播

关节编码器有 0.5° 的误差，手爪的位置会有多大误差？轮子里程计每步有一点噪声，走了 20 米之后机器人到底"可能在哪"？这就是不确定性传播：已知输入的分布，求它穿过一个函数之后的分布。线性函数有漂亮的闭式解 $A\Sigma A^\top$；非线性函数则用雅可比"拉直"，或用 sigma 点、蒙特卡洛去验证。这是卡尔曼滤波"预测"一步的全部内容。

约 30 分钟前置：4.4 高斯分布 · 3.3 雅可比 · 1.4 矩阵乘法 5 个交互

1为什么机器人需要它

机器人几乎所有"算出来"的量，都是由带噪声的量经过函数算出来的：

机械臂：关节角（编码器有噪声）经过正运动学，得到手爪位置。手爪会偏多少？偏在哪个方向？
移动机器人：上一时刻的位姿加上这一步的里程计（都有噪声），得到新位姿。每走一步，不确定性怎样累积？
相机与激光：测到的是距离和方位角，换成 $(x, y)$ 坐标之后，误差还是不是圆的？

知道了输入的均值和协方差，就想直接写出输出的均值和协方差，而不必每次都真的去采样。这要回答两个问题：函数是线性的时候怎么办（有精确的闭式解），函数是非线性的时候怎么办（近似，而且要知道近似在哪里会失效）。后一个问题的答案，决定了 EKF 什么时候好用、什么时候会"飘"。

卡尔曼滤波的预测步 $P^- = FPF^\top + Q$，就是本节公式的直接应用（4.8）。

2直觉：把一团点云穿过函数，看它被"压"成什么样

先从一维开始。输入 $x$ 服从高斯 $\mathcal N(\mu, \sigma^2)$，输出 $y = f(x)$。把许多样本点 $x_i$ 逐个送进函数，得到 $y_i = f(x_i)$，它们的直方图就是输出分布。下图里，从底部输入分布的每个样本出发，向上碰到曲线，再水平移到右侧的输出直方图。

在均值 $\mu$ 附近，曲线几乎是一条斜率为 $f'(\mu)$ 的直线（切线）。所以如果输入的"团"够小，输出近似还是高斯，宽度被斜率放大：$\sigma_y \approx |f'(\mu)|\,\sigma$。斜率陡的地方，不确定性被放大；斜率平的地方，不确定性被压扁。拖动圆点，换几个函数试试。

FIG 1一维：穿过非线性函数

下方青色钟形是输入 $\mathcal N(\mu, \sigma^2)$（拖动底部圆点移动 $\mu$）。灰色细线是 40 个样本的"路径"。右侧青色直方图是 3000 个样本的真实输出分布；黄色曲线是用切线线性化得到的高斯 $\mathcal N(f(\mu), f'(\mu)^2\sigma^2)$。黄色虚线是切线；右侧空心圆是 $f(\mu)$，青色标记是真实输出均值。

试一试

选"二次"，把 $\mu$ 拖到 0（斜率为 0）：线性化说"输出没有不确定性"，真实输出却有很宽的一团。
在"二次"或"正弦"里找到一个位置，使真实输出均值偏离 $f(\mu)$ 0.3 以上：$E[f(x)] \ne f(E[x])$。
任选一个非线性函数，把 $\sigma$ 缩到 0.2 以下：线性化与真实输出几乎重合。输入越小，"直线近似"越准。

核心洞见

不确定性传播有两个来源要分开想：斜率负责把宽度乘上 $|f'|$（这是线性化能抓住的），弯曲负责把均值推离 $f(\mu)$、让输出不再对称（线性化抓不住）。输入越窄，弯曲越来越不重要。

3动手玩：线性映射下，椭圆跟着被拉伸

到二维。若 $\mathbf y = A\mathbf x$ 是线性的，一切都能精确算出。回忆 1.3 节：矩阵 $A$ 把单位圆变成椭圆，把一整个"网格"拉歪。输入的 1σ 椭圆是单位圆经过 $\Sigma^{1/2}$ 变成的，再经过 $A$ 就得到输出的 1σ 椭圆。所以输出的 1σ 椭圆，恰好就是把输入的 1σ 椭圆用 $A$ 变换一下。它的协方差是 $A\Sigma A^\top$。

在左图拖动椭圆的把手改变输入协方差 $\Sigma$，在下方编辑矩阵 $A$（直接输入数字，或在数字上左右拖动），看右图的输出。

FIG 2线性传播：$\Sigma \to A\Sigma A^\top$

输入 x ~ N(0, Σ)

输出 y = A x

矩阵 A =

800 个样本点（青色）在左右两图里是同一批点，右图里每个点是 $A\mathbf x_i$。黄色实线是公式 $A\Sigma A^\top$ 画出的 1σ、2σ 椭圆；紫色虚线是左图 1σ 椭圆的每个点乘以 $A$ 的像，两者重合。

试一试

点"旋转 45°"：输出椭圆转了 45°，但形状和面积都没变（特征值不变）。旋转不增加也不减少不确定性。
让 $|\det A| < 0.2$：输出椭圆的面积是输入的 $|\det A|$ 倍（1.5 节），被压得又扁又小。
点"只保留 x"（秩 1 的投影）：输出椭圆塌成一条线段，一个方向的不确定性完全消失（协方差奇异）。

注意是 $A\Sigma A^\top$，不是 $A\Sigma$

协方差是"二次"量（单位是输入单位的平方），所以矩阵要在两侧各乘一次。一维时就是 $\sigma_y^2 = a^2\sigma_x^2$，也就是 $\sigma_y = |a|\sigma_x$：标准差乘 $|a|$，方差乘 $a^2$。多个独立误差相加，协方差直接相加（$A = [I\;\,I]$）：两个独立的 0.3 m 误差加起来是 $\sqrt{0.3^2 + 0.3^2} \approx 0.42$ m，不是 0.6 m。

4第二个视角：非线性：线性化、sigma 点与蒙特卡洛

真实的映射几乎都是非线性的。以激光雷达为例：测到的是距离 $r$ 和方位角 $\theta$，它们各自有噪声（$\sigma_r$、$\sigma_\theta$）。要得到 $x = r\cos\theta,\ y = r\sin\theta$，噪声的"团"在 $(r, \theta)$ 平面里是一个正放的椭圆，到了 $(x, y)$ 平面就被"掰弯"成一个香蕉：同一个方位角误差，距离越远，弧长越大。三种办法求输出的均值和协方差：

蒙特卡洛 (MC)：抽很多样本，逐个过函数，直接算样本均值和协方差。最准（样本够多时），也最贵，当作"标准答案"。
线性化 (EKF 的做法)：只在均值处求一次雅可比 $J$，用 $f(\mu)$ 和 $J\Sigma J^\top$。便宜，需要导数。
sigma 点 (无迹变换 UT，UKF 的做法)：沿 $\Sigma$ 的每个主轴取一对点，加上均值本身，共 $2n+1$ 个点，逐个过函数，再用权重重新求均值和协方差。不需要导数，通常更准。

拖动输入的均值圆点，改变 $\sigma_\theta$，看三种方法与蒙特卡洛的差距。

FIG 3极坐标 → 直角坐标：线性化、UT、蒙特卡洛

输入 (r, θ)

输出 (x, y)

青色点是 1500 个样本，青色椭圆是它们的 2σ 样本协方差（"标准答案"）。黄色虚线椭圆是线性化 $J\Sigma J^\top$，紫色椭圆是 UT。左图的 5 个紫色菱形是 sigma 点，右图是它们各自过函数后的位置。表中误差越小越好。

试一试

把 $\sigma_\theta$ 调到 3° 以下：三种方法几乎重合，线性化绰绰有余。
把 $\sigma_\theta$ 拉到 25° 以上、距离 $r \ge 6$：点云弯成香蕉，线性化的椭圆是直的，UT 的协方差误差更小。
观察均值误差：香蕉的"重心"比 $f(\mu)$ 更靠近原点。线性化的均值永远是 $f(\mu)$，所以它有系统性偏差。

5再看数学

提示：悬停带底色的符号，FIG 1 到 FIG 3 里对应的几何会高亮。

线性映射（精确）

$\mathbf y = A\mathbf x + \mathbf b$，$\mathbf x$ 的均值为 $\boldsymbol\mu$，协方差为 $\Sigma$。由期望的线性（4.3），$E[\mathbf y] = A\boldsymbol\mu + \mathbf b$，于是 $\mathbf y - E[\mathbf y] = A(\mathbf x - \boldsymbol\mu)$，代入协方差的定义：

$$ \mathrm{Cov}(\mathbf y) = E\big[A(\mathbf x - \boldsymbol\mu)(\mathbf x - \boldsymbol\mu)^\top A^\top\big] = \hl{lin}{A\,\Sigma\,A^\top}, \qquad \boldsymbol\mu_y = A\boldsymbol\mu + \mathbf b $$ $$ \det(A\Sigma A^\top) = \det(A)^2\det\Sigma, \qquad \mathbf x \sim \mathcal N(\boldsymbol\mu, \Sigma) \;\Rightarrow\; \mathbf y \sim \mathcal N(A\boldsymbol\mu + \mathbf b,\; A\Sigma A^\top) $$

第二行：高斯过线性映射还是高斯（4.4 的封闭性），椭圆面积乘以 $|\det A|$。若 $A$ 不满秩，$A\Sigma A^\top$ 奇异：输出落在一个低维子空间里（FIG 2 的塌缩）。

非线性映射（近似）：线性化

用一阶泰勒展开（3.3 节：雅可比是"局部线性映射"）$f(\mathbf x) \approx f(\boldsymbol\mu) + J(\mathbf x - \boldsymbol\mu)$，$J = \partial f/\partial\mathbf x\big|_{\boldsymbol\mu}$，再套用上面的线性公式：

$$ \boldsymbol\mu_y \approx f(\boldsymbol\mu), \qquad \Sigma_y \approx \hl{jac}{J\,\Sigma\,J^\top}, \qquad J = \frac{\partial f}{\partial \mathbf x}\Big|_{\boldsymbol\mu} $$ $$ J_{\mathrm{polar}} = \begin{bmatrix} \cos\theta & -r\sin\theta \\ \sin\theta & r\cos\theta \end{bmatrix}, \quad \Sigma = \begin{bmatrix}\sigma_r^2 & 0 \\ 0 & \sigma_\theta^2\end{bmatrix} $$

这个近似丢掉了二阶项。包含二阶项后，均值的修正是 $E[f_i(\mathbf x)] \approx f_i(\boldsymbol\mu) + \tfrac12\mathrm{tr}\big(H_i\Sigma\big)$，$H_i$ 是第 $i$ 个分量的 Hessian（3.5 节）。例：$f(x) = x^2$ 时 $H = 2$，修正是 $\sigma^2$，与精确值 $E[x^2] = \mu^2 + \sigma^2$ 吻合。再例：极坐标里 $\partial^2 x/\partial\theta^2 = -r\cos\theta$，修正 $-\tfrac12 r\cos\theta\,\sigma_\theta^2$；对高斯 $\theta$，精确的是 $E[x] = r\,e^{-\sigma_\theta^2/2}\cos\theta_0$，展开 $e^{-s/2} \approx 1 - s/2$ 正好一致。这就是香蕉重心向内偏的数学来历。

无迹变换（sigma 点）

取 $n$ 维输入，令 $L$ 为 $\Sigma = LL^\top$ 的一个平方根（Cholesky）。取 $2n+1$ 个 sigma 点和权重（本节取 $\kappa = 3-n$ 使 $n+\kappa = 3$，是高斯分布的常用选择）：

$$ \hl{ut}{\boldsymbol\chi_0 = \boldsymbol\mu,\quad \boldsymbol\chi_{\pm i} = \boldsymbol\mu \pm \sqrt{n+\kappa}\;\mathbf l_i}, \qquad w_0 = \frac{\kappa}{n+\kappa},\;\; w_{\pm i} = \frac{1}{2(n+\kappa)} $$ $$ \mathcal Y_j = f(\boldsymbol\chi_j), \qquad \bar{\mathbf y} = \sum_j w_j\,\mathcal Y_j, \qquad \Sigma_y = \sum_j w_j\,(\mathcal Y_j - \bar{\mathbf y})(\mathcal Y_j - \bar{\mathbf y})^\top $$

$\mathbf l_i$ 是 $L$ 的第 $i$ 列。这些点的加权均值和协方差恰好等于输入的 $\boldsymbol\mu, \Sigma$（可以验证），所以对线性函数它是精确的；对非线性函数，它还能抓住（部分）二阶效应，且不需要求导。$n=2$ 时是 5 个点，权重 $1/3,\ 1/6,\ 1/6,\ 1/6,\ 1/6$。

6机器人中的它：从机械臂的关节噪声到里程计漂移

(a) 机械臂：关节噪声如何变成手爪误差

平面二连杆臂的手爪位置是 $\mathbf p = f(q_1, q_2)$，关节角各有独立的噪声 $\sigma_q$。手爪位置的协方差是 $J\,\sigma_q^2 I\,J^\top = \sigma_q^2 JJ^\top$，其中 $J$ 就是 3.3 节的雅可比。手爪误差椭圆的主轴就是 $J$ 的奇异方向（1.12 节）。在奇异位形（手臂完全伸直或折叠，$\det J = l_1 l_2\sin q_2 = 0$）下，$J$ 的两列平行，手爪沿着径向"动不了"，误差椭圆塌成一条线。拖动手臂的关节。

FIG 4二连杆臂：关节噪声 → 手爪椭圆

拖动肘部圆点改变 $q_1$，拖动手爪圆点改变 $q_2$。青色点是 300 次带噪声的手爪位置（噪声在关节上），黄色椭圆是 $\sigma_q^2 JJ^\top$ 算出的 2σ 椭圆，与点云吻合。

把手臂拉直（$|q_2| < 5°$）：椭圆塌成一条垂直于手臂的细线，$\det J \to 0$。
找出让椭圆最圆的弯曲角：使短轴/长轴超过 0.75。对这两节连杆（0.5 m 与 0.4 m），它在 $|q_2| \approx 130°$ 附近，而不是直觉上的 90°（90° 时只有 0.41）。

(b) 移动机器人：里程计漂移

一台机器人以 $v = 1$ m/s 前进，每个 $\Delta t = 0.2$ s 的步长里，线速度和角速度各带一点独立噪声 $\sigma_v, \sigma_\omega$。状态 $\mathbf s = (x, y, \theta)$，每步 $\mathbf s_{k+1} = f(\mathbf s_k, \mathbf u_k)$（与 4.8 相同）。没有任何外部测量，协方差就只能一步步按下面递推传播：

$P_{k+1} = F_k P_k F_k^\top + G_k Q G_k^\top$，$F_k = \partial f/\partial\mathbf s$，$G_k = \partial f/\partial\mathbf u$，$Q = \mathrm{diag}(\sigma_v^2, \sigma_\omega^2)$。

下图同时跑了300 次真实的带噪声模拟（青色）和这个协方差递推（黄色椭圆）。拖动时间滑块看不确定性怎样随时间"流"出去，试试不同路径和噪声。

FIG 5航位推算：不确定性随时间增长

中性色粗实线是理想（无噪声）轨迹（虚线是它的完整路线）；青色细线是 40 条带噪声的轨迹，青色点是全部 300 条在当前时刻的位置；黄色椭圆是协方差递推在当前时刻给出的位置 2σ 椭圆，淡黄色轮廓是之前每隔 2 秒的椭圆；椭圆里的短线是航向 ±2σ。读数里的"覆盖率"是点云落入当前 2σ 椭圆的比例，理论值（二维高斯）是 86.5%。

选"直线"，把时间拉到最后：横向（垂直于行驶方向）的误差远大于沿行驶方向的误差。为什么？
把 $\sigma_\omega$ 设为 0：横向误差消失，只剩沿行驶方向的误差；航向噪声才是漂移的"放大器"。
选"直线"，把 $\sigma_\omega$ 加到 0.2 以上，时间拉到最后：点云弯成一道弧形的"扇面"，覆盖率掉到 80% 以下。一个椭圆（高斯）装不下弯曲的分布，这正是线性化的极限。

真实系统里

这解释了为什么只靠里程计的机器人"走着走着就迷路"，也解释了为什么航向比位置更要紧：航向误差会随距离放大成横向误差。所以真实系统用陀螺仪/IMU 精确测航向，并用激光、视觉或 GPS 周期性地"拉回"（4.8 的更新步）。ROS 的里程计消息 nav_msgs/Odometry 就带着一个 6×6 的位姿协方差，规划器、定位模块靠它决定信任多少。

对直线行驶，可以估算（小角度近似，$N = t/\Delta t$ 步）：沿行驶方向 $\sigma_{\parallel} \approx \sigma_v\sqrt{\Delta t\, t}$，随 $\sqrt t$ 增长；横向 $\sigma_\perp \approx \dfrac{v\,\sigma_\omega\sqrt{\Delta t}}{\sqrt 3}\,t^{3/2}$，随 $t^{3/2}$ 增长。航向噪声先累积成航向误差（$\propto\sqrt t$），航向误差再沿路积分成横向位移，所以横向误差增长快得多。

7常见误区

误区 1：线性化永远够用

线性化的前提是输入的"团"比函数弯曲的尺度小。FIG 1 里平坦处 $f' = 0$ 时，$J\Sigma J^\top = 0$ 声称"没有不确定性"，真实的输出却有很宽的分布。FIG 3 里大 $\sigma_\theta$ 的香蕉也一样。这种情形下 EKF 会过度自信，滤波器变得不稳。对策：增大过程噪声 $Q$ 留余量；换 UKF；或把不确定性拆小步传播。

误区 2：$E[f(x)] = f(E[x])$

只有线性函数才成立。对凸函数 $E[f(x)] \ge f(E[x])$（Jensen 不等式）。例如 $x^2$ 的均值比均值的平方大 $\sigma^2$。把"带噪声距离的平方"当作"距离的平方"，会得到系统性偏大的结果。

误区 3：忘了平方，或只乘一边

标准差乘 $|a|$，方差乘 $a^2$；协方差矩阵是 $A\Sigma A^\top$，而不是 $A\Sigma$ 或 $A\sqrt\Sigma A^\top$。另外，把几个独立误差相加时是方差相加，不是标准差相加。

误区 4：假设各步噪声互相独立

$P_{k+1} = FPF^\top + GQG^\top$ 假定每步的噪声独立。真实的里程计有系统误差（轮径标定不准、两轮轮距有偏差），它们每一步方向都相同，累积得比"随机游走"快得多（线性增长，而不是 $\sqrt t$）。标定误差必须建模成慢变的偏置状态，不能当作白噪声。

误区 5：在错误的点求雅可比

$J$ 要在当前估计（均值）处求值，而不是在初始点或真值处（真值机器人并不知道）。每一步 $F_k$ 都不同，因为 $\theta_k$ 在变（见 4.8 的 $F$ 矩阵第三列）。

8小练习

1. 激光读数 $x$ 的标准差是 0.5 m。换算 $y = 3x + 2$ 后，$y$ 的标准差是？

平移不改变分散程度，缩放让标准差乘以 $|3|$：$\sigma_y = 3 \times 0.5 = 1.5$。（方差是 $9 \times 0.25 = 2.25$，别把方差当标准差。）

2. $\mathbf x \sim \mathcal N(\mathbf 0, I_2)$，$A = \begin{bmatrix}2 & 0\\ 0 & 0.5\end{bmatrix}$。$\mathbf y = A\mathbf x$ 的协方差是？

$A\Sigma A^\top = AA^\top = \mathrm{diag}(2^2, 0.5^2) = \mathrm{diag}(4, 0.25)$。标准差放大 2 倍和缩小一半，方差是 4 倍和 1/4。

3. $y = x^2$，输入 $x \sim \mathcal N(0, 0.3^2)$。线性化（在 $\mu = 0$ 处）对 $y$ 的方差预测是？这说明什么？

$J = f'(0) = 0$，所以 $J\Sigma J^\top = 0$。但 $y = x^2 \ge 0$ 的均值是 $\sigma^2 = 0.09$，方差是 $2\sigma^4 \approx 0.016$，完全不是 0。线性化只看斜率，而这里斜率恰好为 0，弯曲才是主角。

4. 机器人直线行驶，只有航向噪声。横向位置误差的标准差随时间 $t$ 大致怎样增长？

航向误差是随机游走，标准差 $\propto \sqrt t$；横向位置是它沿路程的积分，再乘一个 $t$：$\propto t^{3/2}$。所以横向漂移比沿行驶方向（$\propto \sqrt t$）增长快得多。

5. 对 2 维输入，无迹变换需要把多少个 sigma 点送过非线性函数？

$2n + 1 = 5$：均值本身，加上沿 $L$ 的每一列正负各一个点。成本只有 5 次函数调用，远少于蒙特卡洛的成百上千次。

9延伸阅读

S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 3 (Sec. 3.3 EKF, Sec. 3.4 UKF) and Ch. 5 — 线性化预测、无迹变换，以及里程计运动模型
T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 2 and Ch. 4 — 高斯过线性/非线性变换，以及三种近似（线性化、sigma 点、蒙特卡洛）的比较
S. J. Julier & J. K. Uhlmann, "Unscented Filtering and Nonlinear Estimation", Proceedings of the IEEE, 92(3), 2004 — 无迹变换的经典综述
R. Siegwart, I. R. Nourbakhsh & D. Scaramuzza, Introduction to Autonomous Mobile Robots, MIT Press, 2nd ed., 2011, Ch. 5 — 差速机器人的里程计误差模型与协方差传播
K. M. Lynch & F. C. Park, Modern Robotics, Cambridge University Press, 2017 — 雅可比与奇异位形（FIG 4 的背景）