一、前置知识

1.1 概率基础

在处理随机现象时，我们不仅关心一个量的典型取值，还关心它会如何波动、多个量之间是否相关，以及估计结果可能偏离真实值多少。期望、方差、协方差和误差协方差正是描述这些问题的基本语言。

期望值

随机变量 \(X\) 的期望记为 \(E[X]\)。它可以理解为：在大量重复试验中，\(X\) 的加权平均结果；出现概率越高的取值，权重越大。

对于离散随机变量，若 \(X\) 取值为 \(x_i\) 的概率为 \(p_i\)，则

\[ E[X]=\sum_i x_i p_i \]

对于连续随机变量，若概率密度函数为 \(f_X(x)\)，则

\[ E[X]=\int_{-\infty}^{\infty}x f_X(x)\,dx \]

若有 \(N\) 个等可能的数据点，每个数据点的概率都是 \(1/N\)，期望就退化为熟悉的算术平均数：

\[ E[X]=\frac{1}{N}\sum_{i=1}^{N}x_i \]

在估计问题中，期望通常可以视为对未知量的中心估计。

方差与协方差

期望只描述“中心在哪里”，却没有说明数据围绕这个中心分散得有多厉害。方差用来衡量这种波动程度：

\[ \operatorname{Var}(X)=\sigma_x^2=E\left[(X-E[X])^2\right] \]

方差越大，说明 \(X\) 的不确定性越高；方差越小，说明取值更集中在期望附近。

当同时关心两个变量 \(X\) 和 \(Y\) 时，还需要知道它们是否会一起变化。协方差定义为：

\[ \operatorname{Cov}(X,Y)=\sigma_{xy} =E\left[(X-E[X])(Y-E[Y])\right] \]

协方差为正，表示二者通常同向偏离各自的期望；协方差为负，表示一方偏大时另一方往往偏小；接近零则表示线性关联较弱。注意，协方差为零不一定意味着两个变量相互独立。

两个随机变量之和的方差

方差描述偏离均值的程度。两个量相加时，除了各自有多分散，还要考虑它们的偏差是否会一起出现，因此一般不能只把两个方差相加。

设随机变量 \(X,Y\) 都有有限方差，均值分别为 \(\mu_X=E[X]\)、\(\mu_Y=E[Y]\)。期望具有线性性质，不需要独立性就有

\[ E[X+Y]=E[X]+E[Y]=\mu_X+\mu_Y. \]

所以，和变量偏离自身均值的量，可以拆成两个变量各自的偏差：

\[ (X+Y)-E[X+Y]=(X-\mu_X)+(Y-\mu_Y). \]

将它代入方差定义，再像普通代数一样展开平方：

\[ \begin{aligned} \operatorname{Var}(X+Y) &=E\left[\big((X-\mu_X)+(Y-\mu_Y)\big)^2\right]\\ &=E\left[(X-\mu_X)^2 +2(X-\mu_X)(Y-\mu_Y) +(Y-\mu_Y)^2\right]\\ &=E[(X-\mu_X)^2] +2E[(X-\mu_X)(Y-\mu_Y)] +E[(Y-\mu_Y)^2]. \end{aligned} \]

最后一步只用了期望的线性性质：和的期望等于各项期望之和，常数 2 可以提出期望符号。根据前面已经给出的定义，三项分别是 \(X\) 的方差、两倍协方差和 \(Y\) 的方差。因此

\[ \boxed{ \operatorname{Var}(X+Y) =\operatorname{Var}(X) +2\operatorname{Cov}(X,Y) +\operatorname{Var}(Y) }. \]

这里的系数 2 来自平方展开中的交叉项。协方差为正时，两种偏差倾向同向叠加，和的方差更大；协方差为负时，偏差倾向互相抵消，和的方差更小。这个推导既不要求变量独立，也不要求它们的均值为零。

如果 \(X,Y\) 独立，则

\[ E[(X-\mu_X)(Y-\mu_Y)] =E[X-\mu_X]E[Y-\mu_Y]=0, \]

交叉项才消失，得到 \(\operatorname{Var}(X+Y)=\operatorname{Var}(X)+\operatorname{Var}(Y)\)。实际上，只要协方差为零，这个简式就成立；独立是充分条件。反过来看，若 \(Y=X\)，两个偏差完全同向，便有 \(\operatorname{Var}(X+X)=4\operatorname{Var}(X)\)，而不是 \(2\operatorname{Var}(X)\)。

将 \(X,Y\) 分别记为 \(e_p,e_v\)，便得到后文使用的代数关系：

\[ \operatorname{Var}(e_p+e_v) =\operatorname{Var}(e_p)+2\operatorname{Cov}(e_p,e_v) +\operatorname{Var}(e_v). \]

在机器人运动中，\(e_p\) 是位置误差，\(e_v\) 是速度误差，二者单位不同。经过时间 \(T\) 后，应当相加的是 \(e_p\) 与位移误差 \(Te_v\)。用相同的展开方法得到

\[ \operatorname{Var}(e_p+Te_v) =\operatorname{Var}(e_p) +2T\operatorname{Cov}(e_p,e_v) +T^2\operatorname{Var}(e_v). \]

后文以米、秒为单位并取 \(T=1\) 秒进行数值计算时，时间系数的数值为 1，才写成前面的简化形式。

误差协方差

在许多估计问题中，除了估计值本身，我们还关心它与真实值之间相差多少。设真实状态为 \(x\)，估计值为 \(\hat{x}\)，定义误差为

\[ e=x-\hat{x} \]

误差协方差就是该误差向量的协方差矩阵：

\[ P=E\left[(e-E[e])(e-E[e])^T\right] \]

若估计是无偏的，即 \(E[e]=0\)，上式可简化为

\[ P=E\left[ee^T\right] \]

以二维误差为例，

\[ e= \begin{bmatrix} e_x\\ e_y \end{bmatrix} \]

则

\[ P= \begin{bmatrix} \operatorname{Var}(e_x) & \operatorname{Cov}(e_x,e_y)\\ \operatorname{Cov}(e_x,e_y) & \operatorname{Var}(e_y) \end{bmatrix} \]

对角线元素分别表示各个方向上的误差大小，非对角线元素表示这些误差是否相关。无论是协方差矩阵 \(\Sigma\)，还是一般的误差协方差矩阵 \(P\)，都可以借助这种形式描述不确定性的形状和方向。

1.2 全概率公式

在很多概率问题中，我们关心的是最终结果，却看不到它经历了哪一种中间状态。全概率公式提供了一种统一思路：分别计算每个中间状态对结果的贡献，再把这些贡献加起来。

离散全概率公式

设 \(B_1,B_2,\ldots\) 是对所有可能情况的一个划分：它们两两互斥，并且其中必有一种发生。要计算事件 \(A\) 的概率，可以先在每个 \(B_i\) 发生的前提下计算 \(A\)，再按 \(B_i\) 本身发生的概率加权：

\[ P(A)=\sum_i P(A|B_i)P(B_i) \]

其中，\(P(A|B_i)\) 表示“处于第 \(i\) 种情况时发生 \(A\) 的概率”，\(P(B_i)\) 表示“第 \(i\) 种情况本身出现的概率”。因此，每一项 \(P(A|B_i)P(B_i)\) 都是一条可能路径对 \(A\) 的贡献。

例如，把学生的复习情况划分为“充分”“一般”“不足”三种互斥情况，记为 \(B_1,B_2,B_3\)。那么考试通过的总概率为：

\[ \begin{aligned} P(\text{通过})={}&P(\text{通过}|B_1)P(B_1)\\ &+P(\text{通过}|B_2)P(B_2)\\ &+P(\text{通过}|B_3)P(B_3) \end{aligned} \]

换句话说，全概率公式不是直接猜测结果，而是把所有可能的中间情况都纳入计算。

连续形式与隐变量

如果中间状态不再是有限个类别，而是可以连续取值的变量 \(Z\)，上面的“求和”便相应变为“积分”：

\[ p(x)=\int p(x|z)p(z)\,dz \]

这就是连续型全概率公式。这里 \(z\) 是一个可能看不见、却会影响观测 \(X\) 的隐变量；\(p(z)\) 描述它出现的可能性，\(p(x|z)\) 描述给定 \(z\) 后观测到 \(x\) 的可能性。积分的含义仍然没有变：把每一个可能的 \(z\) 对 \(x\) 的贡献都加起来。

从联合分布的角度看：

\[ p(x,z)=p(x|z)p(z) \]

当我们只能观测到 \(X\)、却不关心或无法直接观测 \(Z\) 时，就对 \(Z\) 求和或积分：

\[ P(X=x)=\sum_z P(X=x|Z=z)P(Z=z) \]

\[ p(x)=\int p(x|z)p(z)\,dz \]

这个把隐变量“消去”的过程称为边缘化（marginalization）。它不是忽略看不见的原因，而是把它所有可能的取值都考虑进去。

卷积：全概率公式的一个特殊情形

现在来看卷积。设两个随机变量相互独立，并定义：

\[ Z=X+Y \]

若想知道 \(Z\) 取某个值 \(z\) 的概率，就不能只看一种组合，而要枚举所有能满足 \(X+Y=z\) 的拆分方式。若 \(X=x\)，那么 \(Y\) 必须恰好取 \(z-x\)，因此离散情形下：

\[ P(Z=z)=\sum_x P(X=x)P(Y=z-x) \]

连续情形下，求和变为积分：

\[ f_Z(z)=\int f_X(x)f_Y(z-x)\,dx \]

这就是卷积。它与全概率公式的结构完全一致：\(x\) 扮演被枚举或积分掉的中间变量，每一种 \(x\) 都对应一种使结果达到 \(z\) 的可能分解。

卷积中差值的来源

卷积中的 \(z-x\) 并不是人为凑出来的。我们先固定最终结果：

\[ X+Y=z \]

一旦选定 \(X=x\)，另一个变量就被这个约束唯一确定：

\[ Y=z-x \]

所以，\(f_Y(z-x)\) 是 \(Y\) 在补足到目标 \(z\) 所需位移处的密度值，不是连续变量恰好取该值的概率。卷积所做的事，就是对所有这样的拆分方式逐一计算密度贡献，再把它们积分起来。

从上述可以得到三点：

1. 全概率公式：对所有可能的中间状态加权求和。
2. 边缘化：把不可见或不关心的变量通过求和/积分消去。
3. 卷积：当最终变量是两个独立随机变量之和时，全概率公式得到的具体形式。

隐变量是模型中未被直接观测的变量，可以表示真实内部状态，也可以是为解释数据而引入的潜在因素。概率模型通过对它求和或积分，把这些隐藏可能性纳入计算。

1.3 高斯分布

高斯分布（也称正态分布）是概率论与统计学中最常见的分布之一。它的优势在于：只需均值和协方差，就能同时描述数据的中心位置、各方向的不确定性以及变量之间的相关性。

一维高斯分布

一维高斯分布的概率密度函数为

\[ f(x)=\frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right) \]

其中，\(\mu\) 是均值，决定曲线的中心位置；\(\sigma^2\) 是方差，决定曲线的宽窄。方差越大，曲线越平缓，意味着我们对 \(x\) 的不确定性越大。

高斯随机变量之和

两个独立的高斯随机变量之和仍然服从正态分布。两个高斯随机变量的乘积一般不服从高斯分布；对同一个自变量相乘的两个高斯概率密度函数，才与一个新的高斯密度成正比。这是两种不同的乘法。

设

\[ X\sim\mathcal N(\mu_1,\sigma_1^2), \qquad Y\sim\mathcal N(\mu_2,\sigma_2^2) \]

并且 \(X,Y\) 独立。

定义

\[ Z=X+Y. \]

要证明：

\[ Z\sim \mathcal N(\mu_1+\mu_2,\ \sigma_1^2+\sigma_2^2) \]

用卷积推导

因为 \(X,Y\) 独立，所以 \(Z=X+Y\) 的概率密度是卷积：

\[ p_Z(z) = \int_{-\infty}^{+\infty} p_X(x)p_Y(z-x)\,dx. \]

两个高斯的密度分别为

\[ p_X(x) = \frac{1}{\sqrt{2\pi\sigma_1^2}} \exp \left[ -\frac{(x-\mu_1)^2}{2\sigma_1^2} \right] \]

和

\[ p_Y(y) = \frac{1}{\sqrt{2\pi\sigma_2^2}} \exp \left[ -\frac{(y-\mu_2)^2}{2\sigma_2^2} \right]. \]

令 \(y=z-x\)，那么

\[ p_Z(z) = \frac{1}{2\pi\sigma_1\sigma_2} \int \exp\left[ -\frac{(x-\mu_1)^2}{2\sigma_1^2} -\frac{(z-x-\mu_2)^2}{2\sigma_2^2} \right]dx. \]

关键就是把关于 \(x\) 的指数配方。

令

\[ A=\frac1{\sigma_1^2}+\frac1{\sigma_2^2}. \]

指数内部可以整理成

\[ \frac{(x-\mu_1)^2}{\sigma_1^2} + \frac{(z-x-\mu_2)^2}{\sigma_2^2}. \]

展开：

\[ = x^2 \left( \frac1{\sigma_1^2}+\frac1{\sigma_2^2} \right) - 2x \left[ \frac{\mu_1}{\sigma_1^2} + \frac{z-\mu_2}{\sigma_2^2} \right] + \frac{\mu_1^2}{\sigma_1^2} + \frac{(z-\mu_2)^2}{\sigma_2^2}. \]

对 \(x\) 完成平方后，可以写成

\[ A(x-m)^2 + \frac{(z-\mu_1-\mu_2)^2} {\sigma_1^2+\sigma_2^2}, \]

其中 \[ m=\frac{\mu_1/\sigma_1^2+(z-\mu_2)/\sigma_2^2}{A} \] 依赖于保留的变量 \(z\)，不依赖积分变量 \(x\)。对固定 \(z\)，有 \(\int e^{-A(x-m)^2/2}\,dx=\sqrt{2\pi/A}\)，因此可以消去 \(x\) 并得到下面的归一化系数。

因此

\[ p_Z(z) \propto \exp \left[ -\frac{(z-(\mu_1+\mu_2))^2} {2(\sigma_1^2+\sigma_2^2)} \right]. \]

积分掉 \(x\) 后，剩余归一化系数正好是

\[ \frac{1} {\sqrt{2\pi(\sigma_1^2+\sigma_2^2)}}. \]

于是：

\[ \boxed{ p_Z(z) = \frac{1} {\sqrt{2\pi(\sigma_1^2+\sigma_2^2)}} \exp \left[ -\frac{(z-(\mu_1+\mu_2))^2} {2(\sigma_1^2+\sigma_2^2)} \right] } \]

所以：

\[ \boxed{ X+Y \sim \mathcal N (\mu_1+\mu_2,\sigma_1^2+\sigma_2^2) } \]

这就是所谓：

\[ \boxed{\text{高斯 * 高斯 = 高斯}} \]

这里的 \(*\) 是卷积。

高斯概率密度函数相乘

现在不是随机变量 \(XY\)，而是两个 PDF（概率密度函数） 对同一个变量 \(x\) 相乘。

设

\[ p_1(x) = \mathcal N(x;\mu_1,\sigma_1^2) \]

和

\[ p_2(x) = \mathcal N(x;\mu_2,\sigma_2^2). \]

即：

\[ p_1(x) = \frac1{\sqrt{2\pi\sigma_1^2}} \exp \left[ -\frac{(x-\mu_1)^2}{2\sigma_1^2} \right] \]

\[ p_2(x) = \frac1{\sqrt{2\pi\sigma_2^2}} \exp \left[ -\frac{(x-\mu_2)^2}{2\sigma_2^2} \right]. \]

两者相乘：

\[ p_1(x)p_2(x) = \frac1{2\pi\sigma_1\sigma_2} \exp \left[ -\frac12 \left( \frac{(x-\mu_1)^2}{\sigma_1^2} + \frac{(x-\mu_2)^2}{\sigma_2^2} \right) \right]. \]

把括号展开：

\[ \frac{(x-\mu_1)^2}{\sigma_1^2} + \frac{(x-\mu_2)^2}{\sigma_2^2} \]

\[ = \left( \frac1{\sigma_1^2} + \frac1{\sigma_2^2} \right)x^2 - 2 \left( \frac{\mu_1}{\sigma_1^2} + \frac{\mu_2}{\sigma_2^2} \right)x + \frac{\mu_1^2}{\sigma_1^2} + \frac{\mu_2^2}{\sigma_2^2}. \]

我们希望把它重新写成

\[ \frac{(x-\mu)^2}{\sigma^2} +\text{与 }x\text{ 无关的常数}. \]

比较 \(x^2\) 的系数：

\[ \frac1{\sigma^2} = \frac1{\sigma_1^2} + \frac1{\sigma_2^2} \]

因此：

\[ \sigma^2 = \left( \frac1{\sigma_1^2} + \frac1{\sigma_2^2} \right)^{-1} \]

也就是：

\[ \sigma^2 = \frac{\sigma_1^2\sigma_2^2} {\sigma_1^2+\sigma_2^2} \]

再比较 \(x\) 的一次项：

\[ \frac{\mu}{\sigma^2} = \frac{\mu_1}{\sigma_1^2} + \frac{\mu_2}{\sigma_2^2}. \]

所以：

\[ \mu = \sigma^2 \left( \frac{\mu_1}{\sigma_1^2} + \frac{\mu_2}{\sigma_2^2} \right) \]

代入刚才的 \(\sigma^2\)：

\[ \mu = \frac{ \sigma_2^2\mu_1+\sigma_1^2\mu_2 }{ \sigma_1^2+\sigma_2^2 } \]

因此最终得到

\[ \boxed{ p_1(x)p_2(x) \propto \mathcal N(x;\mu,\sigma^2) } \]

其中

\[ \boxed{ \sigma^2 = \left( \sigma_1^{-2}+\sigma_2^{-2} \right)^{-1} } \]

\[ \boxed{ \mu = \sigma^2 \left( \sigma_1^{-2}\mu_1+ \sigma_2^{-2}\mu_2 \right) } \]

1.4 多元高斯分布

要理解多维高斯分布，关键是先理解它的等密度线为什么会是椭圆。

从标准椭圆到二次型

以中心为 \((x_0,y_0)\)、两条半轴长分别为 \(a,b\) 的标准椭圆为例：

\[ \frac{(x-x_0)^2}{a^2}+\frac{(y-y_0)^2}{b^2}=1 \]

将二维坐标写成向量

\[ \mathbf{x}= \begin{bmatrix} x\\ y \end{bmatrix}, \qquad \mathbf{x}_0= \begin{bmatrix} x_0\\ y_0 \end{bmatrix} \]

并定义

\[ Q= \begin{bmatrix} 1/a^2 & 0\\ 0 & 1/b^2 \end{bmatrix} \]

椭圆方程就可以简洁地写成

\[ (\mathbf{x}-\mathbf{x}_0)^TQ(\mathbf{x}-\mathbf{x}_0)=1 \]

这种“向量—矩阵—向量”的二次型写法，正是多元高斯分布指数部分的结构。

旋转后的椭圆

当椭圆的主轴不再与坐标轴平行时，方程形式保持不变：

\[ (\mathbf{x}-\mathbf{x}_0)^TQ(\mathbf{x}-\mathbf{x}_0)=1 \]

但此时 \(Q\) 是一个对称正定矩阵，例如

\[ Q= \begin{bmatrix} q_{11} & q_{12}\\ q_{12} & q_{22} \end{bmatrix} \]

展开后得到

\[ q_{11}(x-x_0)^2 +2q_{12}(x-x_0)(y-y_0) +q_{22}(y-y_0)^2=1 \]

其中的交叉项 \((x-x_0)(y-y_0)\) 反映了两个方向之间的关联；当 \(q_{12}\ne0\) 时，椭圆一般会发生旋转。

多元高斯分布

\(n\) 维高斯分布的概率密度函数为

\[ f(\mathbf{x})= \frac{1}{(2\pi)^{n/2}|\mathbf{\Sigma}|^{1/2}} \exp\left( -\frac{1}{2} (\mathbf{x}-\boldsymbol{\mu})^T \mathbf{\Sigma}^{-1} (\mathbf{x}-\boldsymbol{\mu}) \right) \]

其中，\(\boldsymbol{\mu}\) 是均值向量，\(\mathbf{\Sigma}\) 是协方差矩阵。固定概率密度时，指数中的二次型必须为常数，因此等密度线满足

\[ (\mathbf{x}-\boldsymbol{\mu})^T \mathbf{\Sigma}^{-1} (\mathbf{x}-\boldsymbol{\mu})=c \]

这正是椭圆（高维时为椭球）的方程。这里假设协方差矩阵正定；奇异协方差对应集中在低维子空间上的退化高斯，不能直接使用上述密度公式。均值决定椭圆中心，协方差矩阵的特征向量决定主轴方向，特征值 \(\lambda_i\) 对应的半轴长为 \(\sqrt{c\lambda_i}\)。等密度椭圆不是取值边界：非退化高斯在整个 \(\mathbb R^n\) 上都有正密度。下面的图展示了不同协方差矩阵如何改变二维高斯分布的形状与朝向。

1.5 离散贝叶斯

贝叶斯公式回答的是一个很自然的问题：当观察到新数据后，应当怎样更新对不同假设的判断？当候选假设是有限个或可数个时，可以直接使用离散形式。

设 \(H\) 是一个假设，\(D\) 是观察到的数据，则

\[ P(H|D)=\frac{P(D|H)P(H)}{P(D)} \]

其中分母 \(P(D)\) 是数据出现的总概率。若假设空间由互斥且完备的 \(H_1,\ldots,H_n\) 构成，则由全概率公式可得

\[ P(D)=\sum_{i=1}^{n}P(D|H_i)P(H_i) \]

因此，贝叶斯更新可以概括为：后验 = 似然 × 先验，再除以归一化常数。

似然函数

似然和概率使用同一个表达式，却是在不同的视角下看它。概率把参数视为已知，询问数据出现的可能性；似然把已经观察到的数据固定，用它比较不同参数的解释能力。

例如，设硬币正面朝上的概率为 \(\theta\)，连续抛掷 5 次后观察到

\[ D=HHHHH \]

给定 \(\theta\)，该数据出现的概率为

\[ P(D|\theta)=\theta^5 \]

观察到 \(D\) 后，把右侧看作关于 \(\theta\) 的函数：

\[ L(\theta)=P(D|\theta)=\theta^5 \]

例如，

\[ L(0.8)\approx0.328,\qquad L(0.9)\approx0.590,\qquad L(0.95)\approx0.774 \]

这些数值并不是“不同 \(\theta\) 取值各自发生的概率”；它们表示，在对应假设成立时，当前这组观测数据有多合理。于是似然函数不需要对所有 \(\theta\) 求和或积分为 1。例如 \(L(0.9)+L(0.95)>1\) 并不矛盾。

只有引入先验分布并完成归一化，\(P(\theta|D)\) 或 \(p(\theta|D)\) 才成为关于参数 \(\theta\) 的概率分布。

例 1：盒子抽取

有两个盒子：

• 盒子 \(A\)：3 个红球、1 个蓝球；
• 盒子 \(B\)：1 个红球、3 个蓝球。

先等概率地选择一个盒子，因此

\[ P(A)=P(B)=\frac{1}{2} \]

再从选中的盒子里随机抽取一个球。现在观察到的是红球，记为事件 \(R\)。问题是：这个球来自盒子 \(A\) 的概率是多少？

由盒内构成可知

\[ P(R|A)=\frac{3}{4},\qquad P(R|B)=\frac{1}{4} \]

先计算观察到红球的总概率：

\[ P(R)=P(R|A)P(A)+P(R|B)P(B)=\frac{1}{2} \]

再代入贝叶斯公式：

\[ P(A|R) =\frac{P(R|A)P(A)}{P(R)} =\frac{\frac{3}{4}\cdot\frac{1}{2}}{\frac{1}{2}} =\frac{3}{4} \]

因此，在观察到红球后，来自盒子 \(A\) 的后验概率从 \(0.5\) 提升到了 \(0.75\)。红球更符合盒子 \(A\) 的构成，因此它为 \(A\) 提供了更多支持。

例 2：硬币偏置估计

设待选硬币的正面概率只能是以下 10 个候选值之一：

\[ p_i\in\{0.05,0.15,0.25,\ldots,0.95\} \]

在观察数据前，认为接近公平硬币的可能性更高。用以 \(0.5\) 为中心、标准差为 \(0.125\) 的离散高斯形状表示这一先验：

\[ P(p_i)= \frac{\exp\left[-\frac{(p_i-0.5)^2}{2(0.125)^2}\right]} {\sum_j\exp\left[-\frac{(p_j-0.5)^2}{2(0.125)^2}\right]} \]

现在连续抛掷该硬币 5 次，均为正面，即 \(D=HHHHH\)。若正面概率为 \(p_i\)，则

\[ P(D|p_i)=p_i^5 \]

据此更新得到

