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

不确定性传播Propagating Uncertainty

关节编码器有 0.5° 的误差，手爪的位置会有多大误差？轮子里程计每步有一点噪声，走了 20 米之后机器人到底"可能在哪"？这就是不确定性传播：已知输入的分布，求它穿过一个函数之后的分布。线性函数有漂亮的闭式解 $A\Sigma A^\top$；非线性函数则用雅可比"拉直"，或用 sigma 点、蒙特卡洛去验证。这是卡尔曼滤波"预测"一步的全部内容。 A joint encoder is off by 0.5°: how far off is the gripper? Wheel odometry is slightly noisy at each step: after 20 meters, where might the robot be? That is uncertainty propagation: given the distribution of the input, find the distribution after it passes through a function. A linear function has the beautiful closed form $A\Sigma A^\top$; a nonlinear one is "straightened" with the Jacobian, then checked with sigma points or Monte Carlo. This is the entire content of the Kalman filter's "predict" step.

约 30 分钟~30 min 前置：4.4 高斯分布 · 3.3 雅可比 · 1.4 矩阵乘法Prereqs: 4.4 Gaussians · 3.3 Jacobian · 1.4 Matrix Multiplication 5 个交互5 interactives

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

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

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

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

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

Almost everything a robot "computes" is a function of noisy quantities:

Manipulator: joint angles (noisy encoders) go through forward kinematics to give the gripper position. How far off, and in which direction?
Mobile robot: last pose plus this step's odometry (both noisy) gives the new pose. How does the uncertainty accumulate with each step?
Cameras and lasers: they measure range and bearing; after converting to $(x, y)$, is the error still round?

Given the input's mean and covariance, we want the output's mean and covariance directly, without actually sampling each time. Two questions: what if the function is linear (an exact closed form), and what if it is nonlinear (an approximation, and we must know where it breaks). The answer to the latter decides when the EKF works and when it "drifts away".

The Kalman filter's predict step $P^- = FPF^\top + Q$ is a direct application of this lesson's formulas (4.8).

2直觉：把一团点云穿过函数，看它被"压"成什么样Intuition: push a cloud through a function and see how it gets squeezed

先从一维开始。输入 $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$。斜率陡的地方，不确定性被放大；斜率平的地方，不确定性被压扁。拖动圆点，换几个函数试试。

Start in 1D. The input $x$ is Gaussian $\mathcal N(\mu, \sigma^2)$ and the output is $y = f(x)$. Send many samples $x_i$ through the function one by one to get $y_i = f(x_i)$; their histogram is the output distribution. In the figure each sample starts at the input distribution at the bottom, goes up to hit the curve, then moves horizontally to the output histogram on the right.

Near $\mu$ the curve is almost a straight line of slope $f'(\mu)$ (the tangent). So if the input blob is small enough, the output is still approximately Gaussian with its width scaled by the slope: $\sigma_y \approx |f'(\mu)|\,\sigma$. Where the slope is steep, uncertainty is amplified; where it is flat, uncertainty is squashed. Drag the dot and try different functions.

FIG 1一维：穿过非线性函数1D: through a nonlinear function

下方青色钟形是输入 $\mathcal N(\mu, \sigma^2)$（拖动底部圆点移动 $\mu$）。灰色细线是 40 个样本的"路径"。右侧青色直方图是 3000 个样本的真实输出分布；黄色曲线是用切线线性化得到的高斯 $\mathcal N(f(\mu), f'(\mu)^2\sigma^2)$。黄色虚线是切线；右侧空心圆是 $f(\mu)$，青色标记是真实输出均值。 The cyan bell at the bottom is the input $\mathcal N(\mu, \sigma^2)$ (drag the base dot to move $\mu$). Thin grey lines trace 40 samples. The cyan histogram on the right is the true output distribution from 3000 samples; the amber curve is the tangent-line approximation $\mathcal N(f(\mu), f'(\mu)^2\sigma^2)$. The amber dashed line is the tangent; the hollow circle on the right is $f(\mu)$ and the cyan marker the true output mean.