\[ P(p_i|D)= \frac{P(D|p_i)P(p_i)}{\sum_j P(D|p_j)P(p_j)} \]

数据本身更支持较大的 \(p_i\)，而先验更支持接近 \(0.5\) 的 \(p_i\)；后验则平衡了这两类信息。

最大似然估计与最大后验估计

最大似然估计（MLE）只使用数据：

\[ \hat p_{\mathrm{MLE}} =\arg\max_{p_i}P(D|p_i) =\arg\max_{p_i}p_i^5 =0.95 \]

最大后验估计（MAP）同时使用先验与数据：

\[ \hat p_{\mathrm{MAP}} =\arg\max_{p_i}P(p_i|D) =\arg\max_{p_i}P(D|p_i)P(p_i) =0.65 \]

这个例子说明：MLE 追求“最能解释数据”的参数；MAP 则在此基础上保留了先验信息的影响。

1.6 连续贝叶斯

当未知参数 \(\theta\) 可以在一个连续区间内取值时，不能再逐项相加，而应使用概率密度与积分。连续贝叶斯公式为

\[ p(\theta|x) =\frac{p(x|\theta)p(\theta)}{p(x)} =\frac{p(x|\theta)p(\theta)}{\int p(x|\theta)p(\theta)\,d\theta} \]

这里 \(p(\theta)\) 是先验密度，\(p(x|\theta)\) 是似然，\(p(\theta|x)\) 是后验密度；分母 \(p(x)\) 是观测 \(x\) 的边缘密度。它确保后验密度在整个参数空间上的积分为 1。

比例形式与归一化

在推导后验关于 \(\theta\) 的形状时，分母 \(p(x)\) 与 \(\theta\) 无关，因此可以先写为

\[ p(\theta|x)\propto p(x|\theta)p(\theta) \]

归一化只会整体缩放曲线，并不会改变其峰值、中心或相对形状。例如

\[ 10e^{-(\theta-5)^2} \quad\text{与}\quad e^{-(\theta-5)^2} \]

具有相同的形状，二者仅相差一个常数倍。

这在正态分布中尤其方便。若

\[ p(\theta|x)\propto \exp\left[-\frac{(\theta-23.2)^2}{2\times3.2}\right] \]

便可直接识别出

\[ \theta|x\sim\mathcal N(23.2,3.2) \]

其中第二个参数是方差；相应的归一化系数为 \(1/\sqrt{2\pi\times3.2}\)。

正态先验与正态测量

一个最常用的情形是：先验分布与测量噪声都服从正态分布。设

\[ \theta\sim\mathcal N(\mu_0,\sigma_0^2), \qquad x|\theta\sim\mathcal N(\theta,\sigma_m^2) \]

则后验仍为正态分布：

\[ \theta|x\sim\mathcal N(\mu_{\mathrm{post}},\sigma_{\mathrm{post}}^2) \]

其中

\[ \sigma_{\mathrm{post}}^2 =\left(\frac{1}{\sigma_0^2}+\frac{1}{\sigma_m^2}\right)^{-1} \]

\[ \mu_{\mathrm{post}} =\sigma_{\mathrm{post}}^2 \left(\frac{\mu_0}{\sigma_0^2}+\frac{x}{\sigma_m^2}\right) \]

详情见高斯概率密度函数相乘。这两个公式体现了一个直观原则：不确定性更小的信息，其权重更大。

例 1：测量真实温度

设物体的真实温度为 \(\theta\)。根据经验，温度的先验分布为

\[ \theta\sim\mathcal N(20,4^2) \]

传感器给出测量值 \(x=24^\circ\mathrm{C}\)，且其测量标准差为 \(2^\circ\mathrm{C}\)：

\[ x|\theta\sim\mathcal N(\theta,2^2) \]

代入上述公式：

\[ \sigma_{\mathrm{post}}^2 =\left(\frac{1}{4^2}+\frac{1}{2^2}\right)^{-1} =3.2 \]

\[ \mu_{\mathrm{post}} =3.2\left(\frac{20}{16}+\frac{24}{4}\right) =23.2 \]

因此

\[ \boxed{\theta|x\sim\mathcal N(23.2,3.2)} \]

后验均值位于 \(20\) 与 \(24\) 之间，并且更接近测量值 \(24\)，因为传感器的标准差更小，提供的信息更可靠。

我们再深入理解一下，可以展开模型：

\[ p(x\mid\theta) = \frac1{\sqrt{2\pi}\,2} \exp\left[-\frac{(x-\theta)^2}{2\cdot 2^2}\right]. \]

先验是

\[ p(\theta) = \frac1{\sqrt{2\pi}\,4} \exp\left[-\frac{(\theta-20)^2}{2\cdot4^2}\right]. \]

如果要算贝叶斯公式的分母：

\[ p(x)=\int p(x\mid\theta)p(\theta)\,d\theta. \]

直接代进去：

\[ p(x) = \int \frac1{\sqrt{2\pi}\,2} e^{-\frac{(x-\theta)^2}{8}} \frac1{\sqrt{2\pi}\,4} e^{-\frac{(\theta-20)^2}{32}} \,d\theta. \]

卷积的定义是：

\[ (f*g)(x) = \int f(x-\theta)g(\theta)\,d\theta. \]

观察我们刚才的积分：

\[ \int \underbrace{ \frac1{\sqrt{2\pi}\,2} e^{-\frac{(x-\theta)^2}{8}} }_{f(x-\theta)} \underbrace{ \frac1{\sqrt{2\pi}\,4} e^{-\frac{(\theta-20)^2}{32}} }_{g(\theta)} \,d\theta. \]

它自己就已经长成卷积的样子了：

\[ \boxed{ p(x)=\int f(x-\theta)g(\theta)d\theta } \]

例 2：一维位置测量

设某物体位置的先验估计为

\[ x\sim\mathcal N(10,2^2) \]

一次测量得到 \(z=12\)，且测量误差的标准差为 \(1\)：

\[ z|x\sim\mathcal N(x,1^2) \]

更新后，

\[ \sigma_{\mathrm{post}}^2 =\left(\frac{1}{2^2}+\frac{1}{1^2}\right)^{-1} =0.8 \]

\[ \mu_{\mathrm{post}} =0.8\left(\frac{10}{4}+\frac{12}{1}\right) =11.6 \]

即

\[ \boxed{x|z\sim\mathcal N(11.6,0.8)} \]

因为测量的不确定性小于先验估计的不确定性，更新结果会更靠近测量值 \(12\)。这正是连续贝叶斯更新“按可靠程度融合信息”的含义。

二、综合知识

2.1 矩阵指数

基本信息参考矩阵指数 - 维基百科，自由的百科全书

定义与速查

对任意方阵 \(A\in\mathbb R^{n\times n}\)（或 \(\mathbb C^{n\times n}\)），矩阵指数定义为绝对收敛的幂级数：

\[ \boxed{ e^A =I+A+\frac{A^2}{2!}+\frac{A^3}{3!}+\cdots =\sum_{k=0}^{\infty}\frac{A^k}{k!} }. \]

因此 \(e^A\) 与 \(A\) 同阶；这里的 \(I\) 是同阶单位矩阵。写成 \(e^{At}\) 时，只是把矩阵 \(At\) 代入这个定义：

\[ e^{At}=I+At+\frac{A^2t^2}{2!}+\frac{A^3t^3}{3!}+\cdots. \]

最常用的结论可以先记住：

\[ \begin{aligned} e^{A\cdot 0}&=I,\\ \frac{d}{dt}e^{At}&=Ae^{At}=e^{At}A,\\ \left(e^{At}\right)^{-1}&=e^{-At},\\ e^{A(t+s)}&=e^{At}e^{As}. \end{aligned} \]

最后一条只涉及同一个矩阵 \(A\)，所以始终成立；它正是“先演化 \(s\) 秒、再演化 \(t\) 秒”等于“一共演化 \(t+s\) 秒”。

矩阵指数的动力学含义

\(A\) 表示“此刻的变化速度规则”，而 \(e^{At}\) 表示“按照这个规则连续演化 \(t\) 秒之后的总效果”。

这就是它最核心的物理意义。

先看你熟悉的一维情况。假设某个量 \(x\) 满足

\[ \dot x=ax. \]

意思是：当前 \(x\) 越大，它变化得越快，而且变化率由 \(a\) 决定。解是

\[ x(t)=e^{at}x(0). \]

所以这里的

\[ e^{at} \]

其实不是“为了算 exp 而 exp”。它表示：

\[ \boxed{\text{以瞬时变化规律 }a\text{ 连续演化 }t\text{ 秒后的累计效果}} \]

比如 \(a=0.1\)，那么每一小段时间都有一点增长，所有无穷小增长累积起来，最终出现 \(e^{0.1t}\)。

现在升级到两个状态，例如位置和速度：

\[ x= \begin{pmatrix} p\\ v \end{pmatrix}. \]

假设系统是

\[ \frac{d}{dt} \begin{pmatrix} p\\ v \end{pmatrix} = \begin{pmatrix} 0&1\\ 0&0 \end{pmatrix} \begin{pmatrix} p\\ v \end{pmatrix}. \]

矩阵

\[ A= \begin{pmatrix} 0&1\\ 0&0 \end{pmatrix} \]

不是一张随便的数字表。它是在说：

\[ \dot p=v, \qquad \dot v=0. \]

也就是说：

当前位置变化速度由当前速度决定；速度本身不变。

因此这个 \(A\) 是一个瞬时动力学规则。

现在假设时间只前进一个极小量 \(dt\)。

根据导数：

\[ x(t+dt)\approx x(t)+\dot x(t)dt. \]

因为

\[ \dot x=Ax, \]

所以

\[ x(t+dt) \approx x(t)+Ax(t)dt \]

即

\[ \boxed{ x(t+dt)\approx (I+A\,dt)x(t) }. \]

这一行非常重要。

它说：

\[ I+A\,dt \]

是系统经过极短时间 \(dt\) 的变化矩阵。

那么经过两小步呢？

\[ x(2dt) \approx (I+A\,dt)^2x(0). \]

经过 \(N\) 小步：

\[ x(t) \approx (I+A\,dt)^N x(0), \]

其中

\[ dt=\frac{t}{N}. \]

于是：

\[ x(t) \approx \left(I+A\frac{t}{N}\right)^N x(0). \]

当我们把时间切得无限细：

\[ N\to\infty, \]

得到

\[ \boxed{ e^{At} = \lim_{N\to\infty} \left(I+\frac{At}{N}\right)^N }. \]

这个极限定义把瞬时动力学与有限时间演化连接起来。

因为普通指数也完全一样：

\[ \boxed{ e^{x} = \lim_{N\to\infty} \left(1+\frac{x}{N}\right)^N }. \]

由此可以对照两种情形：

标量 \[ e^{at} = \lim_{N\to\infty} \left(1+\frac{at}{N}\right)^N \]

表示一个标量增长规则连续作用。

矩阵 \[ e^{At} = \lim_{N\to\infty} \left(I+\frac{At}{N}\right)^N \]

表示一个线性动力学规则连续作用。

矩阵指数与相似变换

已知

\[ A=PDP^{-1}, \]

那么

\[ e^{At}=Pe^{Dt}P^{-1}. \]

关键就在矩阵指数的定义：

\[ e^{At} =I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+\cdots \]

把 \(A=PDP^{-1}\) 代进去。

1. 先看 \(A^2\)

\[ A^2 =(PDP^{-1})(PDP^{-1}). \]

因为中间

\[ P^{-1}P=I, \]

所以

\[ A^2=PD^2P^{-1}. \]

同理，

\[ A^3 =(PDP^{-1})^3 =PD^3P^{-1}, \]

一般地，

\[ \boxed{A^n=PD^nP^{-1}}. \]

2. 代入矩阵指数

因此

\[ \begin{aligned} e^{At} &=I+At+\frac{A^2t^2}{2!}+\frac{A^3t^3}{3!}+\cdots\\ &=I+PDP^{-1}t +P D^2P^{-1}\frac{t^2}{2!} +P D^3P^{-1}\frac{t^3}{3!} +\cdots \end{aligned} \]

注意

\[ I=PIP^{-1}, \]

所以每一项都可以提出 \(P\) 和 \(P^{-1}\)：

\[ \begin{aligned} e^{At} &= P\left( I+Dt+\frac{D^2t^2}{2!} +\frac{D^3t^3}{3!} +\cdots \right)P^{-1}\\ &=P e^{Dt}P^{-1}. \end{aligned} \]

所以

\[ \boxed{\displaystyle e^{At}=Pe^{Dt}P^{-1}}. \]

如果 \(D\) 是对角矩阵，例如

\[ D= \begin{pmatrix} \lambda_1&0\\ 0&\lambda_2 \end{pmatrix}, \]

那么这件事特别有用，因为

\[ e^{Dt} = \begin{pmatrix} e^{\lambda_1t}&0\\ 0&e^{\lambda_2t} \end{pmatrix}. \]

于是

\[ \boxed{ e^{At} = P \begin{pmatrix} e^{\lambda_1t}&0\\ 0&e^{\lambda_2t} \end{pmatrix} P^{-1} }. \]