试一试Try this

选"二次"，把 $\mu$ 拖到 0（斜率为 0）：线性化说"输出没有不确定性"，真实输出却有很宽的一团。Pick "Quadratic" and drag $\mu$ to 0 (zero slope): linearization says "no output uncertainty", yet the true output has a wide spread.
在"二次"或"正弦"里找到一个位置，使真实输出均值偏离 $f(\mu)$ 0.3 以上：$E[f(x)] \ne f(E[x])$。In "Quadratic" or "Sine" find a place where the true output mean is more than 0.3 away from $f(\mu)$: $E[f(x)] \ne f(E[x])$.
任选一个非线性函数，把 $\sigma$ 缩到 0.2 以下：线性化与真实输出几乎重合。输入越小，"直线近似"越准。Pick any nonlinear function and shrink $\sigma$ below 0.2: linearization and the truth nearly coincide. The smaller the input, the better the "straight-line" approximation.

核心洞见Key insight

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

Keep two effects apart: the slope multiplies the width by $|f'|$ (linearization captures this), and the curvature pushes the mean away from $f(\mu)$ and makes the output asymmetric (linearization misses this). The narrower the input, the less the curvature matters.

3动手玩：线性映射下，椭圆跟着被拉伸Play: under a linear map the ellipse is stretched with it

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

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

Now 2D. If $\mathbf y = A\mathbf x$ is linear everything can be computed exactly. Recall Lesson 1.3: a matrix $A$ turns the unit circle into an ellipse and bends a whole grid. The input's 1σ ellipse is the unit circle passed through $\Sigma^{1/2}$; pass it through $A$ too and you get the output's 1σ ellipse. So the output's 1σ ellipse is exactly the input's 1σ ellipse transformed by $A$. Its covariance is $A\Sigma A^\top$.

Drag the handles of the ellipse in the left plot to change the input covariance $\Sigma$, and edit the matrix $A$ below (type numbers, or drag left and right on a number); watch the output in the right plot.

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

输入 x ~ N(0, Σ)INPUT x ~ N(0, Σ)

输出 y = A xOUTPUT y = A x

矩阵 A =matrix A =

800 个样本点（青色）在左右两图里是同一批点，右图里每个点是 $A\mathbf x_i$。黄色实线是公式 $A\Sigma A^\top$ 画出的 1σ、2σ 椭圆；紫色虚线是左图 1σ 椭圆的每个点乘以 $A$ 的像，两者重合。 The 800 sample points (cyan) are the same points in both plots; in the right plot each is $A\mathbf x_i$. The solid amber ellipses are 1σ and 2σ drawn from the formula $A\Sigma A^\top$; the dashed purple curve is the image under $A$ of every point of the left 1σ ellipse. They coincide.

试一试Try this

点"旋转 45°"：输出椭圆转了 45°，但形状和面积都没变（特征值不变）。旋转不增加也不减少不确定性。Press "Rotate 45°": the output ellipse turns by 45° but its shape and area stay the same (same eigenvalues). A rotation neither adds nor removes uncertainty.
让 $|\det A| < 0.2$：输出椭圆的面积是输入的 $|\det A|$ 倍（1.5 节），被压得又扁又小。Make $|\det A| < 0.2$: the output ellipse's area is $|\det A|$ times the input's (Lesson 1.5), squashed flat and small.
点"只保留 x"（秩 1 的投影）：输出椭圆塌成一条线段，一个方向的不确定性完全消失（协方差奇异）。Press "Keep x only" (a rank-1 projection): the output ellipse collapses to a segment, all uncertainty in one direction vanishes (singular covariance).

注意是 $A\Sigma A^\top$，不是 $A\Sigma$It is $A\Sigma A^\top$, not $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。

Covariance is a "quadratic" quantity (units are the square of the input's), so the matrix multiplies once on each side. In 1D this reads $\sigma_y^2 = a^2\sigma_x^2$, i.e. $\sigma_y = |a|\sigma_x$: the standard deviation scales by $|a|$, the variance by $a^2$. Adding several independent errors adds their covariances ($A = [I\;\,I]$): two independent 0.3 m errors sum to $\sqrt{0.3^2 + 0.3^2} \approx 0.42$ m, not 0.6 m.

4第二个视角：非线性：线性化、sigma 点与蒙特卡洛A second view: nonlinear maps: linearization, sigma points and Monte Carlo

真实的映射几乎都是非线性的。以激光雷达为例：测到的是距离 $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$，看三种方法与蒙特卡洛的差距。

Real maps are almost always nonlinear. Take a lidar: it measures range $r$ and bearing $\theta$, each with noise ($\sigma_r$, $\sigma_\theta$). To get $x = r\cos\theta,\ y = r\sin\theta$, the noise blob, an upright ellipse in the $(r, \theta)$ plane, gets "bent" into a banana in the $(x, y)$ plane: the same bearing error sweeps a longer arc the further away you are. Three ways to find the output mean and covariance:

Monte Carlo (MC): draw many samples, push each through the function, take the sample mean and covariance. The most accurate (with enough samples) and the most expensive; we treat it as the ground truth.
Linearization (the EKF way): evaluate the Jacobian $J$ once at the mean and use $f(\mu)$ and $J\Sigma J^\top$. Cheap, needs derivatives.
Sigma points (the unscented transform, UT, the UKF way): take a pair of points along each principal axis of $\Sigma$ plus the mean itself, $2n+1$ points in all, push each through the function, then recombine them with weights into a mean and covariance. No derivatives, usually more accurate.

Drag the input mean dot, change $\sigma_\theta$, and see how each method compares with Monte Carlo.

FIG 3极坐标 → 直角坐标：线性化、UT、蒙特卡洛Polar → Cartesian: linearization, UT and Monte Carlo

输入 (r, θ)INPUT (r, θ)

输出 (x, y)OUTPUT (x, y)

青色点是 1500 个样本，青色椭圆是它们的 2σ 样本协方差（"标准答案"）。黄色虚线椭圆是线性化 $J\Sigma J^\top$，紫色椭圆是 UT。左图的 5 个紫色菱形是 sigma 点，右图是它们各自过函数后的位置。表中误差越小越好。 Cyan dots are 1500 samples and the cyan ellipse is the 2σ ellipse of their sample covariance (the "ground truth"). The amber dashed ellipse is linearization $J\Sigma J^\top$, the purple ellipse is the UT. The 5 purple diamonds in the left plot are the sigma points; the right plot shows where each lands after the function. Smaller errors in the table are better.

试一试Try this

把 $\sigma_\theta$ 调到 3° 以下：三种方法几乎重合，线性化绰绰有余。Set $\sigma_\theta$ below 3°: all three methods nearly coincide and linearization is plenty.
把 $\sigma_\theta$ 拉到 25° 以上、距离 $r \ge 6$：点云弯成香蕉，线性化的椭圆是直的，UT 的协方差误差更小。Push $\sigma_\theta$ beyond 25° with $r \ge 6$: the cloud bends into a banana, the linearized ellipse stays straight, and the UT's covariance error is smaller.
观察均值误差：香蕉的"重心"比 $f(\mu)$ 更靠近原点。线性化的均值永远是 $f(\mu)$，所以它有系统性偏差。Look at the mean error: the banana's "center of mass" lies closer to the origin than $f(\mu)$. Linearization's mean is always $f(\mu)$, hence systematically biased.

5再看数学The math

提示：悬停带底色的符号，FIG 1 到 FIG 3 里对应的几何会高亮。Tip: hover the shaded symbols and the matching geometry in FIG 1 to FIG 3 lights up.

线性映射（精确）Linear maps (exact)

$\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)$，代入协方差的定义：