这其实就是为什么解微分方程 \(x'=Ax\) 时特别喜欢把 \(A\) 对角化：难算的 \(e^{At}\)，通过换基变成了逐个计算标量指数即可得到的 \(e^{Dt}\)。

更一般地，有如下规律：

\[ \boxed{f(PDP^{-1})=Pf(D)P^{-1}} \]

只要 \(f\) 可以用幂级数定义，例如

\[ e^A,\quad \sin A,\quad \cos A \]

都有类似性质。

矩阵指数加法乘法

如果 \(A,B\in \mathbb R^{n\times n}\)，那么

\[ e^{At},\ e^{Bt}\in \mathbb R^{n\times n} \]

可以相乘：

\[ \boxed{e^{At}e^{Bt}} \]

但这里有一个非常重要的坑：

\[ \boxed{e^{At}e^{Bt}\neq e^{(A+B)t}} \]

一般情况下不相等。

原因是矩阵乘法通常不交换：

\[ AB\neq BA. \]

如果

\[ \boxed{AB=BA} \]

成立，就可以使用熟悉的指数合并律：

\[ \boxed{ e^{At}e^{Bt}=e^{(A+B)t} } \]

例如如果 \(A,B\) 都是对角矩阵，它们会交换，那么就可以像普通数字一样合并指数。

若 \(A,B\) 不交换，一般就不能这样合并（极少数特殊矩阵可能偶然相等）。

举个简单展开就能看出来：

\[ e^{At} = I+At+\frac{A^2t^2}{2}+\cdots \]

\[ e^{Bt} = I+Bt+\frac{B^2t^2}{2}+\cdots \]

相乘到二阶：

\[ e^{At}e^{Bt} = I+(A+B)t + \left( \frac{A^2}{2}+AB+\frac{B^2}{2} \right)t^2+\cdots \]

而

\[ e^{(A+B)t} = I+(A+B)t + \frac{(A+B)^2t^2}{2}+\cdots \]

注意：

\[ (A+B)^2=A^2+AB+BA+B^2. \]

所以二阶项是

\[ \frac{A^2}{2} +\frac{AB}{2} +\frac{BA}{2} +\frac{B^2}{2}. \]

要和前面的

\[ \frac{A^2}{2}+AB+\frac{B^2}{2} \]

相等，就需要

\[ AB=BA. \]

因此，矩阵指数满足：

\[ \boxed{ \text{矩阵指数可以正常加乘，但“指数合并律”需要 }AB=BA } \]

另外，相加没有什么特殊化简：

\[ e^{At}+e^{Bt} \]

通常就是它自己，不会变成什么 \(e^{(A+B)t}\)。

这也是矩阵世界和标量世界一个非常核心的区别：不是指数变奇怪了，而是矩阵乘法不交换。

与逐元素 exp 的区别

np.exp 是 NumPy 的通用函数，其语义是：

数组里每个格子分别调用标量的 \(e^x\)。

所以：

np.exp(A)

意思实际上更接近：

\[ \begin{pmatrix} a&b\\ c&d \end{pmatrix} \longrightarrow \begin{pmatrix} e^a&e^b\\ e^c&e^d \end{pmatrix}. \]

这是完全合法的运算。

但它不是数学中定义的矩阵指数。

SciPy 的

scipy.linalg.expm(A)

才对应数学中的：

\[ \boxed{e^A}. \]

换句话说，Python 里：

\[ \texttt{np.exp(A)} \]

和

\[ \texttt{expm(A)} \]

其实是两个不同的函数，只是名字长得有点像。

下面的对角矩阵例子可以直观看出二者的区别。

假设

\[ D= \begin{pmatrix} 2&0\\ 0&3 \end{pmatrix}. \]

矩阵指数是：

\[ e^D = I+D+\frac{D^2}{2!}+\cdots \]

因为 \(D\) 是对角矩阵，

\[ D^2= \begin{pmatrix} 2^2&0\\ 0&3^2 \end{pmatrix}, \]

\[ D^3= \begin{pmatrix} 2^3&0\\ 0&3^3 \end{pmatrix}. \]

因此

\[ e^D = \begin{pmatrix} 1+2+\frac{2^2}{2!}+\cdots&0\\ 0&1+3+\frac{3^2}{2!}+\cdots \end{pmatrix} \]

也就是

\[ \boxed{ e^D= \begin{pmatrix} e^2&0\\ 0&e^3 \end{pmatrix}. } \]

注意：

如果你用逐元素 np.exp(D)，得到的是

\[ \begin{pmatrix} e^2&e^0\\ e^0&e^3 \end{pmatrix} = \begin{pmatrix} e^2&1\\ 1&e^3 \end{pmatrix}. \]

也就是把原来的两个 0 变成了 1。

因为 \(D\) 原来表示两个完全没有耦合的系统：

\[ \dot x_1=2x_1, \qquad \dot x_2=3x_2. \]

经过时间 \(t\) 后，它们仍应保持动力学解耦；统计独立还需要初始状态的独立性：

\[ x_1(t)=e^{2t}x_1(0), \]

\[ x_2(t)=e^{3t}x_2(0). \]

因此状态转移矩阵应为

\[ \begin{pmatrix} e^{2t}&0\\ 0&e^{3t} \end{pmatrix}. \]

同理，若对 \(Dt\) 逐元素取 exp，两个 0 会变成 1，从而凭空制造出变量之间的耦合：

\[ \begin{pmatrix} e^{2t}&1\\ 1&e^{3t} \end{pmatrix}. \]

这显然不是我们想要的。

匀速运动与旋转的计算实例

例 1：匀速模型——幂级数会自动截断

令

\[ A= \begin{pmatrix} 0&1\\ 0&0 \end{pmatrix}. \]

这个矩阵满足 \(A^2=0\)。因而从幂级数直接得到

\[ \boxed{ e^{At}=I+At =\begin{pmatrix} 1&t\\ 0&1 \end{pmatrix} }. \]

作用在初始状态 \(x(0)=(p_0,v_0)^\mathsf T\) 上：

\[ \begin{pmatrix}p(t)\\v(t)\end{pmatrix} = \begin{pmatrix} 1&t\\0&1 \end{pmatrix} \begin{pmatrix}p_0\\v_0\end{pmatrix} = \begin{pmatrix}p_0+tv_0\\v_0\end{pmatrix}. \]

这正是熟悉的匀速运动。这个例子也说明：矩阵指数并不总要计算无穷多项；若 \(A^m=0\)，级数在 \(m-1\) 次项后就结束。

例 2：平面旋转

令

\[ J= \begin{pmatrix} 0&-1\\ 1&0 \end{pmatrix}, \qquad J^2=-I. \]

设平面中的位置为

\[ x(t)= \begin{pmatrix} x_1(t)\\x_2(t) \end{pmatrix}. \]

方程 \(\dot x=Jx\) 展开后是

\[ \dot x_1=-x_2, \qquad \dot x_2=x_1. \]

也就是说：横坐标的变化率由纵坐标决定，纵坐标的变化率由横坐标决定，两个分量会持续相互带动。

例如从 \((1,0)^\mathsf T\) 出发，初始变化方向为

\[ \dot x(0) =J \begin{pmatrix}1\\0\end{pmatrix} = \begin{pmatrix}0\\1\end{pmatrix}. \]

所以它一开始向上运动；随着位置变化，运动方向也不断转动，轨迹便是逆时针旋转。

从计算上看，\(J^2=-I\)，因此 \(J\) 的幂会在 \(I,J,-I,-J\) 之间循环：

\[ J^3=-J, \qquad J^4=I, \qquad J^5=J, \quad\ldots \]

把这些幂代入矩阵指数的定义：

\[ \begin{aligned} e^{Jt} &=I+Jt+\frac{J^2t^2}{2!}+\frac{J^3t^3}{3!}+\frac{J^4t^4}{4!}+\cdots\\ &=I+Jt-I\frac{t^2}{2!}-J\frac{t^3}{3!} +I\frac{t^4}{4!}+J\frac{t^5}{5!}-\cdots\\ &=I\left(1-\frac{t^2}{2!}+\frac{t^4}{4!}-\cdots\right) +J\left(t-\frac{t^3}{3!}+\frac{t^5}{5!}-\cdots\right). \end{aligned} \]

括号中的两组级数恰好分别是 \(\cos t\) 和 \(\sin t\) 的幂级数：

\[ \cos t=1-\frac{t^2}{2!}+\frac{t^4}{4!}-\cdots, \qquad \sin t=t-\frac{t^3}{3!}+\frac{t^5}{5!}-\cdots. \]

所以

\[ \begin{aligned} e^{Jt} &=I\cos t+J\sin t\\ &= \begin{pmatrix} \cos t&-\sin t\\ \sin t&\cos t \end{pmatrix}. \end{aligned} \]

这正是逆时针旋转 \(t\) 弧度的旋转矩阵。特别地，

\[ e^{Jt} \begin{pmatrix}1\\0\end{pmatrix} = \begin{pmatrix}\cos t\\\sin t\end{pmatrix}, \]

表示初始点 \((1,0)\) 在时刻 \(t\) 转到了单位圆上的 \((\cos t,\sin t)\)。因此，\(\dot x=Jx\) 的解不是增长或衰减，而是以角速度 \(1\) 绕原点旋转。

与连续时间状态方程的关系

对线性常微分方程

\[ \dot x(t)=Ax(t), \qquad x(0)=x_0, \]

其唯一解为

\[ \boxed{x(t)=e^{At}x_0}. \]

验证很直接：由 \(\frac{d}{dt}e^{At}=Ae^{At}\)，

\[ \frac{d}{dt}\bigl(e^{At}x_0\bigr)=Ae^{At}x_0=Ax(t), \]

且 \(e^{A\cdot0}x_0=Ix_0=x_0\)。

更一般地，若系统在时刻 \(t_0\) 的状态为 \(x(t_0)\)，则经过采样间隔 \(\Delta t\) 后：

\[ \boxed{x(t_0+\Delta t)=\Phi(\Delta t)x(t_0)}, \qquad \boxed{\Phi(\Delta t)=e^{A\Delta t}}. \]

\(\Phi(\Delta t)\) 叫作状态转移矩阵。在连续时间模型离散化、卡尔曼滤波预测步骤中，常见的离散状态转移矩阵 \(F\) 正是这个 \(\Phi(\Delta t)\)。

2.2 概率模型与状态估计

状态、观测与测量模型

设机器人停在直线上的某个位置，传感器连续读出 \(9.8,10.3,9.9,10.2\) 米。真实位置没有因为读数跳动而反复改变；发生变化的是测量误差。把未知真实位置记为 \(X\)，它的一个候选取值记为 \(x\)；把随机观测记为 \(Z\)，已经收到的读数记为 \(z\)。估计值另写成 \(\hat x\)。后文常用小写 \(x_k\) 表示随机状态或其候选值，具体由分布及上下文判断，但带帽的 \(\hat x_k\) 始终表示估计。

一个简单测量模型是 \(Z=X+V\)。若标定表明误差近似零均值高斯，标准差为 \(0.2\) 米，则

\[ p(z\mid x)=\frac{1}{\sqrt{2\pi(0.2)^2}} \exp\left[-\frac{(z-x)^2}{2(0.2)^2}\right]. \]

可以把这个测量模型理解为一组重复测量实验：如果把机器人固定在 \(10.3\) 米处，反复测量，读数就会围绕 \(10.3\) 米波动；如果把机器人固定在 \(10.1\) 米处，读数则会围绕 \(10.1\) 米波动。\(p(z\mid x)\) 描述的就是每一个假设位置下，读数如何分布。

现在传感器给出了 \(10.3\) 米的读数。为了理解这个读数对不同位置的支持程度，先看它附近的区间 \([10.2,10.4]\) 米。按照上述噪声模型：

这两个概率都可以通过对相应的 \(p(z\mid x)\) 在同一区间内积分得到。它们说明：虽然两个位置都可能产生接近 \(10.3\) 米的读数，但这种读数在真实位置为 \(10.3\) 米时更常见。因此，收到这一读数后，数据会更支持 \(x=10.3\)，而不是 \(x=10.1\)。

对于连续模型中的精确读数，我们用该处的密度来作同样的比较。把已经收到的 \(z=10.3\) 固定，让候选位置 \(x\) 变化，就得到似然函数：

\[ L(x)=p(10.3\mid x) \propto\exp\left[-\frac{(10.3-x)^2}{2(0.2)^2}\right]. \]

这里省略的系数对所有候选位置都相同，不影响比较。例如，

\[ \frac{L(10.1)}{L(10.3)} =\exp\left[-\frac{(10.3-10.1)^2}{2(0.2)^2}\right] =e^{-1/2}\approx0.607. \]

也就是说，假设位置为 \(10.1\) 米时，这次读数的似然约为假设位置为 \(10.3\) 米时的 \(60.7\%\)。这个比例不是“机器人位于 \(10.1\) 米的概率”：似然只衡量各个位置对当前读数的解释程度；还要结合观测前对位置的判断，也就是先验，才能得到后验分布。

标定给出测量规律，观测帮助判断当前位置

前面的比较使用了已知的噪声标准差 \(0.2\) 米。这个数可以在使用传感器之前通过标定得到：先把机器人放在已知参考位置，重复记录读数，再计算“读数减参考位置”的误差。误差的平均值帮助发现固定偏置，误差的方差描述读数的波动。

因此，标定解决的是“传感器通常怎样出错”，定位解决的是“这一次机器人在哪里”。知道前一种规律，并不要求我们已经知道使用时的真实位置。下面暂时把标定得到的模型参数当作已知。

测量规律也取决于传感器测的是什么。若直接测位置，理想读数是 \(x\)；若测机器人到右侧墙面的距离，墙的位置为 \(d\)，理想读数就是 \(d-x\)。通常把这层关系记为 \(h(x)\)，写成 \(Z=h(X)+V\)。若还存在固定偏置 \(b\)，则写成 \(Z=h(X)+b+V\)。先明确理想读数如何产生，再描述噪声，才能建立 \(p(z\mid x)\)。

运动规律和测量规律可以来自物理知识、经验或数据拟合。数据拟合也可以用于贝叶斯模型，但需要明确输出代表什么：未经校准的预测分数不能直接当作似然。“置信度”可能指后验概率、区间的覆盖程度或其他评分，必须先说明含义。

离散状态的贝叶斯更新

上一段只比较了哪个位置更容易产生当前读数。接下来还要纳入另一份信息：在看到这次读数之前，我们原本认为机器人更可能在哪里。

为了看清每一步计算，暂时将连续位置简化为 \(9,10,11\) 米三个候选，并换用一个只报告整数位置的传感器。这个离散小例子用于说明更新步骤，不是把前面的连续高斯密度直接当成概率。

假设观测前的位置概率为

\[ P(X=9)=0.2,\qquad P(X=10)=0.5,\qquad P(X=11)=0.3. \]

这就是先验分布。它可以来自初始位置知识，也可以来自之前的测量与运动预测。先验不必是“随意猜测”，但也不保证某个最可能位置就是实际位置。

现在传感器报告 \(z=10\)。标定表明：真实位置为 \(9,10,11\) 时，产生这一报告的概率分别为 \(0.1,0.8,0.1\)。这里每次都固定同一份报告，再比较三个候选位置。

先计算各个位置对报告的贡献

以 \(x=9\) 为例，机器人原本位于 9 米的概率为 \(0.2\)，而处于该位置时报告 10 的概率为 \(0.1\)。所以“机器人位于 9 米，并且传感器报告 10”的联合概率为

\[ P(X=9,Z=10)=0.2\times0.1=0.02. \]

其余两个位置也一样计算：

\[ P(X=10,Z=10)=0.5\times0.8=0.40, \]

\[ P(X=11,Z=10)=0.3\times0.1=0.03. \]

这三个数还不是位置的后验概率。它们描述的是“某个位置与这份报告同时出现”，合计为 \(0.45\)，即传感器报告 10 的总概率。

已知报告出现后重新分配权重

现在我们已经知道报告 10 出现了，只需在与这份报告相容的联合贡献之间比较。将三个贡献分别除以总量 \(0.45\)，得到：

后验中位置 10 的概率由 \(0.5\) 提升到约 \(0.889\)，因为报告 10 更容易由该位置产生。位置 11 的先验高于位置 9，而两者的似然相同，所以更新后位置 11 仍获得更高权重。

现在再把这个计算写成一般式。若候选为 \(x_i\)，先算

\[ w_i=P(z\mid x_i)P(x_i), \]

再求总贡献 \(c=\sum_j w_j\)，最后归一化：

\[ P(x_i\mid z)=\frac{w_i}{c} =\frac{P(z\mid x_i)P(x_i)} {\sum_jP(z\mid x_j)P(x_j)}. \]

分母 \(c\) 也叫证据。第一章的贝叶斯公式在这里就是“逐项计算贡献，再除以贡献之和”。似然这一列不需要在不同位置之间合计为 1；测量模型要求的是：对每个固定位置，所有可能报告的概率合计为 1。

合理的先验仍然会被新数据更新。只有当前报告对所有有先验概率的位置都同样容易出现时，它才不改变这些位置之间的相对权重。如果先验将某个位置的概率设为零，普通乘法更新也无法重新赋予它正概率，因此初始先验应表达真实的不确定性。

后验分布与点估计

更新得到的是三个位置的整组概率。若需要输出一个位置，可以先取最大概率对应的位置，得到最大后验估计 \(\hat x_{\mathrm{MAP}}=10\) 米。

另一种方式是取加权平均：

\[ \hat x_{\mathrm{mean}} =9\frac2{45}+10\frac{40}{45}+11\frac3{45} =\frac{451}{45}\approx10.0222\text{ 米}. \]

两者都来自同一个后验，只是提取信息的方法不同。MAP 选择概率最大的候选；均值同时使用全部候选及其权重。均值落在 10 与 11 之间，不意味着离散模型又增加了一个真实位置候选，它只是输出的点估计。

后验均值在平方误差准则下最优，MAP 则选择后验峰值。单峰高斯的两者相同；多峰分布的均值可能位于两个峰之间。因此，估计值只是分布的摘要，单个数字不能完整表达不确定性。

连续状态的概率密度与更新

三个候选方便手算，但真实位置通常连续。此时先把直线切成许多宽度为 \(\Delta x\) 的小区间，对每个区间分配概率；区间越细，同一段长度内就会包含越多小格，每格的概率通常也越小。

密度描述的是“单位长度内分配了多少概率”。在足够小的区间内，如果密度变化不大，就有

\[ P(x_i\le X<x_i+\Delta x)\approx p(x_i)\Delta x. \]

例如某处密度约为每米 \(0.4\)，宽度为 \(0.1\) 米的小区间就约有 \(0.04\) 的概率。把许多小区间概率加起来，并让区间不断细化，便得到积分：

\[ P(a<X<b)=\int_a^b p(x)\,dx. \]

本文的连续状态具有密度，单个点的概率为零。我们比较点附近的可能程度时使用密度，计算一段范围内的概率时使用积分。连续后验的均值与 MAP 相应写为

\[ \hat x_{\mathrm{mean}}=\int x\,p(x\mid z)\,dx,\qquad \hat x_{\mathrm{MAP}}=\arg\max_xp(x\mid z). \]

连续更新保留同样的三步结构

离散例子中，我们对每个候选计算 \(w_i=P(z\mid x_i)P(x_i)\)。在连续情形下，对每个候选位置 \(x\) 计算

\[ w(x)=p(z\mid x)p(x). \]

固定观测 \(z\) 后，\(w(x)\) 随候选位置变化，是一整条未归一化曲线。再把全部位置上的贡献积分起来，得到归一化常数：

\[ c=\int w(\xi)\,d\xi=\int p(z\mid\xi)p(\xi)\,d\xi. \]

最后除以它：

\[ p(x\mid z)=\frac{w(x)}c =\frac{p(z\mid x)p(x)} {\int p(z\mid\xi)p(\xi)\,d\xi}. \]

这里用 \(\xi\) 表示被遍历的候选位置，避免与分子中保留的 \(x\) 混淆。积分完成后，\(c\) 是一个数，不再包含候选位置；\(p(x\mid z)\) 则仍是一条关于位置的密度曲线。若再指定 \(x=x_0\)，才得到这条曲线在某一点的函数值。

连续情形的证据 \(c=p(z)\) 是观测密度，离散情形的证据是观测概率。它们都用于归一化，但不能混淆概率与密度。给定 \(x\) 时，\(p(z\mid x)\) 对所有观测 \(z\) 积分为 1；固定观测后，似然作为 \(x\) 的函数并不要求积分为 1。

这个积分可能有解析解，也可能需要数值计算。难点来自模型的函数形式，而不是“真实状态未知所以无法定义模型”。第三章会分别用有限位置求和和高斯配方实际完成这些计算。

未知状态与已知参数对应不同问题

第一章的硬币例子也遵循这一结构。若正面率已知为 \(0.5\)，我们预测的是十次抛掷中会出现多少次正面，次数服从 \(\operatorname{Bin}(10,0.5)\)，并非精确高斯。若正面率未知，才对参数 \(\theta\) 指定先验，再用次数数据更新它。正面率、正面次数以及正面率的后验，是三个不同对象；有限数据通常不会把参数后验压成 \(0.5\) 处的一个点。

连续观测与动态状态

到这里，机器人还被当作停在同一位置。若继续收到新的读数，在测量噪声给定位置后相互独立的条件下，就可以将刚得到的后验作为下一次更新的先验。

例如第一次读数为 \(z_1\)，更新得到 \(p(x\mid z_1)\)；第二次收到 \(z_2\)，就计算

\[ p(x\mid z_1,z_2)\propto p(z_2\mid x)p(x\mid z_1). \]

过去读数的作用已经保存在 \(p(x\mid z_1)\) 中，不需要再把同一份读数乘一次。后验与先验不是两种永远固定的分布名称：同一分布相对于已经使用的数据是后验，相对于下一份尚未使用的数据又是先验。

时间下标区分旧位置和新位置

机器人一旦运动，第二时刻的状态 \(x_2\) 就不再等于第一时刻的状态 \(x_1\)。收到新读数之前，还必须先把上一后验沿运动规律传播到当前时刻。这就是预测步骤的来源。

记 \(z_{1:k-1}=(z_1,\ldots,z_{k-1})\)，表示截至上一时刻的全部读数。\(p(x_k\mid z_{1:k-1})\) 是当前观测到来前的状态分布，\(p(x_k\mid z_{1:k})\) 是加入当前观测后的分布。前者称为当前先验，却已经以历史观测为条件；“先验”不等于“无条件概率”。

两个假设使历史可以由当前分布承接

为了从上一时刻递推到下一时刻，我们通常假设：给定上一状态与本次控制，当前状态不再需要更早状态和观测；给定当前状态，当前读数不再需要其他时刻的状态和观测。前者叫一阶马尔可夫假设，后者叫测量的条件独立假设。

它们的实际含义是：当前状态必须包含预测下一步和解释当前读数所需的信息。例如，只记位置却不知道速度，往往不足以预测运动；把速度也纳入状态，才能更合理地建立下一步模型。这正是下一节引入位置—速度向量的原因。

条件独立不代表所有读数无条件独立。不同读数可能共享未知位置或持续漂移，因此仍会相关。若传感器存在持续漂移，就需要校正它或将其纳入状态，而不能把各次误差简单当成独立噪声。

滤波中的观测更新属于贝叶斯推断，静态参数估计也属于贝叶斯推断。本文始终假定读数对应这一个机器人；多个目标时，还需先处理观测归属，再进行相应状态更新。

2.3 线性模型与不确定性传播

上一节说明了怎样根据读数更新位置分布。机器人运动后，下一步还取决于速度，因此我们先建立位置与速度的运动关系，再研究这组量的不确定性如何一起变化。本节先处理“怎样传播”，第三章再把它与观测更新合起来。

位置—速度的线性模型

设机器人当前位置为 \(10\) 米，速度为 \(2\) 米/秒，采样间隔 \(T=1\) 秒。如果这段时间保持匀速，下一时刻位置为 \(10+1\times2=12\)，速度仍为 \(2\)。把两条关系合在一起：

\[ \begin{bmatrix}p_k\\v_k\end{bmatrix} = \begin{bmatrix}1&T\\0&1\end{bmatrix} \begin{bmatrix}p_{k-1}\\v_{k-1}\end{bmatrix}. \]

矩阵第一行与状态相乘得到 \(1p_{k-1}+Tv_{k-1}\)，第二行得到 \(0p_{k-1}+1v_{k-1}\)。\(2\times2\) 矩阵乘 \(2\times1\) 向量，结果是 \(2\times1\)；每一行表示一个输出分量，每一列说明该输入分量对输出的贡献。系数 \(T\) 带时间单位，使速度对位置的贡献具有长度单位。

先把刚才的两条匀速关系单独写出来：

$ p_k=p_{k-1}+Tv_{k-1},v_k=v_{k-1}. $

它们的矩阵写法只是将两个式子放在一起，没有增加新的运动假设。接着逐步加入两种影响。第一种是已知控制：若区间内的恒定加速度为 \(u_k\)，位置还增加 \(T^2u_k/2\)，速度还增加 \(Tu_k\)。第二种是未知扰动，例如地面打滑或未建模的加速度，将它们对位置和速度的影响合在向量 \(w_k\) 中。

于是得到

\[ x_k=F_kx_{k-1}+B_ku_k+w_k,\qquad F_k=\begin{bmatrix}1&T\\0&1\end{bmatrix},\quad B_k=\begin{bmatrix}T^2/2\\T\end{bmatrix}. \]

其中 \(x_k=(p_k,v_k)^T\)，\(F_k\) 描述状态如何运动，\(B_ku_k\) 描述已知控制的影响，\(w_k\) 描述剩余的随机扰动。没有已知加速度输入时，令 \(u_k=0\) 即可。

再建立测量关系。传感器如果只测位置，就应从状态向量中取出第一个分量；第二个分量速度不直接进入读数。因此

\[ z_k=\begin{bmatrix}1&0\end{bmatrix}x_k+\nu_k=H_kx_k+\nu_k. \]

\(H_k\) 是 \(1\times2\)，它把二维状态映射成一个读数。增加速度状态不要求增加速度传感器；能否从连续位置观测中推断速度，还取决于状态间的动态联系。

误差协方差及其传播

状态有了两个分量，不确定性也需要同时描述两个分量。假设位置估计为 \(\hat p\)，速度估计为 \(\hat v\)，误差为 \(e_p=p-\hat p\)、\(e_v=v-\hat v\)。除了分别问位置和速度可能错多少，还要问二者是否倾向于同时偏大或偏小。

为看清计算，设下面四组联合误差等可能出现。位置单位为米，速度单位为米/秒：

两项误差的平均值都是零，所以表中平方的平均值就是各自方差，乘积的平均值就是交叉协方差。具体来说，

$ (e_p)==4, (e_v)==0.5, $

$ (e_p,e_v)==1. $

把这三个数按第一章的定义排进矩阵，就得到

\[ P=\frac14\sum_{i=1}^4 e_i e_i^T =\begin{bmatrix}4&1\\1&0.5\end{bmatrix}. \]

这是给定四点等概率模型的总体协方差，分母用 4；若用四次抽样估计未知总体的无偏样本协方差，才用 \(N-1=3\)。协方差不是把位置和速度数值直接填进矩阵；它来自偏差的平方和乘积。相关系数为 \(1/\sqrt{4\times0.5}=1/\sqrt2\)，说明位置偏大时速度也倾向偏大，但并非完全绑定。

均值与协方差如何经过运动模型传播

先明确真实状态、预测状态和预测误差之间的关系。设当前真实位置、速度为 \(p,v\)，根据已有观测得到的估计为 \(\hat p,\hat v\)。按误差定义，

\[ p=\hat p+e_p,\qquad v=\hat v+e_v. \]

先假设这一段时间内真实速度保持不变，且没有新增运动扰动。经过时间 \(T\) 后，真实状态按照运动规律变化：

\[ p_{\mathrm{new}}=p+Tv,\qquad v_{\mathrm{new}}=v. \]

滤波器不知道真实的 \(p,v\)，只能把同样的运动规律作用于已有估计，得到观测到来前的预测：

\[ \hat p_{\mathrm{new}}^-=\hat p+T\hat v,\qquad \hat v_{\mathrm{new}}^-=\hat v. \]

将“真实值等于估计值加误差”代入真实运动方程：

\[ \begin{aligned} p_{\mathrm{new}} &=(\hat p+e_p)+T(\hat v+e_v)\\ &=\underbrace{\hat p+T\hat v}_{\hat p_{\mathrm{new}}^-} +\underbrace{e_p+Te_v}_{e_p^-}. \end{aligned} \]

因此，预测位置误差要通过真实位置减去预测位置得到：

\[ e_p^-=p_{\mathrm{new}}-\hat p_{\mathrm{new}}^- =(p+Tv)-(\hat p+T\hat v) =e_p+Te_v. \]

速度同理：

\[ e_v^-=v_{\mathrm{new}}-\hat v_{\mathrm{new}}^- =v-\hat v=e_v. \]

这样才得到新误差向量 \((e_p+Te_v,e_v)^T\)。我们接下来计算两项误差的方差与协方差，是为了描述真实的新状态可能偏离预测多少，以及位置偏差与速度偏差怎样一起变化。

在已有观测固定后，\(\hat p,\hat v\) 及其预测值都是已经算出的数。减去这些数不会改变条件方差与协方差，所以新状态相对于预测的协方差，也就是新误差向量的协方差。这里先通过相减得到误差关系，并没有假设真实状态与误差独立。至于 \(e_p,e_v\) 是否相关，则由原协方差矩阵 \(P\) 的非对角线记录。

现在用上述四组误差和 \(T=1\) 秒实算。由刚才推导的关系，误差变成 \((e_p+e_v,e_v)\)，四组分别为 \((3,1),(2,0),(-2,0),(-3,-1)\)，于是预测协方差为

\[ P^-=\begin{bmatrix} (9+4+4+9)/4&(3+0+0+3)/4\\ (3+0+0+3)/4&(1+0+0+1)/4 \end{bmatrix} =\begin{bmatrix}6.5&1.5\\1.5&0.5\end{bmatrix}. \]

速度若比估计大，下一时刻的位置通常也会比预测大，因此位置方差由 4 增至 6.5，交叉协方差也由 1 增至 1.5。这种变化甚至不需要额外噪声，仅传播已有的速度不确定性就会发生。

逐项算方差还可以看出来源：

$ (e_p+e_v) =(e_p)+2(e_p,e_v)+(e_v) =4+2(1)+0.5=6.5. $

同理，\(\operatorname{Cov}(e_p+e_v,e_v)=1+0.5=1.5\)。

再将这两项误差的传播写成矩阵。把原误差记为 \(e=(e_p,e_v)^T\)，预测后的误差记为 \(e^-=(e_p^-,e_v^-)^T\)。本例暂不加入新的运动噪声，经过采样间隔 \(T\) 后，

\[ e^-= \begin{bmatrix}e_p+Te_v\\e_v\end{bmatrix} =\underbrace{\begin{bmatrix}1&T\\0&1\end{bmatrix}}_{F} \begin{bmatrix}e_p\\e_v\end{bmatrix} =Fe. \]

这里 \(F\) 是匀速模型的状态转移矩阵。它首先作用于误差向量：位置误差会累积速度误差的影响，而速度误差保持原样。接下来要计算的是传播后的误差协方差。

本例原误差的均值为零，因此预测误差的均值也为零，协方差可以写成误差外积的期望。将 \(e^-=Fe\) 代入：

\[ \begin{aligned} P^- &=E[e^-(e^-)^T]\\ &=E[(Fe)(Fe)^T]\\ &=E[Fe\,e^TF^T]\\ &=F\,E[ee^T]\,F^T\\ &=FPF^T. \end{aligned} \]

第三行使用了转置的乘法规则 \((Fe)^T=e^TF^T\)；第四行则因为 \(F\) 是已知的确定矩阵，可以将它提出期望符号。\(ee^T\) 描述一次误差取值对应的平方与交叉乘积，对所有可能误差取期望后，才得到 \(P=E[ee^T]\)。

因此，\(FPF^T\) 来自“先传播误差，再计算传播后误差的协方差”。左右两侧的 \(F\) 与 \(F^T\) 分别来自外积中的误差列向量及其转置。先算 \(FP\) 只是完成矩阵乘法的一种计算顺序，无需将它作为单独的物理量引入。

代入本例的 \(T=1\) 秒，以及前面得到的 \(P\)：

\[ F=\begin{bmatrix}1&1\\0&1\end{bmatrix},\qquad P=\begin{bmatrix}4&1\\1&0.5\end{bmatrix}. \]

于是

\[ \begin{aligned} P^-=FPF^T &=\begin{bmatrix}1&1\\0&1\end{bmatrix} \begin{bmatrix}4&1\\1&0.5\end{bmatrix} \begin{bmatrix}1&0\\1&1\end{bmatrix}\\ &=\begin{bmatrix}5&1.5\\1&0.5\end{bmatrix} \begin{bmatrix}1&0\\1&1\end{bmatrix}\\ &=\begin{bmatrix}6.5&1.5\\1.5&0.5\end{bmatrix}. \end{aligned} \]

结果与逐项计算的方差、协方差一致。接下来再加入运动过程中新增的随机扰动。

现在推广。设 \(x\) 的均值为 \(m\)，\(w\) 均值为零，已知控制 \(u\) 视为确定量。线性变换后，

\[ m^-=E[Fx+Bu+w]=Fm+Bu. \]

为了计算方差，要先减掉均值。刚才确定性的控制项 \(Bu\) 同时出现在状态和均值里，所以相减后消失，留下

$ x^–m-=F(x-m)+w. $

预测偏差由两部分组成：原偏差经过 \(F\) 的传播，以及新加入的噪声。像展开 \((a+b)^2\) 一样展开它的外积，就有四项：

\[ \begin{aligned} P^-&=E[(F(x-m)+w)(F(x-m)+w)^T]\\ &=FPF^T+F\,E[(x-m)w^T] +E[w(x-m)^T]F^T+E[ww^T]. \end{aligned} \]

若过程噪声与原状态不相关，两项交叉协方差才为零，故 \(P^-=FPF^T+Q\)，其中 \(Q=\operatorname{Cov}(w)\)。独立是充分条件，但只要求这些二阶矩为零就能完成此步。单独知道噪声均值为零并不足够。如果控制也有随机误差，需要把其协方差及相关项计入。

这里的 \(P\) 可以是以历史观测为条件的状态协方差；当估计取条件均值时，它也就是条件误差协方差。第一章的 \(E[ee^T]\) 写法对应零均值误差，不能在有偏误差下自动当作中心化协方差。

联合误差的高斯表示

前面已经得到位置误差、速度误差的方差与协方差。现在进一步描述各种误差组合的分布，以便在后面的滤波中计算它们如何随运动和观测变化。

从一对误差到联合分布

位置和速度的真实值分别为 \(p,v\)，估计值分别为 \(\hat p,\hat v\)。一次估计对应一对误差：

\[ e=\begin{bmatrix}e_p\\e_v\end{bmatrix} =\begin{bmatrix}p-\hat p\\v-\hat v\end{bmatrix}. \]

例如 \(e=(2,1)^T\) 表示位置比估计大 2 米，同时速度比估计大 1 米/秒；\(e=(2,-1)^T\) 则表示位置比估计大 2 米，但速度比估计小 1 米/秒。联合分布描述的就是这些误差组合各自有多可能。

可以把每一对误差画成平面上的一个点，横轴是位置误差，纵轴是速度误差。这个平面描述两项状态误差，机器人本身仍沿直线运动。

前面四组等可能误差给出的均值为零，协方差为

\[ P=\begin{bmatrix}4&1\\1&0.5\end{bmatrix}. \]

对角线说明位置误差、速度误差各自的波动大小；非对角线为正，说明它们倾向于同向偏离均值。因此，只分别描述两项误差还不够，也需要描述它们如何搭配。

但均值和协方差只是分布的摘要。前面的四点离散模型，以及某些连续分布，都可以具有这组统计量。仅凭均值和协方差，不能确定完整的联合分布。

采用高斯模型后，用均值与协方差确定密度

现在为联合误差采用一个连续高斯模型，并使它具有相同的均值与协方差：

\[ e\sim\mathcal N\left( \begin{bmatrix}0\\0\end{bmatrix}, \begin{bmatrix}4&1\\1&0.5\end{bmatrix} \right). \]

高斯是这里额外采用的建模假设，不是从四组误差必然推出的结论。采用这个假设后，均值和协方差就能确定完整密度。均值为零表示误差分布的中心在零，不表示每次估计都没有误差。

一维高斯的指数包含偏差平方除以方差，即 \(e^2/\sigma^2\)。它按照变量本身的波动尺度衡量偏离程度。二维高斯也需要衡量偏离，但还必须考虑两项误差的相关性。第一章的多元高斯公式将其写成

\[ f(e)=\frac1{2\pi\sqrt{|P|}} \exp\left(-\frac12e^TP^{-1}e\right). \]

这里 \(f(e)\) 是误差组合处的联合密度；\(e^TP^{-1}e\) 衡量根据协方差调整后的联合偏离程度，分母则使密度在整个误差平面上的积分为 1。

先看不相关的高斯误差。若 \(P\) 为对角矩阵，其逆矩阵将各方差变为倒数，于是

\[ P=\begin{bmatrix}\sigma_p^2&0\\0&\sigma_v^2\end{bmatrix} \quad\Longrightarrow\quad e^TP^{-1}e=\frac{e_p^2}{\sigma_p^2}+\frac{e_v^2}{\sigma_v^2}. \]

这相当于分别按各自方差衡量偏差，再将两个结果相加。联合高斯模型下，不相关也意味着独立，因此这个联合密度同时等于两个一维高斯密度的乘积。

本例两项误差相关，使用完整的协方差矩阵。计算得到

\[ |P|=4(0.5)-1^2=1,\qquad P^{-1}=\begin{bmatrix}0.5&-1\\-1&4\end{bmatrix}. \]

将二次型逐项展开：

\[ \begin{aligned} d^2=e^TP^{-1}e &=\begin{bmatrix}e_p&e_v\end{bmatrix} \begin{bmatrix}0.5&-1\\-1&4\end{bmatrix} \begin{bmatrix}e_p\\e_v\end{bmatrix}\\ &=e_p(0.5e_p-e_v)+e_v(-e_p+4e_v)\\ &=0.5e_p^2-2e_pe_v+4e_v^2. \end{aligned} \]

所以，本例的联合误差密度为

\[ f(e_p,e_v) =\frac1{2\pi} \exp\left[-\frac12\left(0.5e_p^2-2e_pe_v+4e_v^2\right)\right]. \]

其中的交叉项 \(-2e_pe_v\) 让密度能够区分两种大小相同、方向搭配不同的误差：

两组误差的绝对值相同，但 \((2,1)\) 符合正协方差描述的同向偏离，因此其联合密度更高。表中的数值是密度之比，不是误差恰好取这两个点的概率；连续模型中，区域内的概率需要对密度积分。

协方差的非对角线不仅通过逆矩阵影响交叉项，也会影响归一化系数。例如保留方差 \(4,0.5\)，但将交叉协方差改为零，行列式就由 1 变为 2。因此，整个协方差矩阵共同决定密度的形状与高度。

从联合密度得到等密度椭圆

在同一个高斯分布中，归一化系数固定，密度只随 \(d^2\) 变化。因此，将密度相同的误差组合连接起来，就得到

\[ e^TP^{-1}e=c. \]

对正定 \(P\)，这是以误差均值为中心的椭圆。各方向的方差与相关性决定椭圆的宽度和倾斜。在所选坐标单位下，只有两个方向方差相同且不相关时，它才是圆。

椭圆表示相同密度，并不是允许取值的边界；椭圆外仍有非零密度。若要把某条椭圆作为概率区域，就必须计算其内部的积分。二维高斯中，\(c=1\) 的椭圆内部概率约为 \(39.35\%\)，与一维高斯中“均值两侧一个标准差内约为 \(68\%\)”不同。

最后，把误差分布接回状态估计。已有观测固定后，估计值 \(\hat x=(\hat p,\hat v)^T\) 是确定的，而真实状态满足

\[ x=\hat x+e. \]

因此，若条件误差分布为 \(e\sim\mathcal N(0,P)\)，条件状态分布就是 \(x\sim\mathcal N(\hat x,P)\)：从误差坐标换到状态坐标，只将分布中心移到估计值，协方差保持不变。后面的卡尔曼滤波正是用这个均值和协方差表示当前状态分布，再进行预测与更新。加入更多状态分量时，同样扩充向量和协方差矩阵即可。

离散转移与过程噪声

前面的协方差传播只考虑了一件事：位置、速度原本就估得不准，这些旧误差经过运动后会怎样变化。现在再加入另一件事：即使起点完全知道，机器人在运动过程中也可能受到新的未知扰动。

因此，本节分别建立两个量：\(F\) 描述按运动模型从旧状态走到新状态；\(Q\) 描述这一段运动新增加的误差有多大，以及各分量怎样相关。

先根据运动方程得到离散转移

设机器人当前位置为 10 米，速度为 2 米/秒。若接下来 1 秒保持匀速，新位置就是 12 米，速度仍为 2 米/秒。这里“离散”表示只计算相隔一个采样间隔的状态，不要求位置只能取整数。

将采样间隔写成 \(T\)，两条匀速关系是

\[ p_{\mathrm{new}}=p+Tv,\qquad v_{\mathrm{new}}=v. \]

合在一起就是

\[ x_{\mathrm{new}} =\begin{bmatrix}p+Tv\\v\end{bmatrix} =\underbrace{\begin{bmatrix}1&T\\0&1\end{bmatrix}}_{F(T)} \begin{bmatrix}p\\v\end{bmatrix}. \]

所以 \(F(T)\) 来自两条运动方程：它将速度在这段时间产生的位移加到位置上，并保留原速度。滤波器把同样的规律用于估计值，得到 \(\hat x_{\mathrm{new}}^-=F(T)\hat x\)；已有误差则按照前面推导的 \(e^-=F(T)e\) 传播。

这也接上了 2.1 的矩阵指数。连续匀速方程为 \(\dot p=v,\dot v=0\)，对应 \(A=\begin{bmatrix}0&1\\0&0\end{bmatrix}\)。因为 \(A^2=0\)，

\[ F(T)=e^{AT}=I+AT=\begin{bmatrix}1&T\\0&1\end{bmatrix}. \]

运动方程和矩阵指数给出了相同的转移矩阵。这里的连续动力学矩阵 \(A\) 在区间内恒定；一般时变系统需要另外处理。

运动扰动先改变真实状态，再形成新的预测误差

回到起点 \(p=10,v=2,T=1\)。暂时假定这两个起点数值完全准确，因此匀速预测为 \((12,2)^T\)。

如果机器人在这一秒内实际受到一个未被模型计入的恒定加速度 \(a=2\) 米/秒平方，真实运动就变为

\[ p_{\mathrm{new}}=10+2(1)+\frac12(2)(1)^2=13, \]

\[ v_{\mathrm{new}}=2+2(1)=4. \]

真实状态与匀速预测的差为 \((1,2)^T\)：位置多出 1 米，速度多出 2 米/秒。即使起点毫无误差，这份偏差也会在运动过程中产生。我们把它记为过程扰动向量 \(w\)。

对一般间隔 \(T\)，恒定加速度 \(a\) 产生的两项额外变化为

\[ w=\begin{bmatrix}T^2a/2\\Ta\end{bmatrix} =\underbrace{\begin{bmatrix}T^2/2\\T\end{bmatrix}}_G a. \]

因此，带扰动的真实状态模型为 \(x_{\mathrm{new}}=Fx+w\)。若起点本来还有误差 \(e=x-\hat x\)，而滤波器仍按匀速模型预测，则

\[ \begin{aligned} e^-&=x_{\mathrm{new}}-\hat x_{\mathrm{new}}^-\\ &=(Fx+w)-F\hat x\\ &=Fe+w. \end{aligned} \]

预测误差由两部分组成：旧误差经过运动的传播 \(Fe\)，以及这一段新增的扰动 \(w\)。过程扰动会影响真实运动；测量噪声则影响传感器读数，两者应分别建模。

用随机加速度计算过程协方差

刚才的 \(a=2\) 只是一种实际情况。做预测时，我们通常不知道下一段实际会受到多大扰动，因此要给它指定分布。本节不设置已知加速度控制，\(a\) 专门表示匀速模型未计入的加速度。

先用一个能手算的模型：每段内加速度保持恒定，可能为 \(+2\) 或 \(-2\)，概率各为一半；不同段的加速度相互独立，并与段初状态误差独立。它的均值与方差为

\[ E[a]=0,\qquad \operatorname{Var}(a)=\frac12(2^2)+\frac12(-2)^2=4. \]

零均值表示预测前不偏向加速或减速，不表示实际加速度总是零。因为 \(E[w]=G E[a]=0\)，预测均值仍按匀速模型传播，但预测协方差需要增加。

当 \(T=1\) 秒时，两种加速度对应的扰动为：

两项扰动均值都为零。按协方差定义，位置扰动方差为 1，速度扰动方差为 4，交叉协方差为 2：

\[ Q=\operatorname{Cov}(w) =\begin{bmatrix} E[w_p^2]&E[w_pw_v]\\ E[w_vw_p]&E[w_v^2] \end{bmatrix} =\begin{bmatrix}1&2\\2&4\end{bmatrix}. \]

这里非对角线为 2，是因为同一个加速度同时改变位置与速度，它们的扰动会一起变号。\(Q\) 不是给位置、速度分别随意指定两个噪声大小，而是从具体的扰动模型计算出来的。这个两点分布只是手算例子；若改用零均值高斯加速度，只要方差仍为 4，计算出的 \(Q\) 就相同，但扰动的完整分布不同。

将加速度方差一般地记为 \(\sigma_a^2\)。由 \(w=Ga\)，

\[ \begin{aligned} Q &=E[ww^T]=E[(Ga)(Ga)^T]\\ &=G E[a^2]G^T\\ &=G\sigma_a^2G^T\\ &=\sigma_a^2 \begin{bmatrix} T^4/4&T^3/2\\ T^3/2&T^2 \end{bmatrix}. \end{aligned} \]

这就是“每段恒定随机加速度”对应的过程协方差。它的两个分量由同一个随机量产生，因此 \(Q\) 可以是奇异的半正定矩阵，不要求正定。

最后将它接回预测。根据前面已经展开的 \(e^-=Fe+w\)，在零均值扰动与原误差不相关的条件下，

\[ P^-=FPF^T+Q. \]

例如原误差协方差为 \(P=\begin{bmatrix}4&1\\1&0.5\end{bmatrix}\)，前面算得 \(T=1\) 时 \(FPF^T=\begin{bmatrix}6.5&1.5\\1.5&0.5\end{bmatrix}\)。再加入本例的过程扰动，得到

\[ P^-= \begin{bmatrix}6.5&1.5\\1.5&0.5\end{bmatrix} +\begin{bmatrix}1&2\\2&4\end{bmatrix} =\begin{bmatrix}7.5&3.5\\3.5&4.5\end{bmatrix}. \]

第一项承接旧的不确定性，第二项加入运动新产生的不确定性。第三章的位置—速度示例使用同一种加速度模型，但会另行指定初始协方差与加速度方差。

区间内持续出现新扰动时，使用另一种噪声模型

前面一个采样区间内只抽取一次加速度，随后整段保持不变。例如抽到 \(a=2\)，整段都按这个加速度运动。如果实际扰动在区间内部不断产生和变化，就需要另一种时间模型。

这一次先描述每个小时间段新产生的速度变化，再计算这些变化到区间结束时累积了多少位置误差。这里只计算本区间新增的过程扰动；区间开始时已经存在的状态误差仍由 \(FPF^T\) 处理。

将长度为 \(T\) 的区间分成 \(N\) 个小段，每段长度为 \(\Delta t=T/N\)。把第 \(i\) 段新增的速度变化记为 \(\delta v_i\)。采用如下模型：

\[ E[\delta v_i]=0,\qquad \operatorname{Var}(\delta v_i)=q\Delta t, \]

并假设不同小段的速度变化相互独立。在线性高斯模型中，还令这些增量服从高斯分布。

\(q\) 描述速度误差方差每单位时间增加多少。例如 \(q=2\) 表示持续 1 秒产生的新增速度方差为 2；持续半秒时，新增方差为 1。这里的比例关系是所选噪声模型的假设，不是从前面“整段恒定加速度”的模型推出来的。

为什么每小段方差要随 \(\Delta t\) 缩小？因为将同一段时间切得更细，不应凭空改变它的总扰动强度。独立增量相加后，区间末新增速度方差为

\[ \operatorname{Var}\left(\sum_{i=1}^N\delta v_i\right) =\sum_{i=1}^N q\Delta t =Nq\frac TN=qT. \]

它与分成几段无关。如果无论小段多短，都给每段分配相同方差，切分越细总方差就越大，无法描述同一个连续时间模型。

先看一份速度变化怎样影响区间末的状态

对有限分段，可以近似把一小段的速度变化集中在该段中点发生。设这个时刻距区间起点为 \(s_i\)，距区间结束还剩

\[ \tau_i=T-s_i. \]

新增速度 \(\delta v_i\) 在剩余的 \(\tau_i\) 时间内继续产生位移，所以它在区间末造成的两项扰动为

\[ w_i= \begin{bmatrix} \tau_i\delta v_i\\ \delta v_i \end{bmatrix}. \]

例如相同的速度变化，若提前 \(0.8\) 秒出现，新增位移约为 \(0.8\delta v_i\)；若只提前 \(0.2\) 秒出现，则约为 \(0.2\delta v_i\)。越早出现的速度扰动，对最终位置的影响越久。这就是位置扰动中时间系数的来源。

这里把小段作用集中到中点，是计算近似；模型不是声称真实速度只在这些中点跳变。将分段不断细化，才得到后面的连续模型。

用两个小段手算一次

取 \(T=1\) 秒、\(q=2\)，先粗略分成两个半秒小段。每段新增速度方差为 \(q\Delta t=2(0.5)=1\)，均值为零，且两段相互独立。将其影响分别集中在 \(s_1=0.25\) 秒和 \(s_2=0.75\) 秒，则剩余时间为 \(0.75\) 秒和 \(0.25\) 秒：

\[ w_1=\begin{bmatrix}0.75\delta v_1\\\delta v_1\end{bmatrix}, \qquad w_2=\begin{bmatrix}0.25\delta v_2\\\delta v_2\end{bmatrix}. \]

区间末的总扰动为

\[ w^{(2)}=w_1+w_2 =\begin{bmatrix} 0.75\delta v_1+0.25\delta v_2\\ \delta v_1+\delta v_2 \end{bmatrix}. \]

上标 \((2)\) 表示采用两个小段的近似。先分别求三个协方差元素。因为 \(\operatorname{Cov}(\delta v_1,\delta v_2)=0\)，

\[ \operatorname{Var}(w_p^{(2)}) =0.75^2(1)+0.25^2(1)=0.625, \]

\[ \operatorname{Var}(w_v^{(2)})=1+1=2, \]

\[ \begin{aligned} \operatorname{Cov}(w_p^{(2)},w_v^{(2)}) &=\operatorname{Cov}(0.75\delta v_1+0.25\delta v_2, \delta v_1+\delta v_2)\\ &=0.75\operatorname{Var}(\delta v_1) +0.25\operatorname{Var}(\delta v_2)\\ &=1. \end{aligned} \]

因此，两小段近似给出的过程协方差是

\[ Q_2=\begin{bmatrix}0.625&1\\1&2\end{bmatrix}. \]

两段的新增速度变化虽然独立，但最终位置扰动与速度扰动并不独立：每一份 \(\delta v_i\) 都同时进入位置和速度，因此产生了非零交叉协方差。

把逐项计算整理成每小段的矩阵贡献

一般地，令 \(b_i=(\tau_i,1)^T\)，则 \(w_i=b_i\delta v_i\)。因为 \(b_i\) 是确定系数、\(\delta v_i\) 均值为零，

\[ \begin{aligned} Q_i &=E[w_iw_i^T]\\ &=b_i E[(\delta v_i)^2]b_i^T\\ &=q\Delta t \begin{bmatrix}\tau_i\\1\end{bmatrix} \begin{bmatrix}\tau_i&1\end{bmatrix}\\ &=q\begin{bmatrix}\tau_i^2&\tau_i\\\tau_i&1\end{bmatrix}\Delta t. \end{aligned} \]

这是一个小段新增的速度变化，对最终位置、速度协方差的贡献。它不是该小段起点的状态协方差，也不包含旧状态误差。

全部小段的总扰动为 \(w^{(N)}=\sum_iw_i\)。展开外积的期望，得到

\[ \begin{aligned} Q_N &=E\left[\left(\sum_iw_i\right)\left(\sum_jw_j\right)^T\right]\\ &=\sum_iE[w_iw_i^T]+\sum_{i\ne j}E[w_iw_j^T]. \end{aligned} \]

不同小段的增量独立且均值为零，因此 \(i\ne j\) 时，

\[ E[w_iw_j^T] =b_iE[\delta v_i\delta v_j]b_j^T =b_iE[\delta v_i]E[\delta v_j]b_j^T=0. \]

于是总协方差就是各小段贡献相加：

\[ Q_N=q\sum_{i=1}^N \begin{bmatrix}\tau_i^2&\tau_i\\\tau_i&1\end{bmatrix}\Delta t. \]

分段趋于无限细时，求和变成积分

先用扰动发生时刻 \(s\) 表示剩余时间 \(T-s\)。让 \(\Delta t\to0\)，上面的求和便成为

\[ Q=q\int_0^T \begin{bmatrix} (T-s)^2&T-s\\ T-s&1 \end{bmatrix}ds. \]

这里 \(s\) 遍历本区间内所有扰动时刻，每一时刻的贡献都先传播到同一个区间终点，再汇总。它与前面的有限分段计算做的是同一件事。

为了简化计算，再令 \(\tau=T-s\)。当 \(s\) 从 0 增至 \(T\) 时，\(\tau\) 从 \(T\) 降至 0；同时 \(ds=-d\tau\)。因此

\[ Q=q\int_T^0 \begin{bmatrix}\tau^2&\tau\\\tau&1\end{bmatrix}(-d\tau) =q\int_0^T \begin{bmatrix}\tau^2&\tau\\\tau&1\end{bmatrix}d\tau. \]

分别积分三个元素，得到

\[ \boxed{ Q=q\begin{bmatrix} T^3/3&T^2/2\\ T^2/2&T \end{bmatrix} }. \]

仍取 \(T=1,q=2\)，连续模型给出

\[ Q=\begin{bmatrix}2/3&1\\1&2\end{bmatrix}. \]

两小段近似中的位置扰动方差为 \(0.625\)；四小段中点近似为 \(0.65625\)；分段不断细化后趋于 \(2/3\approx0.66667\)。有限分段与连续公式之间的差别来自近似精度，而不是中途换了一组噪声参数。

用 2.1 的矩阵指数也可以表达同一个传播过程。令 \(L=(0,1)^T\)，表示速度变化最初只注入速度分量；经过剩余时间 \(\tau\) 后，

\[ e^{A\tau}L =\begin{bmatrix}1&\tau\\0&1\end{bmatrix} \begin{bmatrix}0\\1\end{bmatrix} =\begin{bmatrix}\tau\\1\end{bmatrix}. \]

它正是前面得到的传播系数，因此也有 \(Q=\int_0^T e^{A\tau}LqL^Te^{A^T\tau}\,d\tau\)。这个写法将逐个小段的传播统一起来，并没有引入另一种计算原理。

这种独立增量的连续模型通常用高斯白噪声加速度表示，\(q\) 也称加速度白噪声的谱密度。它直接规定小时间段内速度增量的方差，而不是给每个瞬间指定一个普通有限方差的随机加速度。这与前面的每段恒定随机加速度有不同的时间假设：

\(\sigma_a^2\) 是每段恒定加速度的方差，\(q\) 是速度方差的增长率，含义和单位都不同。选择哪个 \(Q\)，取决于采用哪个扰动模型；改变采样间隔时，也必须用相应公式重新计算。

2.4 多元函数的泰勒展开

多元函数的输入是一个向量。当多个输入同时发生小变化时，输出会怎样变化？泰勒展开把这种变化分解为一阶、二阶以及更高阶的项，让我们可以用一个局部多项式近似原函数。

一阶偏导组织成雅可比矩阵，描述局部线性变化；二阶偏导组织成 Hessian 矩阵，描述曲率。下面先说明展开式怎样产生，再把求和式整理成矩阵形式。

从一元到多元

先回顾一元函数。在 \(x_0\) 附近，令输入变化为 \(\Delta x\)，则

\[ f(x_0+\Delta x) \approx f(x_0)+f'(x_0)\Delta x +\frac12 f''(x_0)(\Delta x)^2. \]

三项依次表示当前位置的函数值、斜率带来的变化，以及曲率带来的修正。只保留前两项，就是用切线近似原曲线。

更一般地，保留到 \(k\) 阶时，一元函数的泰勒展开为

\[ \boxed{ f(x_0+\Delta x) =\sum_{r=0}^{k}\frac{f^{(r)}(x_0)}{r!}(\Delta x)^r +R_k(\Delta x). } \]

其中 \(f^{(r)}\) 表示 \(r\) 阶导数，约定 \(f^{(0)}=f\)、\(0!=1\)。把求和展开，就是

\[ f(x_0+\Delta x) =f(x_0)+f'(x_0)\Delta x +\frac{f''(x_0)}{2!}(\Delta x)^2 +\cdots+\frac{f^{(k)}(x_0)}{k!}(\Delta x)^k +R_k(\Delta x). \]

如果 \(f\) 在 \(x_0\) 与 \(x_0+\Delta x\) 之间及其附近有连续的 \(k+1\) 阶导数，拉格朗日余项可写为

\[ R_k(\Delta x) =\frac{f^{(k+1)}(x_0+\theta\Delta x)}{(k+1)!}(\Delta x)^{k+1}, \qquad 0<\theta<1. \]

它表示：省略部分的大小与下一阶导数、偏移的 \(k+1\) 次幂有关。例如在这段区间上有 \(|f^{(k+1)}(x)|\le M\)，就能估计

\[ |R_k(\Delta x)|\le\frac{M}{(k+1)!}|\Delta x|^{k+1}. \]

当展开点 \(x_0=0\) 时，这称为麦克劳林展开。例如

\[ e^{\Delta x}=1+\Delta x+\frac{(\Delta x)^2}{2!} +\cdots+\frac{(\Delta x)^k}{k!}+R_k(\Delta x). \]

有限阶展开带有余项；只有当阶数趋于无穷时余项趋于零，才能把对应的无穷泰勒级数写成等于原函数。

现在让输出同时依赖两个输入 \(p,a\)。从 \((p_0,a_0)\) 移动到 \((p_0+\Delta p,a_0+\Delta a)\)，二阶展开为

\[ \begin{aligned} f(p_0+\Delta p,a_0+\Delta a) \approx{}&f(p_0,a_0) +f_p\Delta p+f_a\Delta a\\ &+\frac12\left[ f_{pp}(\Delta p)^2 +2f_{pa}\Delta p\Delta a +f_{aa}(\Delta a)^2\right]. \end{aligned} \]

这里的导数都在 \((p_0,a_0)\) 处计算：\(f_p\) 是只改变 \(p\) 时的变化率，\(f_a\) 是只改变 \(a\) 时的变化率。二阶项还包括混合偏导 \(f_{pa}\)，它描述一个方向的变化率怎样受到另一个变量的影响。

为什么交叉项前面有 2？展开中原本有 \(f_{pa}\Delta p\Delta a\) 与 \(f_{ap}\Delta a\Delta p\) 两项。当函数的二阶偏导连续时，混合偏导相等，两项便可以合并。

这些展开用于足够光滑的函数，且近似通常只在展开点附近有效。对于二阶连续可微的函数，二阶展开省略的余项相对于 \(\|\Delta x\|^2\) 趋于零；实际计算中仍要结合偏移大小判断近似是否够好。

二阶展开的推导

设 \(f:\mathbb R^n\to\mathbb R\) 是标量函数，展开点与偏移分别为

\[ x_0=\begin{bmatrix}x_{0,1}&\cdots&x_{0,n}\end{bmatrix}^T, \qquad \Delta x=\begin{bmatrix}\Delta x_1&\cdots&\Delta x_n\end{bmatrix}^T. \]

为了同时处理所有变量，沿着从 \(x_0\) 指向 \(x_0+\Delta x\) 的直线引入一个一元函数：

\[ g(t)=f(x_0+t\Delta x). \]

这里 \(t\) 是沿直线前进的比例：\(g(0)=f(x_0)\)，\(g(1)=f(x_0+\Delta x)\)。固定方向 \(\Delta x\) 后，原来的多元变化就变成了关于 \(t\) 的一元变化。

根据链式法则，一阶导数为

\[ g'(t)=\sum_{i=1}^n \frac{\partial f}{\partial x_i}(x_0+t\Delta x)\Delta x_i. \]

继续对 \(t\) 求导。每个 \(\Delta x_i\) 都是常数，而偏导数仍通过全部坐标依赖于 \(t\)，所以又产生一层求和：

\[ g''(t)=\sum_{i=1}^n\sum_{j=1}^n \frac{\partial^2 f}{\partial x_j\partial x_i}(x_0+t\Delta x) \Delta x_i\Delta x_j. \]

对 \(g(t)\) 在 \(t=0\) 处作一元泰勒展开，并取 \(t=1\)：

\[ g(1)=g(0)+g'(0)+\frac12g''(0)+R_2(\Delta x). \]

代回刚才的导数，便得到 \(n\) 元标量函数的二阶一般式：

\[ \boxed{ \begin{aligned} f(x_0+\Delta x)={}&f(x_0) +\sum_{i=1}^n\frac{\partial f}{\partial x_i}(x_0)\Delta x_i\\ &+\frac12\sum_{i=1}^n\sum_{j=1}^n \frac{\partial^2 f}{\partial x_i\partial x_j}(x_0) \Delta x_i\Delta x_j+R_2(\Delta x). \end{aligned} } \]

这里假设二阶偏导连续，因此交换求导次序不影响结果。\(R_2\) 表示省略二阶以上项后剩余的误差；在该条件下，\(R_2(\Delta x)=o(\|\Delta x\|^2)\)。这表示当偏移趋于零时，余项与 \(\|\Delta x\|^2\) 的比值趋于零。忽略余项，就得到通常使用的二阶近似。

雅可比矩阵

先看多个输入、一个输出的情况。将输入写成列向量 \(x=(x_1,\ldots,x_n)^T\)，一阶偏导组成梯度列向量

\[ \nabla f(x_0)= \begin{bmatrix} \dfrac{\partial f}{\partial x_1}\\ \vdots\\ \dfrac{\partial f}{\partial x_n} \end{bmatrix}_{x_0}. \]

一阶展开便是

\[ f(x_0+\Delta x)\approx f(x_0)+\nabla f(x_0)^T\Delta x. \]

如果函数有多个输出，例如 \(f(x)=(f_1(x),\ldots,f_m(x))^T\)，就对每个输出分别展开，再把一阶偏导按行排列：

\[ J_f(x_0)= \begin{bmatrix} \dfrac{\partial f_1}{\partial x_1}&\cdots&\dfrac{\partial f_1}{\partial x_n}\\ \vdots&\ddots&\vdots\\ \dfrac{\partial f_m}{\partial x_1}&\cdots&\dfrac{\partial f_m}{\partial x_n} \end{bmatrix}_{x_0}. \]

这就是雅可比矩阵（Jacobian），大小为 \(m\times n\)：每行对应一个输出，每列对应一个输入。第 \((i,j)\) 个元素回答“只让第 \(j\) 个输入增加一小段，第 \(i\) 个输出大约怎样变化”。

所有输出的一阶展开统一写成

\[ \boxed{f(x_0+\Delta x)\approx f(x_0)+J_f(x_0)\Delta x.} \]

因此雅可比把输入的小偏移转换为输出的小偏移。只有一个输出时，它就是梯度的转置，是一行 \(n\) 列的矩阵。

Hessian 矩阵

对于标量函数 \(f:\mathbb R^n\to\mathbb R\)，将所有二阶偏导排成矩阵：

\[ \mathcal H_f(x_0)= \begin{bmatrix} \dfrac{\partial^2 f}{\partial x_1^2}&\cdots&\dfrac{\partial^2 f}{\partial x_1\partial x_n}\\ \vdots&\ddots&\vdots\\ \dfrac{\partial^2 f}{\partial x_n\partial x_1}&\cdots&\dfrac{\partial^2 f}{\partial x_n^2} \end{bmatrix}_{x_0}. \]

这就是 Hessian 矩阵（海森矩阵），大小为 \(n\times n\)。对角线记录各方向自身的曲率，非对角线记录混合偏导；二阶偏导连续时，它是对称矩阵。

接下来逐步看，二阶一般式中的双重求和为什么恰好等于一个二次型。记

\[ h_{ij}=\frac{\partial^2f}{\partial x_i\partial x_j}(x_0), \qquad \mathcal H_f=(h_{ij})_{n\times n}. \]

先做矩阵与列向量的乘法。 乘积的第 \(i\) 个分量是第 \(i\) 行与偏移向量的内积：

\[ \mathcal H_f\Delta x= \begin{bmatrix} \sum_{j=1}^n h_{1j}\Delta x_j\\ \vdots\\ \sum_{j=1}^n h_{nj}\Delta x_j \end{bmatrix}. \]

再从左边乘上行向量 \(\Delta x^T\)。 这会给第 \(i\) 个分量再乘一次 \(\Delta x_i\)，并把所有分量加起来：

\[ \begin{aligned} \Delta x^T\mathcal H_f\Delta x &=\sum_{i=1}^n\Delta x_i \left(\sum_{j=1}^n h_{ij}\Delta x_j\right)\\ &=\sum_{i=1}^n\sum_{j=1}^n h_{ij}\Delta x_i\Delta x_j\\ &=\sum_{i=1}^n\sum_{j=1}^n \frac{\partial^2f}{\partial x_i\partial x_j}(x_0) \Delta x_i\Delta x_j. \end{aligned} \]

这正是二阶展开中的双重求和，并没有增加或省略任何一项。维度也吻合：\((1\times n)(n\times n)(n\times1)\) 的结果是一个标量。

以两个变量为例，直接乘开更容易看清交叉项：

\[ \begin{aligned} &\begin{bmatrix}\Delta p&\Delta a\end{bmatrix} \begin{bmatrix}f_{pp}&f_{pa}\\f_{ap}&f_{aa}\end{bmatrix} \begin{bmatrix}\Delta p\\\Delta a\end{bmatrix}\\ &\quad=f_{pp}(\Delta p)^2+f_{pa}\Delta p\Delta a +f_{ap}\Delta a\Delta p+f_{aa}(\Delta a)^2\\ &\quad=f_{pp}(\Delta p)^2+2f_{pa}\Delta p\Delta a +f_{aa}(\Delta a)^2. \end{aligned} \]

最后一步用了 \(f_{pa}=f_{ap}\)。对一般的 \(n\) 个变量，同样可以写成

\[ \Delta x^T\mathcal H_f\Delta x =\sum_{i=1}^n h_{ii}(\Delta x_i)^2 +2\sum_{1\le i<j\le n}h_{ij}\Delta x_i\Delta x_j. \]

因此，二阶求和式可以完整整理为

\[ \boxed{ f(x_0+\Delta x)=f(x_0) +J_f(x_0)\Delta x +\frac12\Delta x^T\mathcal H_f(x_0)\Delta x +R_2(\Delta x). } \]

前面的 \(1/2\) 来自一元泰勒公式的 \(1/2!\)，不是矩阵乘法产生的系数。把余项略去，才得到二阶近似。

雅可比描述局部斜率，Hessian 描述斜率怎样继续变化。若 \(f\) 有 \(m\) 个输出，则每个标量输出 \(f_i\) 各有一个 Hessian，分别用于它自己的二阶展开；一般不能用一个普通的 \(n\times n\) 矩阵装下所有输出的二阶导数。

例：雷达测距

设雷达在原点，目标水平位置为 \(p\)、高度为 \(a\)，距离为

\[ h(p,a)=\sqrt{p^2+a^2}. \]

在 \((3,4)\) 米处，距离为 5 米。令 \(r=\sqrt{p^2+a^2}\)，一阶与二阶导数分别为

\[ J_h=\begin{bmatrix}p/r&a/r\end{bmatrix},\qquad \mathcal H_h=\frac1{r^3} \begin{bmatrix}a^2&-pa\\-pa&p^2\end{bmatrix}. \]

代入 \((3,4)\)，得到

\[ J_h=\begin{bmatrix}0.6&0.8\end{bmatrix},\qquad \mathcal H_h= \begin{bmatrix}0.128&-0.096\\-0.096&0.072\end{bmatrix}\ \mathrm m^{-1}. \]

例如 \(h_p=p/r\)，再对 \(p\) 求导得到 \(h_{pp}=1/r-p^2/r^3=a^2/r^3\)；对 \(a\) 求导则得到 \(h_{pa}=-pa/r^3\)。其余两项同理。

目标在附近移动 \((\Delta p,\Delta a)\) 后，距离的二阶近似为

\[ h(3+\Delta p,4+\Delta a) \approx5+0.6\Delta p+0.8\Delta a +\frac12\left[0.128(\Delta p)^2 -0.192\Delta p\Delta a+0.072(\Delta a)^2\right]. \]

所有长度都以米代入。只沿水平方向移动时，可以比较两种近似：

小范围移动时，局部斜率已经能给出较好的结果；移动范围增大后，忽略曲率造成的差别更明显。二阶项在这里改善了近似，但并不意味着可以把展开式用于任意远的位置。

还可以看一个更能体现曲率的方向：取位移 \((\Delta p,\Delta a)=(-0.8,0.6)\) 米。它垂直于从雷达到 \((3,4)\) 的方向，因此一阶距离变化为

\[ J_h\Delta x=0.6(-0.8)+0.8(0.6)=0. \]

但真实距离变为 \(\sqrt{2.2^2+4.6^2}=\sqrt{26}\approx5.0990\) 米，并没有保持不变。这是沿圆的切线走了一段，而不是沿圆周移动。Hessian 给出的二阶修正为 \(0.1\) 米，预测距离为 5.1 米，解释了一阶近似漏掉的这部分变化。

三、从 g-h 滤波到卡尔曼滤波

3.1 g-h 滤波

运动预测与残差修正

让机器人从位置 0 出发，以每秒 1 米的速度向右运动。为便于比较，先在模拟中知道它前三秒的真实位置为 \(1,2,3\) 米，传感器读数却是 \(1.2,1.9,3.2\) 米。实际滤波器只能收到读数，不能拿真实位置参与更新。

直接采用每个读数会把测量噪声全部带入输出。把截至当前的读数做简单平均，第二秒得到 \(1.55\) 米，第三秒得到 \(2.1\) 米；它们平均的是不同时刻的位置，自然落后于正在前进的机器人。平均对静止目标有帮助，对运动目标还要处理时间变化。

我们因此先用运动模型预测当前位置。若上一时刻位置估计为 \(\hat p_{k-1}\)、速度估计为 \(\hat v_{k-1}\)，间隔为 \(T\)，就先算 \(\hat p_k^-=\hat p_{k-1}+T\hat v_{k-1}\)。上标 \(-\) 表示当前观测到来前的预测，不是负数。

从位置修正形成速度修正

第一秒从 \((\hat p_0,\hat v_0)=(0,1)\) 出发，预测位置为 1，观测为 1.2，残差为 \(r_1=1.2-1=0.2\)。如果暂时决定只接受其中一半的位置差异，修正位置就是 \(1+0.5\times0.2=1.1\)。

一般把位置修正比例写为 \(g\)，得到 \(\hat p_k=\hat p_k^-+gr_k\)。\(g=0\) 表示完全保留运动预测，\(g=1\) 表示位置直接取观测。通常取二者之间来折中，但仅修位置还不能纠正错误的速度。

假如连续很多次观测都在预测右侧，可能不仅是位置偏左，还可能是速度估得偏小。残差除以时间 \(r_k/T\) 具有速度单位；取其中比例 \(h\) 来调整速度，就得到

\[ \begin{aligned} \hat p_k^-&=\hat p_{k-1}+T\hat v_{k-1},\\ r_k&=z_k-\hat p_k^-,\\ \hat p_k&=\hat p_k^-+g r_k,\\ \hat v_k&=\hat v_{k-1}+\frac hT r_k. \end{aligned} \]

两项修正都使用同一份预测残差，不能先把位置修正完，再以新的残差修速度。这就是本文的 g-h 滤波，也叫 \(\alpha\)–\(\beta\) 滤波，其中 \(\alpha=g,\beta=h\)。两个参数都无量纲；\(h/T\) 才是从位置残差映射到速度修正的系数。

连续三步演算

取 \(T=1\) 秒、\(g=0.5,h=0.2\)，初始化 \((0,1)\)。第一步：

\[ \hat p_1^-=0+1=1,\quad r_1=1.2-1=0.2,\quad \hat p_1=1.1,\quad \hat v_1=1+0.2(0.2)=1.04. \]

第二步从刚修正的状态出发：

\[ \hat p_2^-=1.1+1.04=2.14,\quad r_2=1.9-2.14=-0.24, \]

\[ \hat p_2=2.14-0.12=2.02,\quad \hat v_2=1.04-0.048=0.992. \]

第三步：

\[ \hat p_3^-=2.02+0.992=3.012,\quad r_3=3.2-3.012=0.188, \]

\[ \hat p_3=3.012+0.094=3.106,\quad \hat v_3=0.992+0.0376=1.0296. \]

这三步显示了“上一后验成为下一预测的起点”。速度无需直接测量，但位置残差会间接修正它。单次残差也可能只是测量噪声，不能全部解释为速度错误；\(h\) 控制这种修正有多积极。

参数与采样间隔的含义

可以先固定一个具体残差来理解参数。若当前观测比预测右移 1 米，取 \(g=0.2\) 就将位置向右修正 \(0.2\) 米，取 \(g=0.8\) 则修正 \(0.8\) 米。\(g\) 越大，当前位置越快追随读数，也越容易受到测量噪声影响。

仍令残差为 1 米、间隔为 1 秒，取 \(h=0.1\) 就将速度增加 \(0.1\) 米/秒，取 \(h=0.5\) 则增加 \(0.5\) 米/秒。这份速度修正还会影响以后的位置预测，所以 \(h\) 过大时，单次噪声可能被误当成明显的速度变化。

采样间隔也有明确作用。相同的 1 米偏差若在 1 秒内形成，对应的速度差尺度是 1 米/秒；若在 2 秒内形成，对应的速度差尺度就是 \(0.5\) 米/秒。这解释了速度修正为何使用 \(r_k/T\)。改变采样间隔时，运动预测和速度修正都要使用新的 \(T\)，参数也通常需要根据新的噪声与运动条件重新选择。

上例的 \(g=0.5,h=0.2\) 是演示用的固定选择。算法本身尚未告诉我们：传感器误差增大时应该把它们改成多少，或者刚初始化时应该怎样与稳定跟踪时区别对待。这需要引入下一节的不确定性描述。

g-h 与高斯–厄米特的名称辨析

一些素材把 GH 解释为 Gauss–Hermite，这与这里的 g-h 是两个名称相似的方法。高斯–厄米特求积使用节点和权重近似积分，不是这里用两个固定比例修正位置和速度的算法。本文只使用后者；不需要求积节点或非线性滤波代码。

固定增益的局限

现在机器人进入遮挡区，传感器误差标准差由 \(0.2\) 米增为 2 米。假定运动预测仍较可靠，同样出现 1 米残差：在原来的测量噪声下，这个偏差很突出；在遮挡后的噪声下，它可能只是普通的读数波动。固定的 \(g,h\) 却仍会接受相同比例。

同样地，刚初始化时速度几乎未知，长时间稳定跟踪后速度已经较可信，二者不宜使用完全相同的修正方式。为决定该信多少预测、多少测量，需要保存不确定性。下一节先保存整组可能状态；再利用高斯结构，把整组分布压缩为均值与协方差。

3.2 贝叶斯滤波

g-h 滤波只保留一个位置和一个速度估计。现在换一种表示：不急着选出唯一位置，而是先记录机器人可能位于哪里，以及每个位置有多大概率。

接下来把两种不确定性分开处理。机器人运动时，将旧位置的概率沿可能的运动路径传播，得到预测；传感器报告到来后，用 2.2 已经算过的贝叶斯更新重新分配权重。先在少量网格上完整算两轮，再写出一般公式。

离散状态的预测与更新

仍沿一条直线运动，位置单位取 1 米，候选网格为所有整数。初始时只在 \(0,1,2\) 处有非零概率：

\[ b_0(0)=0.2,\quad b_0(1)=0.5,\quad b_0(2)=0.3. \]

这里 \(b_0(i)\) 表示初始时刻位于位置 \(i\) 的概率，三个数合计为 1。下标表示时间，括号中的数表示位置，二者不要混淆。

命令要求向右移动 1 格，但实际可能停留、移动 1 格、移动 2 格，概率分别为 \(0.2,0.6,0.2\)。这是局部运动误差：从一个给定起点出发，只能到达附近三个终点。

这里的 \(0.2,0.6,0.2\) 是人为为示例指定的状态转移概率，决定预测时怎样分配各条运动路径的权重，并不是贝叶斯滤波规定的固定值。实际使用时，可以根据经验设定，也可以从历史运动数据中统计、学习，或在运行过程中通过在线估计调整。调整后，各种可能位移的概率仍须非负且合计为 1，并应反映实际运动误差。

先只看起点 0。机器人在这里的概率为 \(0.2\)，从这里恰好右移 1 格的概率为 \(0.6\)，所以经由这条路径到达位置 1 的贡献为 \(0.2\times0.6=0.12\)。从起点 1 停留，也可以到达位置 1，这条路径的贡献为 \(0.5\times0.2=0.10\)。两条来源不能互相覆盖，应该相加。

将各起点的全部路径列出：

同一终点可以有多条来源，把它们累加：

\[ b_1^-=(0.04,\ 0.12+0.10,\ 0.04+0.30+0.06,\ 0.10+0.18,\ 0.06) =(0.04,0.22,0.40,0.28,0.06). \]

预测概率之和仍为 1。程序中对目标格执行“加上贡献”而非“覆盖赋值”，对应的就是这个汇总步骤。这里采用无限整数网格，当前只列出非零支持；若采用有界地图，必须明确越界的物理处理，不能无意使用取模把直线变成环。

第一轮观测更新

完成运动预测后，才使用本次传感器报告。设报告模型为 \(Z=X+V\)，其中 \(V=-1,0,1\) 的概率分别为 \(0.1,0.8,0.1\)。这组概率描述传感器报告的误差；前面的 \(0.2,0.6,0.2\) 描述执行移动命令时的误差。两种误差分别建模，概率参数不必相同。

收到报告 \(z_1=2\) 时，如果真实位置为 2，需要测量误差为 0，似然就是 \(0.8\)；如果真实位置为 1，需要误差为 1，似然就是 \(0.1\)；位置为 3 时同理。位置 0 和 4 无法在这个测量模型下产生报告 2，似然为零。

似然的权重从哪里来，能否调整？ 本例的测量误差概率 \(0.1,0.8,0.1\) 也是人为指定的模型参数；固定报告后，它们就成为对应候选位置的似然值。它们可以通过传感器标定数据提前学习，例如统计已知真实位置时各种测量误差出现的频率；也可以根据在线误差估计或当前测量条件，动态计算、调整。贝叶斯公式负责用给定的模型完成更新，并不会自动确定这些参数。作为概率模型，对每个给定真实位置，所有可能报告的概率须非负且合计为 1；固定一个报告后，不同候选位置的似然值则不要求合计为 1。

从“决定新观测怎样影响估计”的角度，观测似然可以类比卡尔曼滤波中的增益 \(K\)、g-h 滤波中的更新权重 \(g,h\)。但似然用于逐点乘以预测概率，再归一化；\(K\) 和 \(g,h\) 则用于加权观测残差，二者不是同一个数学量。标准卡尔曼滤波的 \(K\) 由预测协方差、观测模型和测量噪声协方差计算得到，通常通过调整或学习噪声参数来影响它。对于这里的离散更新，调节似然的相对大小会改变观测的作用：在本例对称误差模型中，中心概率越高、两侧概率越低，后验通常越集中于报告附近；三者越接近，观测区分候选位置的能力越弱。因此，这些值是可调的，但应以数据和误差模型为依据。

接下来重复 2.2 的计算：每个位置的预测概率乘以似然，再除以贡献之和。

证据为 \(0.022+0.320+0.028=0.370\)，后验为 \((11,160,14)/185\)，均值为 \((11+320+42)/185=373/185\approx2.0162\) 米。零似然来自这个有界测量误差模型；如果实际可能产生更远离的读数，就应使用带尾部的模型。

第二轮从上一后验继续递推

继续发送相同移动命令。将第一轮后验沿相同三条局部路径传播：

例如终点 3 的三份贡献分别来自位置 3 停留、位置 2 右移 1 格、位置 1 右移 2 格。它们的分子为 \(14,480,11\)，合计为 505。所有终点的分子合计为 925，说明运动传播后总概率仍为 1。

再收到 \(z_2=3\)，便固定这个新报告，按测量模型计算似然并更新：

证据为 \(4435/9250\approx0.479459\)，后验之和为 1，均值为 \((386+12120+808)/4435=13314/4435\approx3.0020\) 米。第二轮使用的是第一轮更新后的分布，不是重新使用初始先验，也不必重新逐条读取全部历史观测。

下面用静态流程表示这两轮的共同结构：

上一后验 ── 沿各条运动路径加权汇总 ──> 当前预测
                                            │
                                   逐点乘当前观测的似然
                                            ↓
                                       除以证据
                                            ↓
                                         当前后验
                                            │
                                     下一轮的起点

贝叶斯滤波的递推公式

把终点为 \(j\)、起点为 \(i\) 的转移概率记为 \(T_k(j\mid i,u_k)\)，当前观测似然记为 \(L_k(z_k\mid j)\)。上面两轮计算可写为

\[ b_k^-(j)=\sum_i T_k(j\mid i,u_k)b_{k-1}(i), \]

\[ b_k(j)=\frac{L_k(z_k\mid j)b_k^-(j)} {\sum_\ell L_k(z_k\mid\ell)b_k^-(\ell)}. \]

对每个起点 \(i\)，应有 \(\sum_j T_k(j\mid i,u_k)=1\)，于是交换有限求和次序便得 \(\sum_jb_k^-(j)=\sum_ib_{k-1}(i)=1\)。预测本身使用全概率公式；更新使用贝叶斯公式，其证据也用全概率公式求得。二者共享联合概率结构，但保留的变量与所回答的问题不同。

网格细化后得到连续贝叶斯滤波

离散预测的操作已经明确：对某个终点，遍历所有起点，将“起点概率乘以到该终点的转移概率”相加。连续预测保留同样的操作，只是起点和终点都变成小区间。

令旧位置候选为 \(\xi_i\)，旧网格宽度为 \(\Delta\xi\)；固定当前终点附近的区间，中心为 \(x\)、宽度为 \(\Delta x\)。旧位置落在第 \(i\) 格的概率近似为

\[ p(\xi_i\mid z_{1:k-1})\Delta\xi. \]

给定旧位置 \(\xi_i\)，运动后进入当前终点区间的概率近似为 \(p(x\mid\xi_i,u_k)\Delta x\)。两者相乘就是从这一个起点到该终点区间的贡献。将所有起点的贡献相加：

\[ p^-(x)\Delta x \approx\sum_i \big[p(x\mid\xi_i,u_k)\Delta x\big] \big[p(\xi_i\mid z_{1:k-1})\Delta\xi\big]. \]

两边除以相同的终点区间宽度 \(\Delta x\)，再让起点网格不断细化，就得到

\[ p^-(x)=\int p(x\mid\xi,u_k) p(\xi\mid z_{1:k-1})\,d\xi. \]

这里 \(p^-(x)\) 就是 \(p(x_k=x\mid z_{1:k-1})\)。计算时固定终点 \(x\)，遍历所有旧位置 \(\xi\)；对每一个终点分别完成这个积分，才得到整条预测密度。积分消去的是旧位置，保留的是当前位置。已知控制历史默认省略在条件栏之外。

从联合分布看预测使用的条件

如果不作模型假设，转移密度还应以过去观测为条件，写成 \(p(x\mid\xi,z_{1:k-1},u_k)\)。2.2 的马尔可夫假设允许我们在已知旧状态与当前控制后，使用较简单的 \(p(x\mid\xi,u_k)\)。历史没有被丢掉，它仍通过上一后验 \(p(\xi\mid z_{1:k-1})\) 起作用。

当前报告 \(z_k\) 到来后，再按 2.2 的连续更新计算

\[ p(x_k\mid z_{1:k}) =\frac{p(z_k\mid x_k)p(x_k\mid z_{1:k-1})} {\int p(z_k\mid\eta)p(\eta\mid z_{1:k-1})\,d\eta}. \]

这里使用了测量条件独立：给定当前状态，不再额外需要历史观测来解释当前报告。分母遍历的是当前状态候选 \(\eta\)，计算这个报告在预测下的总密度，即证据。

可以把两处积分分别读成：预测时，汇总“所有旧位置怎样到达这个新位置”；更新的分母中，汇总“所有新位置怎样产生这份报告”。前者给出当前位置的密度曲线，后者在报告固定后给出一个归一化常数。

高斯模型下的预测与更新

前面的整数网格已经展示了全部路径怎样汇总。现在改用连续位置，并选择第一章已经介绍过的高斯分布，使积分可以完整算出来。

假设当前后验位置 \(X\sim\mathcal N(10,4)\)：均值为 10 米，方差为 4，标准差为 2 米。本次实际位移 \(D\sim\mathcal N(1,1)\)：平均右移 1 米，但实际位移有噪声，方差为 1。这里约定 \(\mathcal N(\mu,P)\) 的第二个参数始终是方差，并假设 \(X,D\) 独立。

移动后位置 \(Y=X+D\)。例如终点为 12 时，起点可以是 10、位移为 2，也可以是 11、位移为 1，还可以有其他组合。要计算终点 12 附近的密度，必须将这些拆分对应的贡献汇总。

将终点一般地记为 \(y\)，起点记为 \(\xi\)，所需位移就是 \(y-\xi\)。代入刚才的连续预测公式，得到

\[ p_Y(y)=\int p_D(y-\xi)p_X(\xi)\,d\xi. \]

这已经是卷积：一个密度描述起点，另一个密度描述补足到终点所需的位移。写出两个具体密度，

$ p_X()=e^{-()2/8}, p_D(y-)=e^{-(y-)2/2}, $

所以

$ p_Y(y)= d. $

为了对 \(\xi\) 积分，先将指数整理成“关于 \(\xi\) 的平方，加上不含 \(\xi\) 的部分”。展开与配方得到

\[ \frac{(\xi-10)^2}{4}+(y-\xi-1)^2 =\frac54\left(\xi-\frac{4y+6}{5}\right)^2 +\frac{(y-11)^2}{5}. \]

固定终点 \(y\) 时，\((y-11)^2/5\) 不随积分变量 \(\xi\) 变化，可以移到积分外。令 \(m_y=(4y+6)/5\)，则

$ p_Y(y)=e^{-(y-11)2/10} e^{-(-m_y)2},d. $

积分内是一个未归一化高斯，其积分为 \(\sqrt{2\pi/(5/4)}\)。它与 \(1/(4\pi)\) 相乘得到 \(1/\sqrt{10\pi}\)，于是

\[ p_Y(y)=\frac1{\sqrt{2\pi\cdot5}} \exp\left[-\frac{(y-11)^2}{2\cdot5}\right], \qquad Y\sim\mathcal N(11,5). \]

结果的均值为 \(10+1=11\)，方差为 \(4+1=5\)。旧位置有不确定性，实际位移又引入新的独立不确定性，所以移动后更分散。

第一章已给出一般卷积推导。若旧位置为 \(\mathcal N(\mu,P)\)，独立位移为 \(\mathcal N(u,Q)\)，同样的计算给出 \(\mathcal N(\mu+u,P+Q)\)。这里相加的是随机位置与随机位移，不是两条密度曲线。

高斯测量下的更新是密度乘积

预测完成后，机器人可能位置的分布为 \(\mathcal N(11,5)\)。现在收到读数 \(z=12\)，并且传感器测量噪声方差为 \(R=4\)。接下来不再移动机器人，而是利用这份读数重新判断同一个当前位置。

先写出两份信息：预测密度描述候选位置原本有多可能，似然描述各个位置产生报告 12 的程度。忽略与候选 \(x\) 无关的共同系数，它们分别为

$ p^-(x)e{-(x-11)^2/(2)}, p(12x)e^{-(12-x)2/(2)}. $

按贝叶斯更新将两者逐点相乘，指数相加，得到

\[ p(x\mid z=12)\propto \exp\left[-\frac12\left(\frac{(x-11)^2}{5} +\frac{(12-x)^2}{4}\right)\right]. \]

展开并配方：

\[ \begin{aligned} \frac{(x-11)^2}{5}+\frac{(12-x)^2}{4} &=\frac9{20}x^2-\frac{52}{5}x+\frac{301}{5}\\ &=\frac9{20}\left(x-\frac{104}{9}\right)^2+\frac19. \end{aligned} \]

配方后，最后的 \(1/9\) 与候选 \(x\) 无关，只会给整条曲线乘上同一个系数。去掉这个共同系数后，后验的形状为

$ p(xz=12) . $

将它与高斯密度的形式 \(\exp[-(x-\mu)^2/(2P)]\) 对照：平方中心给出 \(\mu=104/9\)，而 \(1/P=9/20\)，所以 \(P=20/9\)。补回使面积为 1 的系数，就得到

\[ X\mid z=12\sim\mathcal N\left(\frac{104}{9},\frac{20}{9}\right). \]

后验均值约为 \(11.5556\)，位于预测中心 11 与读数 12 之间；方差约为 \(2.2222\)，小于预测方差 5。密度相乘使二次项系数由 \(1/5\) 增为 \(1/5+1/4=9/20\)，曲线因此更集中。这就是下一节“按可靠程度修正”的计算来源。

如果还要显式求证据，可以先预测传感器读数的分布。读数等于预测位置加测量噪声，因此由刚算过的独立高斯加法得到 \(Z\sim\mathcal N(11,5+4)\)。再在收到的 \(z=12\) 处取值：

$ p(z=12)=. $

它是本次报告的预测密度，用于把后验归一化；固定报告后它只是一个数。这个证据积分虽然也能写成卷积，但被保留的是观测变量，而更新分子保留的是状态变量，不能将二者混为一谈。

三种运算使用同样的加乘符号，却处理不同对象

全概率公式中的求和是对路径贡献求和；只有加性、独立且平移形式的转移模型才成为普通卷积。贝叶斯分母对已经相乘的贡献求和，不是在用“高斯相加”替换分子的“高斯相乘”。

在加性独立模型下，预测方差 \(P^-=P+Q\ge P\)；在线性高斯测量下，后验方差满足 \(1/P^+=1/P^-+1/R\)，所以 \(0<P^+<P^-,R\)。这依赖正的有限方差及正确的独立噪声模型。不能把“更新一定变窄”推广到任意非高斯条件分布，也不能把“预测一定扩大每个方向方差”推广到任意 \(F\)。两个使用了同一份数据的后验更不能直接相乘，否则会重复计算证据。

3.3 卡尔曼滤波

上一节的两次高斯计算表明：运动预测之后还是高斯，测量更新之后也还是高斯。因此，在这些模型假设成立时，不必保存整条密度曲线，只保存均值与方差，就能重建完整分布。增加状态维度后，将方差换成协方差矩阵即可。

本节先由一维运动模型推导预测，再将高斯更新整理成残差修正，完成完整递推后连续算几步。随后让速度也变成未知量，逐项看位置观测怎样影响速度，最后才整理成矩阵递推。

一维卡尔曼滤波

本小节只把位置作为未知状态，假设每一步的平均位移已知。真实位移仍有不确定性；速度也未知的情况，留到下一小节扩充状态后处理。

从上一时刻的后验开始

记上一时刻的位置后验为

\[ x_{k-1}\mid z_{1:k-1}\sim\mathcal N(m_{k-1},P_{k-1}). \]

其中 \(m_{k-1}\) 是已经使用上一时刻观测后的估计位置，\(P_{k-1}\) 是对应方差。滤波器接下来要先预测当前位置，再利用当前读数更新。

设本步平均位移为已知数 \(u_k\)，实际位移相对于它的误差为 \(w_k\)，则真实运动模型是

\[ x_k=x_{k-1}+u_k+w_k,\qquad w_k\sim\mathcal N(0,Q_k). \]

例如 \(u_k=1\) 表示平均右移 1 米，不表示实际位移恰好为 1 米；\(Q_k\) 描述这一步位移误差的方差。这里状态只有位置，因此 \(Q_k\) 是一个标量，与前面位置—速度模型中的过程协方差矩阵维数不同。

传感器直接测位置，测量模型为

\[ z_k=x_k+\nu_k,\qquad \nu_k\sim\mathcal N(0,R_k). \]

假设各时刻的过程噪声、测量噪声相互独立，并与初始状态独立。下面的均值、方差及协方差均以已有观测 \(z_{1:k-1}\) 为条件；为简洁，推导中省略条件栏。

上标 \(-\) 表示当前观测到来前的预测，上标 \(+\) 表示加入当前观测后的结果。写时间下标时，直接用 \(m_k,P_k\) 表示后验，省略上标 \(+\)。

预测均值与方差

在收到 \(z_k\) 之前，先对运动方程取期望。由于 \(u_k\) 已知、\(w_k\) 均值为零，

\[ \begin{aligned} m_k^- &=E[x_{k-1}+u_k+w_k]\\ &=E[x_{k-1}]+u_k+E[w_k]\\ &=m_{k-1}+u_k. \end{aligned} \]

再计算预测方差。已知位移 \(u_k\) 只是把分布整体平移，不增加不确定性，所以

\[ \begin{aligned} P_k^- &=\operatorname{Var}(x_{k-1}+u_k+w_k)\\ &=\operatorname{Var}(x_{k-1}+w_k)\\ &=P_{k-1}+2\operatorname{Cov}(x_{k-1},w_k)+Q_k\\ &=P_{k-1}+Q_k. \end{aligned} \]

最后一步使用了本步过程噪声与旧状态独立的假设，因此交叉协方差为零。独立高斯相加仍为高斯，得到当前观测前的预测分布：

\[ x_k\mid z_{1:k-1}\sim\mathcal N(m_k^-,P_k^-). \]

这就是上一节卷积预测在均值、方差层面的结果。比如上一后验为 \(\mathcal N(10,4)\)，本步平均位移为 1，位移噪声方差为 1，则

\[ m_k^-=10+1=11,\qquad P_k^-=4+1=5. \]

因此，\(\mathcal N(11,5)\) 是通过上一后验与运动模型预测出来的，而不是更新时额外给定的一个起点。

根据当前观测更新

现在收到读数 \(z_k=12\)，测量噪声方差为 \(R_k=4\)。沿用刚才预测出的 \(\mathcal N(11,5)\)，将预测密度乘以当前读数的似然并归一化。上一节的高斯配方已经算出后验均值为 \(104/9\)、方差为 \(20/9\)。先把后验均值整理成残差修正：

\[ \begin{aligned} \frac{104}{9} &=\frac{4(11)+5(12)}{5+4}\\ &=\frac{(5+4)11+5(12-11)}{5+4}\\ &=11+\frac59(12-11). \end{aligned} \]

这里的 \(5/9\) 等于预测方差除以两份方差之和，即 \(5/(5+4)\)。因此，“预测加上一部分残差”的结构可以从高斯更新结果中整理出来。接下来先推导一般修正比例，再说明同一个比例为什么也出现在方差更新中。

把预测均值记为 \(m^-\)、预测方差记为 \(P^-\)、测量噪声方差记为 \(R\)。沿用高斯配方中二次项与一次项的系数比较，后验方差与均值分别为

\[ P^+=\left(\frac1{P^-}+\frac1R\right)^{-1} =\frac{P^-R}{P^-+R}, \]

\[ m^+=P^+\left(\frac{m^-}{P^-}+\frac zR\right) =\frac{Rm^-+P^-z}{P^-+R} =m^-+\frac{P^-}{P^-+R}(z-m^-). \]

将均值修正中出现的比例定义为卡尔曼增益：

\[ K=\frac{P^-}{P^-+R}. \]

于是均值更新写成 \(m^+=m^-+K(z-m^-)\)。再回到已经由配方得到的后验方差，逐步改写：

\[ \begin{aligned} P^+ &=\frac{P^-R}{P^-+R}\\ &=P^-\frac{R}{P^-+R}\\ &=P^-\left(1-\frac{P^-}{P^-+R}\right)\\ &=(1-K)P^-. \end{aligned} \]

其中第三行只是使用 \(R=(P^-+R)-P^-\)，不是另加了一条方差运算规则。这就解释了同一个 \(K\) 为什么同时出现在均值与方差的更新中。

代回本例的 \(P^-=5,R=4\)，有 \(K=5/9\)，所以

\[ P^+=\frac{5(4)}{5+4} =5\frac49 =5\left(1-\frac59\right) =\frac{20}{9}. \]

因此，原来的数值等式可以明确写为

\[ \frac{20}{9}=\left(1-\frac59\right)5. \]

这是从高斯更新的方差结果整理出的等价形式。现在将均值与方差的两条递推合在一起：

\[ m^+=m^-+K(z-m^-),\qquad P^+=(1-K)P^-. \]

可以用同一个预测 \(m^-=11,P^-=5\) 与读数 \(z=12\)，单独改变测量方差来观察增益的作用：

测量越不可靠，修正越小；固定测量方差时，预测越不确定，修正越大。这样就回应了 g-h 滤波留下的问题：修正比例可以由两份信息的不确定性计算出来。

将预测与更新接成完整递推

把时间下标补齐，一轮一维卡尔曼滤波为：

\[ \begin{aligned} \text{预测：}\quad m_k^-&=m_{k-1}+u_k,\\ P_k^-&=P_{k-1}+Q_k,\\[2pt] \text{更新：}\quad r_k&=z_k-m_k^-,\\ K_k&=\frac{P_k^-}{P_k^-+R_k},\\ m_k&=m_k^-+K_kr_k,\\ P_k&=(1-K_k)P_k^-. \end{aligned} \]

预测使用上一后验与本步运动模型，不使用尚未到来的当前读数；更新才使用 \(z_k\)。更新后的 \((m_k,P_k)\) 随后成为下一轮预测的起点。

例如刚才更新得到 \(m_k=104/9,P_k=20/9\)。若下一步仍平均移动 1 米、过程噪声方差仍为 1，则下一次观测到来前先算

\[ m_{k+1}^-=\frac{104}{9}+1=\frac{113}{9},\qquad P_{k+1}^-=\frac{20}{9}+1=\frac{29}{9}. \]

再用下一份读数更新这组预测，而不是继续更新上一时刻的位置。下面换一个初始位置，从头连续演算几轮。

已知位移时的一维多步计算

若每次已知平均位移为 1 米，过程扰动方差 \(Q=1\)，传感器噪声方差 \(R=4\)，则从初始 \(m_0=0,P_0=4\) 开始，第一步预测为 \((1,5)\)。收到 \(z_1=1.2\)：

\[ K_1=\frac5{5+4}=\frac59,\quad m_1=1+\frac59(0.2)=1.111111,\quad P_1=\left(1-\frac59\right)5=\frac{20}{9}. \]

第二步从这个后验继续，\(m_2^-=2.111111\)，\(P_2^-=20/9+1=29/9\)，因此 \(K_2=29/65\)。用 \(z_2=1.9\) 修正得到 \(m_2\approx2.016923,P_2=116/65\approx1.784615\)。完整递推为

\[ m_k^-=m_{k-1}+1,\quad P_k^-=P_{k-1}+Q,\quad K_k=\frac{P_k^-}{P_k^-+R}, \]

\[ m_k=m_k^-+K_k(z_k-m_k^-),\qquad P_k=(1-K_k)P_k^-. \]

表中保留六位小数，下一步使用未舍入结果。每轮预测增加方差，测量再降低方差；后验并未逐次压到零，因为运动仍会持续引入未知扰动。

位置与速度的联合估计

现在平均速度也未知，不能每步都机械地给位置加 1。用 \(x=(p,v)^T\)，\(T=1\)，初始均值 \(\hat x_0=(0,1)^T\)，初始误差协方差 \(P_0=\operatorname{diag}(4,1)\)。位置和速度的初始误差暂不相关，但速度不确定性会通过运动生成交叉协方差。

例如初始等可能误差可取 \((2,1),(2,-1),(-2,1),(-2,-1)\)。它们的协方差正好是 \(\operatorname{diag}(4,1)\)。经过匀速传播后变为 \((3,1),(1,-1),(-1,1),(-3,-1)\)，位置方差为 5，速度方差为 1，交叉协方差为 1。位置偏大与速度偏大开始相互关联。这里只用四点演示二阶矩传播，实际卡尔曼模型采用具有同样初始协方差的联合高斯状态。

先规定运动模型：采样间隔为 \(T=1\) 秒，每段恒定随机加速度的方差取 \(\sigma_a^2=1\)，因此

\[ F=\begin{bmatrix}1&1\\0&1\end{bmatrix},\qquad Q=\begin{bmatrix}1/4&1/2\\1/2&1\end{bmatrix}. \]

再独立规定测量模型：传感器只测位置，读数为

\[ z=Hx+\nu=p+\nu,\qquad H=\begin{bmatrix}1&0\end{bmatrix}. \]

本例采用零均值高斯测量噪声，并取其方差为

\[ R=\operatorname{Var}(\nu)=1\ \mathrm m^2. \]

也就是测量噪声标准差为 1 米，并假设它与预测状态误差独立。这个 \(R\) 是对传感器噪声单独作出的设定，不由运动噪声 \(Q\) 推出，也不沿用上一小节一维示例中的 \(R=4\)。

先完成运动预测。预测均值为 \((1,1)^T\)，预测协方差为

\[ P_1^-=FP_0F^T+Q =\begin{bmatrix}5&1\\1&1\end{bmatrix} +\begin{bmatrix}1/4&1/2\\1/2&1\end{bmatrix} =\begin{bmatrix}5.25&1.5\\1.5&2\end{bmatrix}. \]

先把预测协方差中的三个数读出来：位置误差方差为 \(5.25\)，速度误差方差为 2，二者的协方差为 \(1.5\)。接下来只测位置，必须回答两个问题：位置应修正多少，以及为什么速度也应该修正。

先计算残差包含多少不确定性

预测位置为 1 米，若收到 \(z_1=1.2\)，实际可计算的残差为 \(r_1=1.2-1=0.2\) 米。但在接收读数之前，残差也是随机的：真实位置可能偏离预测，传感器本身也会出错。

令预测位置误差为 \(e_p^-=p-\hat p^-\)，测量噪声为 \(\nu\)。由 \(z=p+\nu\) 可得

\[ r=z-\hat p^-=e_p^-+\nu. \]

按照本例已经规定的测量模型，\(\operatorname{Var}(\nu)=R=1\)，而预测位置误差方差为 \(5.25\)。展开残差方差，再使用两种误差独立的假设：

\[ \begin{aligned} S=\operatorname{Var}(r) &=\operatorname{Var}(e_p^-+\nu)\\ &=\operatorname{Var}(e_p^-) +2\operatorname{Cov}(e_p^-,\nu) +\operatorname{Var}(\nu)\\ &=5.25+0+R\\ &=5.25+1=6.25. \end{aligned} \]

因此不能把观测比预测大出的部分，全部当作真实位置偏差：其中可能包含测量噪声。刚才的 \(0.2\) 是本次残差的实际数值，\(6.25\) 则描述收到观测之前它可能怎样波动。

用修正后误差的方差选择系数

先看位置。假设用残差的 \(a\) 倍修正预测，那么修正后的误差就是 \(e_p^--ar\)。因为测量噪声与状态误差独立，

\[ \operatorname{Cov}(e_p^-,r) =\operatorname{Cov}(e_p^-,e_p^-+\nu)=5.25. \]

展开修正后误差的方差：

\[ \begin{aligned} \operatorname{Var}(e_p^--ar) &=5.25-2a(5.25)+a^2(6.25)\\ &=6.25(a-0.84)^2+0.84. \end{aligned} \]

平方项在 \(a=0.84\) 时最小，所以位置的修正系数为 \(0.84\)，修正后的位置方差为 \(0.84\)。这个结果也等于一维公式 \(5.25/(5.25+1)\)。

再看速度。假设用残差的 \(b\) 倍修正预测速度，误差变为 \(e_v^--br\)。测量噪声与速度误差独立，所以

\[ \operatorname{Cov}(e_v^-,r) =\operatorname{Cov}(e_v^-,e_p^-)=1.5. \]

速度误差的方差相应为

\[ \begin{aligned} \operatorname{Var}(e_v^--br) &=2-2b(1.5)+b^2(6.25)\\ &=6.25(b-0.24)^2+1.64. \end{aligned} \]

于是 \(b=0.24\) 时最小。位置残差之所以能修正速度，依据正是预测误差之间的协方差 \(1.5\)。若它为零，速度方差表达式中就没有一次项，此时最优系数为零，这次位置读数不会直接修正速度。

将两个系数写成一个列向量，就是本次卡尔曼增益：

\[ K=\frac1{6.25}\begin{bmatrix}5.25\\1.5\end{bmatrix} =\begin{bmatrix}0.84\\0.24\end{bmatrix}. \]

将同一个残差用于联合状态修正

现在代入已经收到的残差 \(0.2\)，得到

\[ \hat x_1=\begin{bmatrix}1\\1\end{bmatrix} +\begin{bmatrix}0.84\\0.24\end{bmatrix}(0.2) =\begin{bmatrix}1.168\\1.048\end{bmatrix}. \]

位置增加 \(0.168\) 米，速度增加 \(0.048\) 米/秒。传感器仍只测了位置；速度修正来自模型保存的位置—速度误差关联。

两个方差刚才已经通过配方求出。修正后的交叉协方差也需要更新：

\[ \begin{aligned} \operatorname{Cov}(e_p^--0.84r,e_v^--0.24r) &=1.5-0.24(5.25)-0.84(1.5)+0.84(0.24)(6.25)\\ &=0.24. \end{aligned} \]

所以后验协方差为

\[ P_1=\begin{bmatrix}0.84&0.24\\0.24&1.64\end{bmatrix}. \]

位置和速度的不确定性都减小了，但单次位置观测并没有把速度确定下来。将这些逐项计算合并，可以写成 \(P_1=P_1^--K\begin{bmatrix}5.25&1.5\end{bmatrix}\)；后面会推导它的一般矩阵形式。

再走一步检验修正的延续

第二步预测均值为 \((1.168+1.048,1.048)^T=(2.216,1.048)^T\)。从上一次后验传播：

\[ P_2^-= \begin{bmatrix}0.84+2(0.24)+1.64&0.24+1.64\\ 0.24+1.64&1.64\end{bmatrix}+Q =\begin{bmatrix}3.21&2.38\\2.38&2.64\end{bmatrix}. \]

所以 \(S_2=4.21\)，\(K_2=(3.21,2.38)^T/4.21\)。读数 \(1.9\) 带来残差 \(-0.316\)，修正为 \((1.975059,0.869359)^T\)。三步结果如下：

位置增益无量纲，速度增益的单位为每秒。两个增益都可以随历史不确定性变化，而不是两个任意固定的平滑系数。

矩阵形式的卡尔曼递推

刚才的演算已经完成了一轮完整滤波：传播状态均值与误差协方差，计算残差及其方差，选择使修正后误差最小的系数，最后更新均值和协方差。矩阵公式把这些逐项操作同时应用于所有状态分量。

设状态有 \(n\) 维、观测有 \(m\) 维，采用线性模型

\[ x_k=F_kx_{k-1}+B_ku_k+w_k,\qquad z_k=H_kx_k+\nu_k. \]

假设初始状态为高斯，\(w_k,\nu_k\) 为零均值高斯噪声，协方差分别为 \(Q_k,R_k\)；各时刻噪声相互独立，并与初始状态独立。控制和模型参数已知。所有期望与协方差都以截至上一时刻的观测为条件，为简洁省略这一条件。下面省略下标 \(k\) 时，所有量均对应同一当前步骤。

预测均值与协方差

刚才数值例中的 \(F\hat x\) 和 \(FPF^T+Q\)，由 2.3 的展开一般化为

\[ \hat x_k^-=F_k\hat x_{k-1}+B_ku_k,\qquad P_k^-=F_kP_{k-1}F_k^T+Q_k. \]

\(F\) 为 \(n\times n\)，\(P,Q\) 为 \(n\times n\)；\(B\) 的列数等于控制向量维数。线性变换保持高斯，加上独立高斯噪声仍是高斯，所以预测分布完全由这两个量确定。

观测残差及其与预测误差的交叉协方差

前面的数值例子用“测量位置减去预测位置”得到残差。一般观测可能只包含部分状态，或者是状态的线性组合，因此要先将预测状态转换为预测观测：

\[ \hat z^-=H\hat x^-. \]

实际观测与预测观测之差为

\[ r=z-\hat z^-=z-H\hat x^-. \]

这个更新前的观测残差也称为新息（innovation）。它描述当前观测中尚未由预测解释的部分。为与前面的演算一致，下文统一称为“观测残差”。

观测残差与状态预测误差是两个不同的量。预测误差为 \(e^-=x-\hat x^-\)；由测量模型 \(z=Hx+\nu\)，

\[ \begin{aligned} r&=(Hx+\nu)-H\hat x^-\\ &=H(x-\hat x^-)+\nu\\ &=He^-+\nu. \end{aligned} \]

因此，残差同时包含预测状态误差经过 \(H\) 映射后的影响，以及测量噪声。它不等于测量噪声 \(\nu\)，也不等于状态预测误差 \(e^-\)。

下面要计算两件事：残差自身有多不确定，以及残差与每个状态分量的预测误差有多大关联。仍以过去观测为条件，在当前观测尚未固定时计算这些统计量。收到读数后，可计算出残差的具体值；这个具体值与其预测协方差是不同对象。

由于 \(\hat x^-\) 是预测分布的条件均值，\(E[e^-]=0\)；测量噪声也为零均值，所以

\[ E[r]=H E[e^-]+E[\nu]=0. \]

先计算观测残差的协方差，记为 \(S\)。因为残差均值为零，

\[ \begin{aligned} S=\operatorname{Cov}(r) &=E[rr^T]\\ &=E[(He^-+\nu)(He^-+\nu)^T]\\ &=HP^-H^T+H E[e^-\nu^T] +E[\nu(e^-)^T]H^T+R\\ &=HP^-H^T+R. \end{aligned} \]

最后一步使用了测量噪声与预测误差独立的假设。\(S\) 为 \(m\times m\)：它衡量整个观测残差向量的波动，包含预测误差与测量噪声两种来源；\(R\) 则只描述测量噪声。

再计算状态预测误差与观测残差的交叉协方差，记为 \(C\)。两者均值均为零，因此

\[ \begin{aligned} C=\operatorname{Cov}(e^-,r) &=E[e^-r^T]\\ &=E[e^-(He^-+\nu)^T]\\ &=E[e^-(e^-)^T]H^T+E[e^-\nu^T]\\ &=P^-H^T. \end{aligned} \]

\(C\) 为 \(n\times m\)，第 \((i,j)\) 个元素是第 \(i\) 个状态预测误差与第 \(j\) 个观测残差的协方差。它描述观测残差能够怎样提示各个状态分量的预测偏差。交叉协方差的两方可以有不同维数，因此 \(C\) 不必是方阵，也不要求对称。

在刚才只测位置的数值例中，

\[ S=6.25,\qquad C=\begin{bmatrix}5.25\\1.5\end{bmatrix}. \]

其中 \(5.25=\operatorname{Cov}(e_p^-,r)\)，\(1.5=\operatorname{Cov}(e_v^-,r)\)。这两个数正是前面分别推导位置、速度修正系数时使用的量。它们经由残差方差调整后，构成增益 \(K=CS^{-1}\)；下面从误差最小化推导这个关系。

这里统一将 \(C\) 称为“预测误差与观测残差的交叉协方差”，使名称与公式中的 \(e^-,r\) 对应。在过去观测固定时，\(\hat x^-\) 是确定量，因而 \(\operatorname{Cov}(x,r)=\operatorname{Cov}(e^-,r)\)；称其为“状态与残差的交叉协方差”也有数学依据，但需要说明这个条件。

增益与后验协方差的推导

先求形如 \(\hat x^+=\hat x^-+Kr\) 的线性修正。上标 \(-\)、\(+\) 分别表示同一时刻使用当前观测之前和之后；更新改变的是估计，真实状态仍是同一个 \(x\)。

预测误差与后验误差分别定义为

\[ e^-=x-\hat x^-,\qquad e^+=x-\hat x^+. \]

把估计更新式代入后验误差定义：

\[ \begin{aligned} e^+ &=x-\hat x^+\\ &=x-(\hat x^-+Kr)\\ &=(x-\hat x^-)-Kr\\ &=e^--Kr. \end{aligned} \]

估计向 \(Kr\) 的方向修正后，在真实状态不变的情况下，误差就相应减去 \(Kr\)。再代入前面由测量模型推导的残差关系 \(r=He^-+\nu\)：

\[ \begin{aligned} e^+ &=e^--K(He^-+\nu)\\ &=e^--KHe^--K\nu\\ &=(I-KH)e^--K\nu. \end{aligned} \]

这里 \(I\) 是 \(n\times n\) 单位矩阵，\(e^-=Ie^-\)，所以前两项可以合并。修正后误差包含两部分：原预测误差经过 \((I-KH)\) 的作用，以及通过观测更新带入的测量噪声 \(-K\nu\)。因此，增大修正幅度也会改变测量噪声对后验误差的影响，需要通过下面的协方差计算选择 \(K\)。

现在把误差外积展开，先保留每一项：

$

再分别代入 \(P^-,C^T,C,S\)，得到

\[ P^+(K)=P^--KC^T-CK^T+KSK^T. \]

这与刚才的标量式 \(5.25-2a(5.25)+a^2(6.25)\) 具有相同结构。标量情形的一次交叉项写成 \(-2aC\)；矩阵乘法顺序不能交换，因此这里要保留为 \(-KC^T-CK^T\) 两项。

求 \(K\) 可以先对一个标量目标求导。矩阵 \(P^+(K)\) 的各对角线元素是各状态分量的误差方差，将它们相加得到迹，记为

\[ J(K)=\operatorname{tr}P^+(K). \]

这里 \(\operatorname{tr}(A)\) 表示矩阵 \(A\) 的对角线元素之和。由于当前条件下后验误差均值为零，\(J(K)=E[(e^+)^Te^+]\)，即按当前坐标计算的误差分量平方和的期望。

对刚才得到的协方差表达式取迹：

\[ \begin{aligned} J(K) &=\operatorname{tr}(P^-) -\operatorname{tr}(KC^T)-\operatorname{tr}(CK^T) +\operatorname{tr}(KSK^T)\\ &=\operatorname{tr}(P^-) -2\operatorname{tr}(KC^T) +\operatorname{tr}(KSK^T). \end{aligned} \]

两个交叉项的迹相同，因为 \(CK^T=(KC^T)^T\)，而矩阵与其转置的对角线之和相同。

现在对 \(K\) 的各元素求偏导，并将结果按原位置排成矩阵。第一项与 \(K\) 无关，导数为零；第二项的导数为 \(-2C\)；由于协方差矩阵 \(S\) 对称，最后一项的导数为 \(2KS\)，所以

\[ \frac{\partial J}{\partial K}=-2C+2KS. \]

其中二次项的求导也可以逐元素看清。它的迹为

\[ \operatorname{tr}(KSK^T) =\sum_i\sum_j\sum_\ell K_{ij}S_{j\ell}K_{i\ell}. \]

对某个元素 \(K_{ab}\) 求导时，两个 \(K\) 因子分别贡献一项：

\[ \begin{aligned} \frac{\partial\,\operatorname{tr}(KSK^T)}{\partial K_{ab}} &=\sum_\ell S_{b\ell}K_{a\ell} +\sum_jK_{aj}S_{jb}\\ &=2(KS)_{ab}. \end{aligned} \]

第二行使用了 \(S_{b\ell}=S_{\ell b}\)。同样，\(\operatorname{tr}(KC^T)=\sum_{i,j}K_{ij}C_{ij}\)，对 \(K_{ab}\) 求导得到 \(C_{ab}\)，于是得到上面的矩阵导数。

令导数为零，在 \(S\) 正定、因而可逆时，

\[ -2C+2K_*S=0 \quad\Longrightarrow\quad K_*S=C \quad\Longrightarrow\quad \boxed{K_*=CS^{-1}}. \]

求导先给出驻点，接下来再用配方验证它确实使误差协方差最小。利用 \(K_*S=C\)、\(SK_*^T=C^T\)，展开下面的矩阵平方：

$ (K-K_)S(K-K_)^T =KSK^T-KCT-CK^T+CS{-1}C^T. $

代回原式，就得到

\[ P^+(K)=P^--CS^{-1}C^T +(K-K_*)S(K-K_*)^T. \]

最后一项总是半正定。具体来说，对于任意方向向量 \(a\)，它在该方向上的值是

$ a^{T(K-K_)S(K-K_)}Ta. $

因此令 \(K=K_*\) 就能消去这一项，使任意方向上的误差方差都不大于其他线性增益的结果。这正是前面分别将两个标量平方项降至零的推广。代入 \(C=P^-H^T\) 和 \(S=HP^-H^T+R\)，得到

\[ \boxed{K=P^-H^T(HP^-H^T+R)^{-1}}, \]

\[ \boxed{\hat x^+=\hat x^-+K(z-H\hat x^-)}, \]

\[ \boxed{P^+=P^--P^-H^TS^{-1}HP^-=(I-KH)P^-}. \]

将 \(K=CS^{-1}\) 代入 \(P^+(K)\)，两个负的交叉项与一个正的二次项合并，留下 \(P^+=P^--CS^{-1}C^T\)；再用 \(C=P^-H^T\)，就得到上面的后验协方差公式。

\(K\) 为 \(n\times m\)，乘 \(m\times1\) 观测残差得到 \(n\times1\) 修正。只有一个观测时，\(S^{-1}\) 就是除以残差方差；有多个观测时，它同时考虑各残差的方差与相关性，不能逐元素相除。

下面的表将一维例子、位置—速度例子与一般矩阵式对应起来：

矩阵卡尔曼滤波的完整递推

将前面的推导汇总到同一轮计算中。从上一时刻的后验估计 \(\hat x_{k-1}\)、后验误差协方差 \(P_{k-1}\) 出发，得到：

\[ \begin{aligned} \text{预测：}\qquad \hat x_k^-&=F_k\hat x_{k-1}+B_ku_k,\\ P_k^-&=F_kP_{k-1}F_k^T+Q_k,\\[6pt] \text{观测残差：}\qquad r_k&=z_k-H_k\hat x_k^-,\\ S_k&=H_kP_k^-H_k^T+R_k,\\[6pt] \text{卡尔曼增益：}\qquad C_k&=P_k^-H_k^T,\\ K_k&=C_kS_k^{-1} =P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1},\\[6pt] \text{后验更新：}\qquad \hat x_k&=\hat x_k^-+K_kr_k,\\ P_k&=(I-K_kH_k)P_k^-. \end{aligned} \]

这里 \(\hat x_k^-,P_k^-\) 是使用当前观测之前的预测，\(\hat x_k,P_k\) 是使用当前观测之后的后验。预测阶段使用运动模型及已知控制；更新阶段使用当前观测 \(z_k\)。没有已知控制输入时，省去 \(B_ku_k\) 项。

一轮结束后，将 \(\hat x_k,P_k\) 作为下一轮的起点，再按同样顺序递推。接下来说明这些公式成立的条件，以及初始化、漏测和持续递推时需要处理的事项。

线性高斯假设决定最优性的范围

在线性高斯模型及正确噪声参数下，卡尔曼滤波给出精确的高斯后验；条件均值使所有基于已有观测的估计器中，条件期望平方误差最小。因为高斯的均值与众数相同，此时也与 MAP 一致。

如果只有线性模型、已知二阶矩和相应的不相关条件，而噪声并非高斯，上面的协方差配方仍给出最佳线性估计；后验本身却未必是高斯，卡尔曼结果通常不再是所有非线性估计器中的最优解。若模型偏置、误差相关或噪声参数不正确，所计算的 \(P\) 也未必反映真实误差。最优性是相对于给定模型与准则而言的，不保证每次读数修正后都离真实值更近。

初始化、噪声参数与漏测

前面的位置—速度例子用 \(\hat x_0=(0,1)^T\) 与 \(P_0=\operatorname{diag}(4,1)\) 初始化。它表达的是“位置中心先取 0、速度中心先取 1，同时位置标准差为 2、速度标准差为 1，初始误差暂不相关”。速度估计为 1，不等于已经知道真实速度就是 1。

因此，初值要同时包含 \(\hat x_0\) 与 \(P_0\)。如果位置来自独立的初始测量，可以把读数作为位置初值并用其误差方差初始化位置项；若速度未知，速度方差应表达这份未知，而不能仅因初始速度填了 0 就把速度方差也设为 0。初始误差相关时应填交叉项。若把首个读数用于初始化，就不要又作为独立信息更新一次。

\(R\) 描述传感器误差，而不是原始读数随机器人运动产生的方差。它可从固定参考状态下的标定残差估计。\(Q\) 描述运动模型误差，需与 2.3 的离散噪声模型一致，可根据加速度变化、标定轨迹残差等选择。位置—速度协方差的单位依次是 \(\mathrm m^2,\mathrm m^2/\mathrm s,\mathrm m^2/\mathrm s^2\)；单个“噪声大小”数字不能随意填满整个矩阵。

较大的 \(Q\) 会使预测更不确定，通常加大对新观测的响应；较大的 \(R\) 通常减小对该观测的响应。不能靠无限减小 \(Q,R\) 来制造真实精度，两者必须解释实际误差。对固定模型和预先给定的 \(Q,R\)，协方差与增益递推不依赖读数的具体数值；它们取决于模型、初始协方差和观测是否到达。

漏测时只传播，不制造观测

仍用前面一维的例子。第二步更新后，\(m_2\approx2.016923\)、\(P_2\approx1.784615\)。假如第三步没有读数，只能预测：

$ m_3^-=2.016923+1=3.016923, P_3^-=1.784615+1=2.784615. $

将这两个预测量作为第三步结果，不能再执行测量更新。下一步继续从它们出发，所以第四步观测前的均值约为 \(4.016923\)，方差约为 \(3.784615\)。连续缺少测量时，运动新增的不确定性仍会累计。

一般地，漏测时保留 \((\hat x_k^-,P_k^-)\)，随后继续传播。用预测值伪装成一次测量会让残差为零，却仍通过协方差更新虚假降低不确定性。

不规则采样应按实际间隔重算 \(F(T),Q(T)\)。若跨过多个间隔合并预测，转移矩阵是各步转移的乘积，过程噪声也要累计传播；不能只将原先一个间隔的 \(Q\) 加一次。

协方差的数值计算与稳态

前面已经得到后验协方差简式 \(P^+=(I-KH)P^-\)。它适合纸上推导；实际数值计算时，还可以从误差本身出发，得到更能保留协方差结构的形式。

位置—速度数值例中，第一步后验为 \(\begin{bmatrix}0.84&0.24\\0.24&1.64\end{bmatrix}\)。从误差 \(e^+=(I-KH)e^--K\nu\) 直接展开，还可写为

\[ \boxed{ P^+=(I-KH)P^-(I-KH)^T+KRK^T }. \]

这就是 Joseph 形式。例如第一步 \(I-KH=\begin{bmatrix}0.16&0\\-0.24&1\end{bmatrix}\)。两项分别为

\[ \begin{bmatrix}0.1344&0.0384\\0.0384&1.5824\end{bmatrix} +\begin{bmatrix}0.7056&0.2016\\0.2016&0.0576\end{bmatrix} =\begin{bmatrix}0.84&0.24\\0.24&1.64\end{bmatrix}. \]

对精确卡尔曼增益，它与简式等价。有限精度计算时，两个半正定项相加通常比相减更能保留对称性和非负方差；对另外指定的线性增益，Joseph 形式也仍能正确表达协方差，而 \((I-KH)P^-\) 的简化不再普遍成立。实现中一般通过解线性方程求增益，避免显式求逆。

方差收敛描述误差尺度，不描述状态停止

回到一维随机游走模型，后验方差递推为

\[ P_k=\frac{R(P_{k-1}+Q)}{P_{k-1}+Q+R}. \]

上一张一维表显示 \(P_k\) 从 \(4\) 降到 \(2.2222,1.7846,1.6417,1.5910\)，正在接近固定值。令稳态后验方差为 \(P_\infty\)，则

\[ P_\infty(P_\infty+Q+R)=R(P_\infty+Q), \]

\[ P_\infty^2+QP_\infty-QR=0,\qquad P_\infty=\frac{-Q+\sqrt{Q^2+4QR}}2. \]

对 \(Q=1,R=4\)，得到 \(P_\infty=(\sqrt{17}-1)/2\approx1.561553\)。每轮预测方差趋于 \(P_\infty+Q\approx2.561553\)，后验方差趋于 \(1.561553\)；二者形成稳定的预测—更新循环，并不是循环内部同一个方差完全不变。

相应稳态增益为

\[ K_\infty=\frac{P_\infty+Q}{P_\infty+Q+R}\approx0.390388. \]

即使这个增益稳定，均值仍会随机器人前进和新观测持续变化。稳定的是模型下误差的统计尺度，不是位置、速度或真实误差本身固定。

恒定噪声并不单独保证一般矩阵系统收敛

上面的模型是“上一位置加已知位移，再加过程噪声”，通常称为带已知输入的随机游走模型。其协方差递推与已知位移的具体数值无关，在 \(Q>0,R>0\) 时有稳定正解。若 \(Q=0\) 且状态静止，连续独立测量会使方差趋于零，增益也趋于零。对一般矩阵系统，是否存在有限稳态解还取决于动态与观测结构。例如 \(x_k=2x_{k-1}\)、\(H=0\)、\(Q=1\) 时，\(P_k=4P_{k-1}+1\)，即使噪声恒定也会发散，因为观测根本看不到状态。

这个反例说明，先要检查不确定性是否会被系统自身放大，以及观测能否持续约束这些方向。若一个越来越不稳定的状态方向始终不能被观测辨认，恒定的噪声参数也无法让滤波误差稳定。

在定常模型的理论中，常用的充分条件表述为：\((F,H)\) 可检测、\((F,Q^{1/2})\) 可稳定化，并且测量噪声协方差正定。“可检测”要求无法由观测辨认的模式自身稳定；“可稳定化”在这里要求过程噪声的作用覆盖需要处理的不稳定模式。在这些条件下，相应的协方差递推（Riccati 递推）具有稳定收敛性质。时间变化或长期漏测时，需要重新检查条件，不能只看 \(Q,R\) 是否恒定。

稳态卡尔曼增益连接 g-h 滤波

在位置—速度模型、单个位置观测、固定间隔 \(T\) 下，卡尔曼均值递推可以写成

\[ \hat p_k^-=\hat p_{k-1}+T\hat v_{k-1},\qquad r_k=z_k-\hat p_k^-, \]

\[ \hat p_k=\hat p_k^-+K_{p,k}r_k,\qquad \hat v_k=\hat v_{k-1}+K_{v,k}r_k. \]

这与 3.1 完全具有相同的均值更新结构。若模型定常、协方差和增益收敛，并采用稳态增益，就可以令

\[ g=K_{p,\infty},\qquad h=T K_{v,\infty}. \]

在同样的匀速模型、采样间隔和初始化均值下，这个特定 g-h 滤波器与采用固定稳态增益的卡尔曼滤波器产生相同的均值递推。启动阶段的时变卡尔曼增益、变化的 \(Q,R\)、不规则采样和漏测阶段通常不满足这种固定增益等价。

因此，g-h 可以看作以两个固定系数保存运动趋势；贝叶斯滤波传播并更新整组候选状态；在线性高斯条件下，卡尔曼滤波用均值与协方差完整表示这组分布，并从模型中的不确定性计算修正比例。

3.4 扩展卡尔曼滤波（EKF）

前面的卡尔曼滤波使用线性模型：状态通过矩阵 \(F,H\) 转换成下一时刻的状态和传感器读数。但实际中，这种转换可能是非线性的。例如，我们估计目标的水平位置与高度，雷达报告的却是目标到雷达的距离。

EKF 的思路是：在当前估计附近，用局部线性关系描述误差的变化，再沿用卡尔曼滤波的预测与更新。 随着估计位置改变，这个局部关系也要重新计算。

用雷达测距理解局部线性化

雷达位于原点，状态为水平位置与高度

\[ x=\begin{bmatrix}p\\a\end{bmatrix}. \]

这里 \(p\) 与 \(a\) 都是相对于雷达的位置坐标。雷达只给出一个斜距读数，状态却有两个分量，所以需要回答：距离比预测值更大时，应当怎样分别修正水平位置与高度？

先假设运动预测已经完成，得到 \(\hat x^-=(3,4)^T\) 米、\(P^-=I\ \mathrm m^2\)。这表示两个方向的预测标准差均为 1 米，误差暂不相关。

由勾股定理，雷达测量斜距

\[ z=\sqrt{p^2+a^2}+\nu,\qquad R=0.25\ \mathrm m^2, \]

并报告 \(z=5.5\) 米。预测中心到雷达的距离是 \(\sqrt{3^2+4^2}=5\) 米，因而读数比这个距离大 0.5 米。

在当前预测位置附近，分别改变两个位置分量，距离会有两种不同的变化率。把它们写成一行，就得到观测函数的雅可比：

\[ H(p,a)= \begin{bmatrix} \dfrac{p}{\sqrt{p^2+a^2}}& \dfrac{a}{\sqrt{p^2+a^2}} \end{bmatrix}. \]

雅可比的两个元素分别表示：在当前位置附近，水平位置或高度增加一小段时，距离大约增加多少。

在预测点 \((3,4)\) 处，

\[ \hat z^-=5,\qquad H=\begin{bmatrix}0.6&0.8\end{bmatrix},\qquad r=0.5\ \mathrm m. \]

若真实位置为 \((3+\Delta p,4+\Delta a)\)，则局部读数近似为

\[ h(3+\Delta p,4+\Delta a)\approx5+0.6\Delta p+0.8\Delta a. \]

于是 \(r\approx0.6\Delta p+0.8\Delta a+\nu\)。这说明一次读数首先约束的是两个位置误差的一个组合，并没有分别告诉我们 \(\Delta p\) 与 \(\Delta a\)。

将这个一行两列的 \(H\) 代入 \(S=HP^-H^T+R\)、\(C=P^-H^T\)，得到

\[ S=0.6^2+0.8^2+0.25=1.25\ \mathrm m^2,\qquad C=\begin{bmatrix}0.6\\0.8\end{bmatrix}\mathrm m^2, \]

\(S\) 只有一个数，因为本次只有一个观测；\(C\) 有两个分量，因为需要分别描述水平位置误差、高度误差与这个距离残差的关联。再由 \(K=C/S\) 计算两个修正系数：

\[ K=\begin{bmatrix}0.48\\0.64\end{bmatrix},\qquad \hat x=\begin{bmatrix}3.24\\4.32\end{bmatrix}\mathrm m, \]

\[ P=P^--CS^{-1}C^T =\begin{bmatrix}0.712&-0.384\\-0.384&0.488\end{bmatrix}\mathrm m^2. \]

具体的修正量为 \((0.48,0.64)^T\times0.5=(0.24,0.32)^T\) 米，方向与从雷达指向预测点的方向一致。后验中的负协方差也有几何含义：在同一个近似距离上，水平位置更大时，高度往往要更小，才能补偿对距离的影响。

这次观测主要减少了沿雷达视线方向的不确定性。一个距离对应一条圆周，不能单独确定二维位置，仍需结合先验、运动信息或其他传感器。

非线性运动与观测

刚才从已经给定的位置预测开始，只处理了非线性观测。现在给同一个雷达场景补上运动过程：目标按当前速度和朝向前进，雷达每秒测量一次距离。这样可以同时看到运动端和观测端的非线性分别怎样处理。

将状态选为

\[ x=\begin{bmatrix}p&a&v&\theta\end{bmatrix}^T, \]

其中 \(p,a\) 是水平位置与高度，\(v\) 是速度大小，\(\theta\) 是相对水平正方向的运动角度，以弧度计。水平速度是 \(v\cos\theta\)，竖直速度是 \(v\sin\theta\)。设采样间隔为 \(T\)，在相邻测量之间近似保持速度和朝向不变，运动函数为

\[ x_k=f(x_{k-1})+w_k,\qquad f(x)=\begin{bmatrix} p+Tv\cos\theta\\ a+Tv\sin\theta\\ v\\ \theta \end{bmatrix}. \]

即使我们假设这段时间内近似匀速，位置对状态中的速度、朝向也不是线性关系：函数中既有三角函数，又有变量相乘。观测函数则仍然是

\[ z_k=h(x_k)+\nu_k,\qquad h(x)=\sqrt{p^2+a^2}. \]

过程噪声与测量噪声取相互独立的零均值加性噪声，并与此前的状态独立。下面取 \(T=1\) 秒，初始估计为

\[ \hat x_0=\begin{bmatrix}2&4&1&0\end{bmatrix}^T, \qquad P_0=\operatorname{diag}(0.5,0.5,0.25,0.25). \]

目标大概在 \((2,4)\) 米处，速度为 1 米/秒，朝向角为 0，即向水平方向前进。四个对角线元素分别是位置、位置、速度、角度的方差，单位依次为 \(\mathrm m^2,\mathrm m^2,(\mathrm m/\mathrm s)^2,\mathrm{rad}^2\)。初始误差暂不相关。这些初值需要来自已有信息，单凭一条距离读数无法确定它们。

为便于演算，取 \(Q=\operatorname{diag}(0.25,0.25,0.04,0.04)\)，各项单位与相应状态方差一致；雷达噪声方差仍为 \(R=0.25\ \mathrm m^2\)。这里用独立过程扰动简化表示每秒新增的位置、速度与朝向不确定性。

状态与协方差预测

先直接把上一估计代入运动函数。 第一秒得到 \[ \hat x_1^-=f(\hat x_0) =\begin{bmatrix} 2+1\times1\times\cos0\\ 4+1\times1\times\sin0\\ 1\\0 \end{bmatrix} =\begin{bmatrix}3&4&1&0\end{bmatrix}^T. \]

预测中心直接用原函数计算。接下来才求运动雅可比，用它描述当前估计附近的小误差会怎样变化：

\[ F(x)=\frac{\partial f}{\partial x} =\begin{bmatrix} 1&0&T\cos\theta&-Tv\sin\theta\\ 0&1&T\sin\theta&Tv\cos\theta\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}. \]

例如，第一行的后两项分别表示：速度估计偏一点、朝向估计偏一点，会让下一时刻水平位置偏多少。在初始估计处，\(v=1,\theta=0,T=1\)，所以

\[ F_1=F(\hat x_0)=\begin{bmatrix} 1&0&1&0\\0&1&0&1\\0&0&1&0\\0&0&0&1 \end{bmatrix}, \]

\[ P_1^-=F_1P_0F_1^T+Q =\begin{bmatrix} 1&0&0.25&0\\ 0&1&0&0.25\\ 0.25&0&0.29&0\\ 0&0.25&0&0.29 \end{bmatrix}. \]

左上角的位置协方差恰好是前面使用的 \(I\)，所以先前的位置预测有了来源。运动还让误差产生了联系：速度估小，下一秒的水平位置也容易估小；朝向角估小，下一秒的高度也容易估小。这些联系记录在位置与速度、位置与朝向的非对角线元素中。

这里必须区分两个步骤：\(f\) 用于预测中心，\(F_1\) 用于传播局部误差。不能一般地把 \(f(\hat x_0)\) 替换成 \(F_1\hat x_0\)；只是本轮选取的数值恰好让两者相同。下一轮就会看到它们的区别。

距离观测更新

雷达读到 5.5 米时，先把预测状态代入原始观测函数，再在同一个预测点求观测雅可比：

\[ \hat z_1^-=h(\hat x_1^-)=5\ \mathrm m,\qquad H_1=\left.\frac{\partial h}{\partial x}\right|_{\hat x_1^-} =\begin{bmatrix}0.6&0.8&0&0\end{bmatrix}. \]

后两个元素是零，因为位置不变时，只改变速度或朝向不会立即改变当前距离。但这不意味着它们不能被修正：运动预测已经把它们的误差与位置误差联系起来。

残差为 \(r_1=5.5-5=0.5\) 米，按卡尔曼更新计算：

\[ S_1=H_1P_1^-H_1^T+R=1.25, \qquad K_1=P_1^-H_1^T/S_1 =\begin{bmatrix}0.48&0.64&0.12&0.16\end{bmatrix}^T. \]

\[ \hat x_1=\hat x_1^-+K_1r_1 =\begin{bmatrix}3.24&4.32&1.06&0.08\end{bmatrix}^T. \]

位置更新与前面完全一致，同时速度调整为 1.06 米/秒，朝向调整为 0.08 弧度，约为 \(4.58^\circ\)。距离比预测大，滤波器结合已有误差关系，认为目标可能既比预计更远，也可能速度稍大、运动方向稍向上。这是已有估计与读数共同产生的修正，并非雷达直接测出了速度和朝向。

更新后的协方差为

\[ P_1=P_1^--K_1S_1K_1^T =\begin{bmatrix} 0.712&-0.384&0.178&-0.096\\ -0.384&0.488&-0.096&0.122\\ 0.178&-0.096&0.272&-0.024\\ -0.096&0.122&-0.024&0.258 \end{bmatrix}. \]

下一轮要保留整个 \(P_1\)，包括各误差之间的相关性。

连续递推

第二秒仍直接用原函数预测状态：

\[ \hat x_2^-=f(\hat x_1) =\begin{bmatrix} 3.24+1.06\cos0.08\\ 4.32+1.06\sin0.08\\ 1.06\\0.08 \end{bmatrix} \approx\begin{bmatrix}4.2966&4.4047&1.06&0.08\end{bmatrix}^T. \]

在上一后验中心计算的运动雅可比则是

\[ F_2\approx\begin{bmatrix} 1&0&0.9968&-0.0847\\ 0&1&0.0799&1.0566\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix}. \]

若错误地把预测写成 \(F_2\hat x_1\)，前两个分量约为 \((4.2898,4.4892)\)，与正确的 \((4.2966,4.4047)\) 不同。原因是雅可比只描述偏离中心的小变化：局部展开是 \(f(\hat x_1+e)\approx f(\hat x_1)+F_2e\)，并不是 \(f(x)\approx F_2x\)。

用 \(P_2^-=F_2P_1F_2^T+Q\) 传播协方差后，再把当前预测代入距离函数：

\[ \hat z_2^-=h(\hat x_2^-)\approx6.1532\ \mathrm m,\qquad H_2\approx\begin{bmatrix}0.6983&0.7158&0&0\end{bmatrix}. \]

所以 \(F_2\) 在上一后验处计算，\(H_2\) 在当前预测处计算。两者分别描述运动和观测的局部变化，不能一直沿用上一轮的值。

若第二次读数为 6.2 米，残差约为 0.0468 米，算得

\[ S_2\approx1.0799,\qquad K_2\approx\begin{bmatrix}0.6404&0.4489&0.2257&0.1686\end{bmatrix}^T, \]

\[ \hat x_2\approx\begin{bmatrix}4.3266&4.4257&1.0706&0.0879\end{bmatrix}^T. \]

继续收到第三次读数 7.4 米，重复相同步骤。计算使用未舍入的数值，三轮结果为：

这个例子同时使用了非线性的运动函数和观测函数，但 EKF 不要求两端都非线性：如果运动是线性的，就用线性运动模型及其转移矩阵；如果观测是线性的，例如直接测量水平位置 \(z=p+\nu\)，观测函数就是 \(h(x)=p\)，观测矩阵为固定的 \(H=(1,0,0,0)\)。哪一端非线性，就在哪一端重新求局部雅可比，其余计算方式相同。

此外，单个雷达只提供距离，仍不能唯一确定二维轨迹的方向。连续更新的结果依赖已有初值和运动假设；增加角度观测或其他位置来源，才能进一步区分产生相同距离序列的不同运动。

一般的 EKF 递推

运动和观测都可能是非线性的，统一写成

\[ x_k=f_k(x_{k-1},u_k)+w_k,\qquad z_k=h_k(x_k)+\nu_k. \]

这里采用相互独立的零均值加性噪声，协方差为 \(Q_k,R_k\)，并假设它们与此前的状态独立。EKF 仍然每轮保存状态估计 \(\hat x\) 和误差协方差 \(P\)。

第一步：预测。 用运动函数预测状态，在上一估计处计算运动雅可比 \(F_k\)，用它传播误差：

\[ \begin{aligned} \hat x_k^-&=f_k(\hat x_{k-1},u_k),\\ F_k&=\left.\frac{\partial f_k}{\partial x}\right|_{\hat x_{k-1},u_k},\\ P_k^-&=F_kP_{k-1}F_k^T+Q_k. \end{aligned} \]

第二步：更新。 用观测函数预测读数，在当前预测处计算观测雅可比 \(H_k\)，再按卡尔曼公式修正：

\[ \begin{aligned} \hat z_k^-&=h_k(\hat x_k^-),\\ H_k&=\left.\frac{\partial h_k}{\partial x}\right|_{\hat x_k^-},\\ r_k&=z_k-\hat z_k^-,\\ S_k&=H_kP_k^-H_k^T+R_k,\\ K_k&=P_k^-H_k^TS_k^{-1},\\ \hat x_k&=\hat x_k^-+K_kr_k,\\ P_k&=(I-K_kH_k)P_k^-. \end{aligned} \]

最后一行是便于理解的简式，实际计算可沿用 3.3 的 Joseph 形式，以减小浮点误差的影响。

注意两种计算的分工：状态和读数的中心值用原函数 \(f,h\) 计算；误差的传播用雅可比 \(F,H\) 计算。 例如雷达预测读数仍用勾股公式，不能直接写成 \(H\hat x^-\)。每轮先在上一后验处计算 \(F_k\)，再在当前预测处计算 \(H_k\)；漏测时只做预测。

什么时候近似会不可靠

EKF 用当前估计附近的切线或切平面代替原函数，因此在估计较准、不确定性较小、函数在附近变化平缓时比较合适。

仍以雷达为例：目标离原点较远、位置范围很小时，局部近似通常较好；位置范围很大或靠近原点时，同一条切线难以代表整个可能区域。在原点，距离函数的雅可比甚至没有定义。初值偏差较大时，也可能在错误的位置线性化，导致修正不准确。

所以 EKF 给出的是非线性问题的近似估计。下一节的 UKF 将换一种做法：让几个代表点直接通过非线性函数，再汇总它们的变化。

3.5 无迹卡尔曼滤波

上一节的 EKF 已经能处理非线性函数，但它只用当前估计附近的局部斜率传播误差。如果可能的位置分布得比较宽，一条切线就可能描述不全这片范围内的变化。

UKF 换了一个思路：在估计中心周围选几个有代表性的点，让它们分别通过原函数，再把结果汇总成新的均值和协方差。 不需要先求函数的雅可比。

这几个点称为 sigma 点，这种“取点、变换、汇总”的方法称为无迹变换（UT）；用它完成滤波中的预测与更新，就是无迹卡尔曼滤波（UKF）。

可以先把两种方法的区别记成：EKF 计算中心附近的变化率；UKF 直接计算中心附近几个位置的函数值。两者都保存均值与协方差，也都用观测残差修正状态。

基本思路

先沿用雷达例子的位置部分。运动预测已经给出

\[ \hat x^-=\begin{bmatrix}3\\4\end{bmatrix}\mathrm m, \qquad P^-=I\ \mathrm m^2. \]

预测中心是 \((3,4)\)，两个方向的标准差都是 1 米，误差暂不相关。雷达距离函数为

\[ h(p,a)=\sqrt{p^2+a^2}. \]

把中心点代入，得到 5 米。但目标并不一定恰好在中心，它还可能位于附近。如果把附近的位置都考虑进去，这些位置对应距离的平均值，不一定等于中心位置对应的距离。

例如，只看水平位置为 \(3-1\) 和 \(3+1\)、高度都为 4 的两个位置，它们到雷达的距离分别约为 4.472 米和 5.657 米，平均约为 5.064 米，而非 5 米。两个位置虽然围绕中心对称，通过距离函数后，两个距离却不再围绕 5 对称。

这个小计算只是说明曲率的影响。UKF 接下来要做得更系统：点的位置和权重必须配合，让它们在变换前就能代表原来的均值和协方差。

Sigma 点的选取

二维状态常用一个中心点，加上两对侧点，共五个点。对本例，先采用下面这套具体选择，再解释一般公式：

表中的 \(\chi^{(i)}\) 表示第 \(i\) 个代表点，上标是编号，不是幂。每个点都是一组完整的位置坐标。

为什么侧点偏移 \(\sqrt2\) 米，而不是 1 米？ 因为计算均值时，四个侧点各占 \(1/4\) 的权重。水平方向只有第 1、3 两个点偏离中心，要恢复原来为 1 的水平位置方差，就需要

\[ \frac14(\sqrt2)^2+\frac14(-\sqrt2)^2=1. \]

竖直方向同理。正负偏移互相抵消，所以加权中心仍是 \((3,4)\)；每个侧点只沿一个坐标方向偏移，因此输入交叉协方差为零，也与 \(P^-=I\) 一致。

为什么有两组权重？ 均值权重负责算中心，协方差权重负责算围绕中心的分散程度。本节采用的常见构造，会在协方差计算时额外增加中心点的权重，用来改善非线性变换后的不确定性近似。具体参数公式放在本节后面；现在先用表中这组数完成一次更新。

中心点的均值权重为零，仍然需要保留：经过非线性函数后，中心点的输出不一定等于所有输出的加权中心，它会参与协方差计算。

这些权重是计算规则，不是“目标恰好位于这些点的概率”。尤其协方差权重不要求加起来等于 1，也不能自行把它们重新归一化。sigma 点只是在少量函数计算中保留原分布的重要信息，并没有把连续状态变成五种可能。

观测预测

现在对每个位置点直接使用勾股公式，得到五个预测距离：

先算预测距离的中心。 用均值权重加权：

\[ \hat z^-=\sum_{i=0}^4W_i^{(m)}\mathcal Z^{(i)} =\frac{5.956952+6.189807+4.302873+3.960592}{4} \approx5.102556\ \mathrm m. \]

UKF 得到的预测距离约为 5.103 米，比中心点对应的 5 米大一些。这是附近位置经过距离函数后汇总的结果；这里算的是近似均值，并不是精确积分。

再算预测距离有多不确定。 每个距离减去刚才算出的中心，平方后按协方差权重汇总，最后加上雷达自身的噪声方差 \(R=0.25\ \mathrm m^2\)：

\[ S=\sum_{i=0}^4W_i^{(c)}(\mathcal Z^{(i)}-\hat z^-)^2+R. \]

本例具体为

\[ S=2(5-5.102556)^2 +\frac14\sum_{i=1}^{4}(\mathcal Z^{(i)}-5.102556)^2+0.25 \approx1.234956\ \mathrm m^2. \]

它与前面的卡尔曼滤波中的 \(S\) 含义相同：即将使用的观测残差有多不确定。计算中使用未舍入的点值，上式的小数只是展示结果。

观测更新

现在知道预测距离的中心和方差，还差一个问题：如果实际距离比预计大，水平位置和高度分别该修正多少？

这需要知道位置误差与距离误差的关联，即交叉协方差。计算方法仍然是偏差相乘，只不过这次相乘的是“位置偏差”和“由这个位置产生的距离偏差”：

\[ C=\sum_{i=0}^4W_i^{(c)} (\chi^{(i)}-\hat x^-)(\mathcal Z^{(i)}-\hat z^-). \]

位置有两个分量，距离只有一个分量，所以 \(C\) 是两行一列：第一行表示水平位置与距离的关联，第二行表示高度与距离的关联。配对时必须使用同一编号的输入点与输出点。

以水平分量为例，只有第 1、3 个点存在水平偏移，因此

\[ \begin{aligned} C_p&=\frac14\sqrt2(\mathcal Z^{(1)}-\hat z^-) +\frac14(-\sqrt2)(\mathcal Z^{(3)}-\hat z^-)\\ &=\frac{\sqrt2}{4}(\mathcal Z^{(1)}-\mathcal Z^{(3)}) \approx0.584805\ \mathrm m^2. \end{aligned} \]

高度方向同理，得到

\[ C_a=\frac{\sqrt2}{4}(\mathcal Z^{(2)}-\mathcal Z^{(4)}) \approx0.788146\ \mathrm m^2. \]

两个分量均为正，表示在这组点所代表的局部范围内，水平位置偏大或高度偏大，距离通常也偏大。独立的测量噪声不会增加位置与距离的关联，所以只在 \(S\) 中加 \(R\)，不在 \(C\) 中加。

准备好 \(\hat z^-,S,C\) 后，就可以沿用卡尔曼式更新：

\[ K=CS^{-1} \approx\begin{bmatrix}0.473543\\0.638198\end{bmatrix}. \]

雷达实际读到 5.5 米，残差为

\[ r=5.5-5.102556=0.397444\ \mathrm m. \]

因此

\[ \hat x=\hat x^-+Kr \approx\begin{bmatrix}3\\4\end{bmatrix} +\begin{bmatrix}0.473543\\0.638198\end{bmatrix}\times0.397444 =\begin{bmatrix}3.188207\\4.253648\end{bmatrix}\mathrm m, \]

\[ P=P^--KSK^T \approx\begin{bmatrix} 0.723069&-0.373222\\-0.373222&0.497007 \end{bmatrix}\mathrm m^2. \]

这样就完成了一次 UKF 观测更新：取位置点、算各点距离、汇总中心与不确定性、算关联，最后用残差修正位置。

与上一节对同一份位置预测的 EKF 更新对照：

UKF 已经把附近位置的距离变化纳入计算，因此本次读数相对于预测中心的差更小。两者的增益也不同。表格展示了计算方法产生的差异，并不能仅凭结果大小判定哪一种估计更准确。

多维 Sigma 点

刚才为了看清观测更新，只使用二维位置。如果沿用上一节完整的状态 \(x=(p,a,v,\theta)^T\)，就要让每个代表点都包含位置、速度和朝向，不能只取位置点。

常见构造对 \(n\) 维状态使用 \(2n+1\) 个点：一个中心点，加上 \(n\) 对正负侧点。因此一维用三个点，二维用五个点，四维用九个点。

取点需要同时体现各分量的方差和它们之间的相关性。若状态误差相关，侧点就可能同时改变多个分量，而不是只沿某个坐标轴移动。

具体做法是先选一个正的展开尺度 \(c\)，再求矩阵 \(L\)，使

\[ LL^T=cP. \]

这里是矩阵分解，不是对每个元素开平方。例如 \(P\) 正定时可用下三角 Cholesky 因子。记 \(L\) 的第 \(i\) 列为 \(L_{:i}\)，取点公式为

\[ \begin{aligned} \chi^{(0)}&=\bar x,\\ \chi^{(i)}&=\bar x+L_{:i},\qquad i=1,\ldots,n,\\ \chi^{(i+n)}&=\bar x-L_{:i},\qquad i=1,\ldots,n. \end{aligned} \]

\(\bar x\) 表示本次取点所用的状态中心。每一列 \(L_{:i}\) 都是一个完整的偏移向量，决定一对侧点怎样从中心展开。

例如刚才 \(P=I,c=2\)，所以 \(L=\sqrt2I\)，两列分别是 \((\sqrt2,0)^T\) 与 \((0,\sqrt2)^T\)，恰好给出那四个侧点。

如果

\[ P=\begin{bmatrix}1&0.5\\0.5&1\end{bmatrix},\qquad c=2, \]

可以取

\[ L=\begin{bmatrix}\sqrt2&0\\1/\sqrt2&\sqrt{3/2}\end{bmatrix}. \]

这时第一对侧点的偏移为 \(\pm(\sqrt2,1/\sqrt2)^T\)，两个位置分量一起变化，体现了正相关性。只用各分量的标准差分别取轴向点，就会丢掉这种联系。

递推流程

先把一轮 UKF 的计算顺序列出来，下面再逐步展开公式。设状态有 \(n\) 个分量：

1. 生成 Sigma 点：根据上一时刻的状态均值 \(\hat x_{k-1}\) 和协方差 \(P_{k-1}\)，按规则生成 \(2n+1\) 个代表点。
2. 状态预测：把这些点分别代入运动函数 \(f\)，加权计算预测状态均值 \(\hat x_k^-\) 和协方差 \(P_k^-\)，并在协方差中加入过程噪声 \(Q_k\)。
3. 观测预测：根据已经包含过程噪声的 \(\hat x_k^-,P_k^-\) 重新生成 Sigma 点，再分别代入观测函数 \(h\)，加权得到预测观测均值 \(\hat z_k^-\)。
4. 计算观测协方差：将各预测观测点相对于 \(\hat z_k^-\) 的偏差加权汇总，得到观测协方差，并加入观测噪声 \(R_k\)，记为 \(S_k\)。
5. 计算交叉协方差：将同一编号的预测状态偏差与预测观测偏差配对、加权，得到状态与观测的交叉协方差 \(P_{xz,k}\)，本文记为 \(C_k\)。这里使用第 3 步重新生成的状态点。
6. 更新：计算卡尔曼增益 \(K_k=C_kS_k^{-1}\)，用实际读数与预测读数的残差更新状态均值和协方差，作为下一轮的起点。

有了取点方法，就可以同时处理非线性运动与非线性观测：

\[ x_k=f_k(x_{k-1},u_k)+w_k,\qquad z_k=h_k(x_k)+\nu_k. \]

这里采用独立的零均值加性噪声，协方差为 \(Q_k,R_k\)，并与此前的状态独立。每轮从上一后验 \(\hat x_{k-1},P_{k-1}\) 出发。

第一步：让点运动，再汇总。 从上一后验取点 \(\chi_{k-1}^{(i)}\)，把每个点代入原运动函数：

\[ \mathcal X_k^{(i)}=f_k(\chi_{k-1}^{(i)},u_k). \]

如果用上一节的速度、朝向模型，每个点都分别计算 \(p+Tv\cos\theta\)、\(a+Tv\sin\theta\)，不需要求运动雅可比。随后加权汇总：

\[ \hat x_k^-=\sum_iW_i^{(m)}\mathcal X_k^{(i)}, \]

\[ P_k^-=\sum_iW_i^{(c)} (\mathcal X_k^{(i)}-\hat x_k^-) (\mathcal X_k^{(i)}-\hat x_k^-)^T+Q_k. \]

均值公式算运动后的中心，协方差公式算运动后的分散程度，\(Q_k\) 再补入本次运动新增的不确定性。与 EKF 不同，UKF 的预测中心由多个运动结果汇总，一般不等于只计算中心的 \(f_k(\hat x_{k-1},u_k)\)。

第二步：重新取点，预测读数。 从刚得到的 \(\hat x_k^-,P_k^-\) 重新取点 \(\chi_k^{-,(i)}\)，再代入观测函数：

\[ \mathcal Z_k^{(i)}=h_k(\chi_k^{-,(i)}),\qquad \hat z_k^-=\sum_iW_i^{(m)}\mathcal Z_k^{(i)}. \]

这里重新取点，是为了让送入观测函数的点也包含刚加入的过程噪声 \(Q_k\)。例如运动后已有方差 0.75，再加 \(Q=0.25\)，当前预测方差应为 1；继续沿用加噪声前的旧点，就无法体现完整的不确定性。本节始终采用这种“预测后重新取点”的实现方式。

然后计算观测的不确定性与状态、观测的关联：

\[ S_k=\sum_iW_i^{(c)} (\mathcal Z_k^{(i)}-\hat z_k^-) (\mathcal Z_k^{(i)}-\hat z_k^-)^T+R_k, \]

\[ C_k=\sum_iW_i^{(c)} (\chi_k^{-,(i)}-\hat x_k^-) (\mathcal Z_k^{(i)}-\hat z_k^-)^T. \]

这就是刚才雷达算例中 \(S,C\) 的向量形式。观测只有一个距离时，\(S\) 是一个数；如果观测有 \(m\) 个分量，\(S\) 就是 \(m\times m\) 矩阵。状态有 \(n\) 个分量时，\(C\) 是 \(n\times m\) 矩阵。

第三步：用实际读数更新。

\[ \boxed{\begin{aligned} r_k&=z_k-\hat z_k^-,\\ K_k&=C_kS_k^{-1},\\ \hat x_k&=\hat x_k^-+K_kr_k,\\ P_k&=P_k^--K_kS_kK_k^T. \end{aligned}} \]

保存这次更新后的均值与协方差，下一轮从它们重新取点。如果漏测，只执行运动预测。

对完整的雷达跟踪状态而言，\(C_k\) 的四行分别对应位置、位置、速度和朝向与距离的关联。即使雷达没有直接测量速度或朝向，点中保留的误差联系仍能让距离残差修正这些分量，与上一节解释的道理相同。

参数与权重

前面的计算已经可以说明 UKF 怎样运行。这里再补齐常见缩放 sigma 点方案的参数公式。

它用 \(\alpha,\beta,\kappa\) 决定取点尺度和权重，先定义

\[ \lambda=\alpha^2(n+\kappa)-n,\qquad c=n+\lambda=\alpha^2(n+\kappa)>0. \]

其中 \(c\) 就是前面 \(LL^T=cP\) 中的尺度。权重为

\[ W_0^{(m)}=\frac\lambda c,\qquad W_0^{(c)}=\frac\lambda c+1-\alpha^2+\beta, \]

\[ W_i^{(m)}=W_i^{(c)}=\frac1{2c},\qquad i=1,\ldots,2n. \]

可以分开理解这三个参数的作用：\(\alpha,\kappa\) 共同影响点离中心的距离，并相应改变权重；\(\beta\) 调整中心点在协方差计算中的权重，对高斯输入常取 2。

本节为方便手算，采用 \(\alpha=1,\beta=2,\kappa=0\)。二维时 \(c=2,\lambda=0\)，因此中心均值权重为 0、中心协方差权重为 2，四个侧点权重各为 \(1/4\)，正好得到开头的表格。这是本例的演算选择，具体应用仍需结合模型和数值表现选参数。

点的尺度与权重为什么要一起改变？每对正负侧点对输入协方差的贡献为

\[ \frac1{2c}L_{:i}L_{:i}^T +\frac1{2c}(-L_{:i})(-L_{:i})^T =\frac1cL_{:i}L_{:i}^T. \]

把 \(n\) 对点相加，就得到 \(LL^T/c=P\)。均值方面，正负偏移相消，权重之和又满足 \(W_0^{(m)}+2n/(2c)=1\)，所以能恢复输入中心。中心点的输入偏差为零，因此额外的协方差权重不会改变输入协方差，但经过非线性变换后，它可能发挥作用。

这些条件保证点能匹配输入均值与协方差，并不保证变换后所有统计量都精确，也不能仅凭这些条件唯一确定中心协方差的修正项。

适用范围与注意事项

UKF 通常能比单点线性化更充分地考虑函数在附近范围内的变化，但它仍用一组均值与协方差表示状态，也仍通过 \(Kr\) 修正估计。因此它是近似方法，并不保证每次都比 EKF 更准确，也不能完整表示多个相距较远的可能状态。

本节的公式针对噪声直接相加的模型。如果噪声进入乘法或其他非线性运算，需要让取点过程同时考虑状态与噪声，不能机械地在最后加 \(Q,R\)。

如果完整状态中包含朝向角，均值与残差要考虑角度的周期性。例如 \(179^\circ\) 与 \(-179^\circ\) 实际很接近，不能直接把平均方向算成 \(0^\circ\)。上面的雷达数值算例只涉及位置；推广到朝向跨越边界的情形时，需要使用适合角度的均值和差值计算。

实际计算增益时，可通过求解 \(K_kS_k=C_k\) 避免显式求逆，并检查协方差的对称性与正定性。sigma 点参数与噪声参数作用不同：前者决定怎样取点和汇总，\(Q,R\) 则描述运动与传感器误差，不能互相替代。