$\mathbf y = A\mathbf x + \mathbf b$ with $\mathbf x$ having mean $\boldsymbol\mu$ and covariance $\Sigma$. By linearity of expectation (4.3), $E[\mathbf y] = A\boldsymbol\mu + \mathbf b$, so $\mathbf y - E[\mathbf y] = A(\mathbf x - \boldsymbol\mu)$; insert into the definition of covariance:

$$ \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 的塌缩）。

Second line: a Gaussian through a linear map is again Gaussian (the closure of 4.4), and the ellipse area is multiplied by $|\det A|$. If $A$ is rank-deficient, $A\Sigma A^\top$ is singular: the output lives in a lower-dimensional subspace (the collapse in FIG 2).

非线性映射（近似）：线性化Nonlinear maps (approximate): linearization

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

Use the first-order Taylor expansion (Lesson 3.3: the Jacobian is a "local linear map") $f(\mathbf x) \approx f(\boldsymbol\mu) + J(\mathbf x - \boldsymbol\mu)$, $J = \partial f/\partial\mathbf x\big|_{\boldsymbol\mu}$, and apply the linear formula above:

$$ \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$ 正好一致。这就是香蕉重心向内偏的数学来历。

This approximation drops the second-order terms. Including them, the mean gets the correction $E[f_i(\mathbf x)] \approx f_i(\boldsymbol\mu) + \tfrac12\mathrm{tr}\big(H_i\Sigma\big)$, with $H_i$ the Hessian of component $i$ (Lesson 3.5). Example: for $f(x) = x^2$, $H = 2$ and the correction is $\sigma^2$, matching the exact $E[x^2] = \mu^2 + \sigma^2$. Another: in polar coordinates $\partial^2 x/\partial\theta^2 = -r\cos\theta$, a correction of $-\tfrac12 r\cos\theta\,\sigma_\theta^2$; for Gaussian $\theta$ the exact value is $E[x] = r\,e^{-\sigma_\theta^2/2}\cos\theta_0$, and expanding $e^{-s/2} \approx 1 - s/2$ agrees. That is the math of the banana's center of mass sitting inwards.

无迹变换（sigma 点）The unscented transform (sigma points)

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

For an $n$-dimensional input let $L$ be a square root of $\Sigma = LL^\top$ (Cholesky). Take $2n+1$ sigma points and weights (here $\kappa = 3-n$, so $n+\kappa = 3$, a common choice for Gaussians):

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

$\mathbf l_i$ is the $i$-th column of $L$. The weighted mean and covariance of these points equal the input's $\boldsymbol\mu, \Sigma$ exactly (check it), so it is exact for linear functions; for nonlinear ones it also captures (part of) the second-order effects, with no derivatives. For $n=2$ that is 5 points with weights $1/3,\ 1/6,\ 1/6,\ 1/6,\ 1/6$.

6机器人中的它：从机械臂的关节噪声到里程计漂移In robotics: from arm joint noise to odometry drift

(a) 机械臂：关节噪声如何变成手爪误差(a) A manipulator: how joint noise becomes hand error

平面二连杆臂的手爪位置是 $\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$ 的两列平行，手爪沿着径向"动不了"，误差椭圆塌成一条线。拖动手臂的关节。

A planar two-link arm's hand position is $\mathbf p = f(q_1, q_2)$ and each joint angle has independent noise $\sigma_q$. The hand-position covariance is $J\,\sigma_q^2 I\,J^\top = \sigma_q^2 JJ^\top$, where $J$ is exactly Lesson 3.3's Jacobian. The error ellipse's principal axes are the singular directions of $J$ (Lesson 1.12). At a singular configuration (arm fully stretched or folded, $\det J = l_1 l_2\sin q_2 = 0$) the two columns of $J$ are parallel, the hand "cannot move" radially, and the error ellipse collapses to a line. Drag the arm's joints.

FIG 4二连杆臂：关节噪声 → 手爪椭圆Two-link arm: joint noise → hand ellipse

拖动肘部圆点改变 $q_1$，拖动手爪圆点改变 $q_2$。青色点是 300 次带噪声的手爪位置（噪声在关节上），黄色椭圆是 $\sigma_q^2 JJ^\top$ 算出的 2σ 椭圆，与点云吻合。 Drag the elbow dot to change $q_1$, the hand dot to change $q_2$. The cyan dots are 300 noisy hand positions (noise on the joints); the amber ellipse is the 2σ ellipse from $\sigma_q^2 JJ^\top$, matching the cloud.

把手臂拉直（$|q_2| < 5°$）：椭圆塌成一条垂直于手臂的细线，$\det J \to 0$。Straighten the arm ($|q_2| < 5°$): the ellipse collapses to a thin line perpendicular to the arm, $\det J \to 0$.
找出让椭圆最圆的弯曲角：使短轴/长轴超过 0.75。对这两节连杆（0.5 m 与 0.4 m），它在 $|q_2| \approx 130°$ 附近，而不是直觉上的 90°（90° 时只有 0.41）。Find the bend that makes the ellipse roundest: get the minor/major ratio above 0.75. For these links (0.5 m and 0.4 m) it lies near $|q_2| \approx 130°$, not the intuitive 90° (only 0.41 there).

(b) 移动机器人：里程计漂移(b) A mobile robot: odometry drift

一台机器人以 $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 次真实的带噪声模拟（青色）和这个协方差递推（黄色椭圆）。拖动时间滑块看不确定性怎样随时间"流"出去，试试不同路径和噪声。

A robot drives forward at $v = 1$ m/s. In every $\Delta t = 0.2$ s step its linear and angular velocities each carry a little independent noise $\sigma_v, \sigma_\omega$. The state is $\mathbf s = (x, y, \theta)$ and each step is $\mathbf s_{k+1} = f(\mathbf s_k, \mathbf u_k)$ (the same as in 4.8). With no external measurement, the covariance can only be propagated step by step:

$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)$.

The figure runs 300 actual noisy simulations (cyan) and this covariance recursion (amber ellipse) side by side. Drag the time slider to see uncertainty "flow" outward over time, and try different paths and noise levels.

FIG 5航位推算：不确定性随时间增长Dead reckoning: uncertainty grows with time

中性色粗实线是理想（无噪声）轨迹（虚线是它的完整路线）；青色细线是 40 条带噪声的轨迹，青色点是全部 300 条在当前时刻的位置；黄色椭圆是协方差递推在当前时刻给出的位置 2σ 椭圆，淡黄色轮廓是之前每隔 2 秒的椭圆；椭圆里的短线是航向 ±2σ。读数里的"覆盖率"是点云落入当前 2σ 椭圆的比例，理论值（二维高斯）是 86.5%。 The thick neutral solid line is the ideal (noise-free) path (dashed: its full route); thin cyan lines are 40 noisy paths, and the cyan dots are all 300 at the current time; the amber ellipse is the 2σ position ellipse from the covariance recursion at the current time, faint amber outlines are the ellipses every 2 s before; the short ticks in the ellipse show heading ±2σ. "Coverage" in the readout is the fraction of the cloud inside the current 2σ ellipse; the theoretical value (2D Gaussian) is 86.5%.

选"直线"，把时间拉到最后：横向（垂直于行驶方向）的误差远大于沿行驶方向的误差。为什么？Pick "Straight" and drag time to the end: the lateral error (perpendicular to travel) far exceeds the along-track error. Why?
把 $\sigma_\omega$ 设为 0：横向误差消失，只剩沿行驶方向的误差；航向噪声才是漂移的"放大器"。Set $\sigma_\omega$ to 0: the lateral error vanishes and only along-track error remains; heading noise is the "amplifier" of drift.
选"直线"，把 $\sigma_\omega$ 加到 0.2 以上，时间拉到最后：点云弯成一道弧形的"扇面"，覆盖率掉到 80% 以下。一个椭圆（高斯）装不下弯曲的分布，这正是线性化的极限。Pick "Straight", raise $\sigma_\omega$ to 0.2 or more and drag time to the end: the cloud fans out into a curved "banana" and coverage drops below 80%. One ellipse (a Gaussian) cannot hold a curved distribution: this is the limit of linearization.

真实系统里In real systems

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

This explains why a robot with odometry alone "gets lost as it goes", and why heading matters more than position: a heading error is magnified by distance into a lateral error. Real systems therefore measure heading precisely with a gyro/IMU and periodically pull the estimate back with laser, vision or GPS (the update step of 4.8). ROS's nav_msgs/Odometry message carries a 6×6 pose covariance for exactly this, and planners and localizers use it to decide how much to trust the odometry.

对直线行驶，可以估算（小角度近似，$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$），航向误差再沿路积分成横向位移，所以横向误差增长快得多。

For straight-line driving one can estimate (small-angle approximation, $N = t/\Delta t$ steps): along-track $\sigma_{\parallel} \approx \sigma_v\sqrt{\Delta t\, t}$, growing like $\sqrt t$; lateral $\sigma_\perp \approx \dfrac{v\,\sigma_\omega\sqrt{\Delta t}}{\sqrt 3}\,t^{3/2}$, growing like $t^{3/2}$. Heading noise first accumulates into a heading error ($\propto\sqrt t$), and the heading error is then integrated along the path into lateral displacement, so the lateral error grows much faster.

7常见误区Common pitfalls

误区 1：线性化永远够用Pitfall 1: linearization is always good enough

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

Linearization presumes the input blob is smaller than the scale at which the function bends. At a flat spot of FIG 1 where $f' = 0$, $J\Sigma J^\top = 0$ claims "no uncertainty" while the true output is widely spread; the large-$\sigma_\theta$ banana of FIG 3 is the same story. The EKF then becomes overconfident and unstable. Remedies: inflate the process noise $Q$ for margin, use a UKF, or propagate in smaller steps.

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

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

True only for linear functions. For convex $f$, $E[f(x)] \ge f(E[x])$ (Jensen's inequality): the mean of $x^2$ exceeds the square of the mean by $\sigma^2$. Treating the "square of a noisy distance" as "the square of the distance" gives a systematically large result.

误区 3：忘了平方，或只乘一边Pitfall 3: forgetting the square, or multiplying one side only

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

The standard deviation scales by $|a|$ and the variance by $a^2$; the covariance matrix becomes $A\Sigma A^\top$, not $A\Sigma$ or $A\sqrt\Sigma A^\top$. And when adding independent errors it is the variances that add, not the standard deviations.

误区 4：假设各步噪声互相独立Pitfall 4: assuming noise at different steps is independent

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

$P_{k+1} = FPF^\top + GQG^\top$ assumes independent noise at each step. Real odometry has systematic errors (a miscalibrated wheel radius or wheel base) that push the same way every step and accumulate far faster than a random walk (linearly instead of as $\sqrt t$). Calibration errors must be modelled as slowly varying bias states, not white noise.

误区 5：在错误的点求雅可比Pitfall 5: evaluating the Jacobian at the wrong point

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

$J$ must be evaluated at the current estimate (the mean), not at the initial point or at the truth (which the robot does not know). $F_k$ changes at every step because $\theta_k$ does (see the third column of $F$ in 4.8).

8小练习Check yourself

1. 激光读数 $x$ 的标准差是 0.5 m。换算 $y = 3x + 2$ 后，$y$ 的标准差是？1. A laser reading $x$ has standard deviation 0.5 m. After converting to $y = 3x + 2$, what is the standard deviation of $y$?

平移不改变分散程度，缩放让标准差乘以 $|3|$：$\sigma_y = 3 \times 0.5 = 1.5$。（方差是 $9 \times 0.25 = 2.25$，别把方差当标准差。）A shift does not change the spread, scaling multiplies the standard deviation by $|3|$: $\sigma_y = 3 \times 0.5 = 1.5$. (The variance is $9 \times 0.25 = 2.25$; do not confuse it with the standard deviation.)

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$ 的协方差是？2. $\mathbf x \sim \mathcal N(\mathbf 0, I_2)$ and $A = \begin{bmatrix}2 & 0\\ 0 & 0.5\end{bmatrix}$. What is the covariance of $\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。$A\Sigma A^\top = AA^\top = \mathrm{diag}(2^2, 0.5^2) = \mathrm{diag}(4, 0.25)$. Standard deviations are doubled and halved, variances quadrupled and quartered.

3. $y = x^2$，输入 $x \sim \mathcal N(0, 0.3^2)$。线性化（在 $\mu = 0$ 处）对 $y$ 的方差预测是？这说明什么？3. $y = x^2$ with input $x \sim \mathcal N(0, 0.3^2)$. What variance does linearization (at $\mu = 0$) predict for $y$, and what does that show?

$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，弯曲才是主角。$J = f'(0) = 0$ so $J\Sigma J^\top = 0$. But $y = x^2 \ge 0$ has mean $\sigma^2 = 0.09$ and variance $2\sigma^4 \approx 0.016$, nowhere near 0. Linearization sees only the slope, which is zero here; the curvature is the star.

4. 机器人直线行驶，只有航向噪声。横向位置误差的标准差随时间 $t$ 大致怎样增长？4. A robot drives straight with only heading noise. Roughly how does the lateral position error's standard deviation grow with time $t$?

航向误差是随机游走，标准差 $\propto \sqrt t$；横向位置是它沿路程的积分，再乘一个 $t$：$\propto t^{3/2}$。所以横向漂移比沿行驶方向（$\propto \sqrt t$）增长快得多。The heading error is a random walk with standard deviation $\propto \sqrt t$; lateral position is its integral along the path, one more factor of $t$: $\propto t^{3/2}$. That is why lateral drift grows much faster than along-track error ($\propto \sqrt t$).

5. 对 2 维输入，无迹变换需要把多少个 sigma 点送过非线性函数？5. For a 2D input, how many sigma points does the unscented transform push through the nonlinear function?

$2n + 1 = 5$：均值本身，加上沿 $L$ 的每一列正负各一个点。成本只有 5 次函数调用，远少于蒙特卡洛的成百上千次。$2n + 1 = 5$: the mean itself plus one point on each side along every column of $L$. Just 5 function calls, far fewer than Monte Carlo's hundreds or thousands.

9延伸阅读Further reading

S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 3 (Sec. 3.3 EKF, Sec. 3.4 UKF) and Ch. 5 — 线性化预测、无迹变换，以及里程计运动模型Linearized prediction, the unscented transform, and odometry motion models
T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 2 and Ch. 4 — 高斯过线性/非线性变换，以及三种近似（线性化、sigma 点、蒙特卡洛）的比较Gaussians through linear and nonlinear maps, and a comparison of the three approximations (linearization, sigma points, Monte Carlo)
S. J. Julier & J. K. Uhlmann, "Unscented Filtering and Nonlinear Estimation", Proceedings of the IEEE, 92(3), 2004 — 无迹变换的经典综述The classic review of the unscented transform
R. Siegwart, I. R. Nourbakhsh & D. Scaramuzza, Introduction to Autonomous Mobile Robots, MIT Press, 2nd ed., 2011, Ch. 5 — 差速机器人的里程计误差模型与协方差传播The odometry error model and covariance propagation for a differential-drive robot
K. M. Lynch & F. C. Park, Modern Robotics, Cambridge University Press, 2017 — 雅可比与奇异位形（FIG 4 的背景）Jacobians and singular configurations (the background of FIG 4)