卡尔曼滤波（Kalman Filter）

预测的必要性

我们先从问题的表述开始，以理解为何需要一种用于状态估计与预测的算法。

为了说明这一点，考虑一个跟踪雷达的示例：

假设我们有一部用于跟踪飞机的雷达。在该场景中，飞机就是系统，需要估计的量是它的位置，即系统状态。

雷达通过将一束窄波束指向目标来采样，并获得飞机的位置测量。基于这些测量，我们可以估计系统状态（即飞机的位置）。

为了持续跟踪，雷达需要以固定的时间间隔重新指向目标。这意味着雷达必须能够为下一次指向预测飞机的未来位置；若无法做到，波束可能会指向错误方向而丢失跟踪。要进行这种预测，我们需要了解飞机的运动方式——也就是描述系统随时间行为的动态模型。

为了简化示例，假设一个一维世界：飞机沿一条直线向雷达靠近或远离。

系统状态定义为飞机相对于雷达的距离，记为 \( r \)。雷达向飞机发射脉冲，脉冲在目标上反射后返回雷达。通过测量脉冲发射与接收之间的时间间隔，并利用电磁波以光速传播的事实，雷达可以轻松计算飞机的距离 \( r \)。除了距离，雷达还可以测量飞机的速度 \( v \)，类似警用雷达枪利用多普勒效应测得汽车速度。

假设在时间 \( t_{0} \) ，雷达以很高的精度与准确度测得飞机的距离与速度：距离为 10{,}000 米、速度为 200 米/秒。这就得到系统状态：

\[ r_{t_{0}} = 10,000m \]

下一步是在时间 \( t_{1}=t_{0}+\Delta t \) 预测系统状态，其中 \( \Delta t \) 为目标重访时间。鉴于飞机预计保持恒定速度，我们可采用恒速动态模型来预测其未来位置。

在时间间隔 \( \Delta t \) 内的位移为：

\[ \Delta r = v \cdot \Delta t \]

若采样间隔为 5 秒，则在 \( t_{1} \) 的预测位置为：

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

这是一个建立在简单原理上的初级算法：当前系统状态从测量获得，并用动态模型预测未来状态。

在现实世界中情况更为复杂。首先，雷达测量并非完全精确，它受到噪声影响而带有随机性。如果十部不同雷达在同一时刻测量飞机距离，将得到十个略有差异的结果，这些结果可能接近但不完全相同。测量差异源于测量噪声。

这引出一个新问题：我们的估计有多可靠？我们需要一种算法，不仅能给出估计值，还能告诉我们该估计的可信程度。

另一个问题是动态模型的准确性。尽管我们假设飞机以恒速运动，但诸如风等外部因素会使其偏离该假设。这些不可预测的影响称为过程噪声。

正如我们希望评估基于测量的估计的确定性，我们也希望了解对预测的可信程度。

卡尔曼滤波是一种状态估计算法，可同时给出当前状态的估计与未来状态的预测，并为二者提供不确定度度量。此外，它还是一种最优算法，能最小化状态估计的不确定度。这也是卡尔曼滤波被广泛使用与信赖的原因。

卡尔曼滤波示例

让我们从一个简单示例开始：一维雷达通过向飞机发射脉冲并接收回波来测量距离与速度。脉冲发射与回波接收之间的时间延迟提供飞机距离 \(r\) 的信息，而回波频率的变化（多普勒效应）提供飞机速度 \(v\) 的信息。

在本示例中，系统状态由飞机的距离 \(r\) 与速度 \(v\) 描述。我们用向量 \(\boldsymbol{x}\) 定义系统状态，其中包含上述两项：

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

我们约定用小写粗体表示向量、用大写粗体表示矩阵。

由于系统状态包含多个变量，我们使用线性代数工具（如向量与矩阵）来描述卡尔曼滤波的数学。如果你对线性代数不够熟悉，请参阅在线教程中的一维卡尔曼滤波章节或书籍。该部分以高中数学介绍卡尔曼滤波方程及其推导，并给出四个完整的例题。

迭代 0

滤波器初始化

在本示例中，我们用第一次测量来初始化卡尔曼滤波（关于初始化技术及其对卡尔曼滤波性能的影响，参见书籍第21章）。在时间 \(t_0\) ，雷达测得距离 \(10{,}000m\) 与速度 \(200m/s\)。测量记为 \(\boldsymbol{z}\)。
我们将测量堆叠为测量向量 \(\boldsymbol{z}\)：

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

下标 \(0\) 表示时间 \(t_0\)。

测量并不等同于真实状态。测量受随机噪声污染，因此每次测量都是一个随机变量。

我们能信任该测量吗？它有多可靠？每个测量都伴随一个平方的测量不确定度（有时称为测量误差），其数值即测量的方差。你可以在基础知识 I中了解更多关于方差的内容；关于测量不确定度的更详细讨论，参见一维卡尔曼滤波部分。

在雷达系统中，测量不确定度主要由接收信号强度与噪声的比值决定。信噪比越高，测量方差越低，我们对测量的信心也越高。

下图比较了在噪声存在时，低信号与高信号两种情况。

假设距离测量的标准差为 \(4\,\mathrm{m}\)，速度测量的标准差为 \(0.5\,\mathrm{m/s}\)。由于方差是标准差的平方，测量不确定度的平方（记为 \(\boldsymbol{R}\)）为：

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\( \boldsymbol{R} \) 是协方差矩阵（Covariance Matrix）。主对角线元素为各测量的方差，非主对角线元素为测量之间的协方差。

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

在本例中，我们假设距离与速度测量的误差彼此不相关，因此测量协方差矩阵的非主对角元素设为零。

如需回顾方差与标准差，请参见在线教程的基础知识 I部分； 关于协方差矩阵，请参见基础知识 II部分。

在初始化阶段，我们仅有一次测量。在本例中，测量与系统状态由相同的量（\(r\) 与 \(v\)）描述。因此，我们可以将该测量作为系统状态的初始估计。此方法仅适用于初始化阶段：

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

下标 \(0,0\) 的含义如下：

• 第一个索引表示系统时刻，本例为 \(t_0\)。
• 第二个索引表示进行估计的时刻，同样为 \(t_0\)。

换言之，该估计针对时刻 \(t_0\)，且计算也发生在 \(t_0\)。

预测

现在我们来预测下一状态。假设目标重访时间为 5 秒（\(\Delta t=5s\)），因此 \(t_1=5s\)。

为了估计未来的系统状态，我们必须描述系统随时间的演化方式。本例假设恒速动态模型（运动模型）：

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

（关于加速度动态模型的示例，参见书籍第 9 章。）

我们用矩阵形式描述该动态模型：

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

下标 \(1,0\) 的含义如下：

• 第一个索引表示系统时刻，为 \(t_1\)。
• 第二个索引表示进行估计的时刻，为 \(t_0\)。

因此，\( \hat{\boldsymbol{x}}_{1,0} \) 是我们基于 \(t_0\) 时刻的信息，对 \(t_1\) 时刻系统状态的估计；换言之，它是对未来状态的预测。

矩阵 \( \boldsymbol{F} \) 称为状态转移矩阵（State Transition Matrix），用于描述系统状态随时间的演化：

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

关于任意线性系统动力学的建模方法，详见书籍的附录 C。

该方程为：

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} \]

是状态外插（预测）方程（State Extrapolation Equation）。它告诉我们如何由当前状态计算下一时刻的状态：用当前状态估计结合系统的运动模型来预测下一时刻的状态。

状态外插方程的完整形式为：

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]

其中：

• \(\boldsymbol{u}_{n}\) 为输入变量
• \(\boldsymbol{G}\) 为输入转移矩阵

输入向量表示提供给卡尔曼滤波的额外已知信息，例如机载加速度计的读数。

在本简单示例中，我们假定无输入，因此 \(\boldsymbol{u}_n=0\)。

关于包含输入项的示例，请参见在线教程的状态外插方程页面或书籍中的完整示例 10。

在卡尔曼滤波中，每个测量与每个估计都带有不确定度信息。预测下一状态后，我们也应问：这个预测有多精确？

当前状态估计的平方不确定度由协方差矩阵表示：

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

然而，预测协方差并非按如下方式计算：

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

这是因为 \(\boldsymbol{P}\) 是协方差矩阵，而方差与协方差涉及平方项。

协方差外插方程（忽略过程噪声）为：

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

其完整推导见在线教程的协方差外插方程部分。

在本例中：

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

观察协方差矩阵的主对角线。

速度方差 \(\sigma_v^2\) 仍为 \(0.25\,\mathrm{m^2/s^2}\)。由于动态模型假设速度恒定，所以它没有变化。

相比之下，距离方差 \(\sigma_r^2\) 从 \(16\,\mathrm{m^2}\) 增至 \(22.25\,\mathrm{m^2}\)。这反映出速度的不确定性会随着时间推移导致距离不确定性增加。

如前所述，恒速动态的假设并不完全准确。现实中，飞机速度会受诸如风等外部且未知因素影响。因此，实际预测不确定性高于简单模型所给出的值。

这些不可预测的影响称为 过程噪声（Process Noise），记为 \(\boldsymbol{Q}\)。为将其纳入考虑，我们在协方差外插方程中加入 \(\boldsymbol{Q}\)：

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

若想直观理解过程噪声如何影响卡尔曼滤波性能，请参见在线教程中的 示例 6。

假设随机加速度的标准差为 \(\sigma_a=0.2\,\mathrm{m/s^2}\)。这代表由不可预测的环境影响导致的飞机随机加速度不确定性。

因而随机加速度的方差 \(\sigma_a^2=0.04\,\mathrm{m^2/s^4}\)。

在本例中，过程噪声矩阵为：

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

当 \(\Delta t=5\,\mathrm{s}\) 且 \(\sigma_a^2=0.04\,\mathrm{m^2/s^4}\) 时，上式化为：

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

过程噪声矩阵的推导见 书籍 的 第 8.2.2 节。

加入过程噪声后，我们的预测不确定度平方为：

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

迭代 0 小结

• 初始化
  我们用第一次测量作为初始状态估计 \( {\hat{\boldsymbol{x}}}_{0,0} \)，并用测量协方差作为初始状态协方差 \(\boldsymbol{P}_{0,0}\)。
  注意：这仅在初始化阶段可行。
• 预测
  我们在雷达下一次重访目标的时刻，预测状态及其不确定度。卡尔曼滤波的预测方程如下：
  
  
  状态外插方程（State Extrapolation Equation）
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  协方差外插方程（Covariance Extrapolation Equation）
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  其中：
  
  • \(\hat{\boldsymbol{x}}_{n,n}\) 是时刻 \(n\) 的系统状态估计向量
  • \(\hat{\boldsymbol{x}}_{n+1,n}\) 是由时刻 \(n\) 的信息计算得到的对时刻 \(n+1\) 的系统状态预测向量
  • \(\boldsymbol{u}_n\) 是控制量或输入量，表示对系统的已知外部输入
  • \(\boldsymbol{F}\) 为状态转移矩阵
  • \(\boldsymbol{G}\) 为输入（控制）矩阵或输入转移矩阵，用于将输入映射到状态变量
  • \(\boldsymbol{P}_{n,n}\) 为当前状态的协方差矩阵（平方不确定度）
  • \(\boldsymbol{P}_{n+1,n}\) 为预测状态的协方差矩阵（平方不确定度）
  • \(\boldsymbol{Q}\) 为过程噪声矩阵

迭代 1

滤波器更新

假设在 \(t_1\) 获得第二次测量：

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

由于本次测量期间出现强噪声尖峰，其信噪比显著低于第一次测量，因此第二次测量的不确定度更高。

设距离测量的标准差为 \(6m\)，速度测量的标准差为 \(1.5m/s\)。对应的测量协方差矩阵为：

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

我们希望估计当前系统状态 \(\hat{\boldsymbol{x}}_{1,1}\)。在时间 \(t_1\)，我们拥有两部分信息：

• 预测状态 \(\hat{\boldsymbol{x}}_{1,0}\)（上一时刻计算得到），以及
• 新的测量 \(\boldsymbol{z}_1\)

我们该更信任哪一个？

直觉上，我们可能更愿意用测量作为当前估计，即 \(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\)，因为它比预测更“新”。

另一方面，测量也更嘈杂。比较预测协方差 \(\boldsymbol{P}_{1,0}\) 与测量协方差 \(\boldsymbol{R}_1\) 的主对角元素可见，预测的不确定度小于测量的不确定度：

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

那么我们是否应该忽略新测量而保留预测，即 \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)？

这样做会丢失当前测量提供的新信息。

卡尔曼滤波的关键思想是我们两者都不单独采用，而是将预测与测量加权融合，并对不确定度较低的一方赋予更大的权重。

解决方案是对测量与预测进行加权平均：

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

其中权重 \(K_1\) 为卡尔曼增益（Kalman Gain）。它决定测量与预测的加权比例，并以最小化估计不确定度的方式进行，这使得卡尔曼滤波成为最优滤波（在系统与噪声满足模型假设的前提下）。

稍后我们将给出卡尔曼增益的方程。先来看状态更新方程，其矩阵形式为：

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

其中 \(\boldsymbol{I}\) 是单位矩阵（主对角为 1、其余为 0 的方阵）。

我们将其改写为：

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

该形式表明，更新后的状态等于预测 \(\hat{\boldsymbol{x}}_{1,0}\) 加上修正项 \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\)。

修正项与测量与预测之间的差值 \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\) 成比例，该差值称为新息（测量残差）。

在本例中，系统状态与测量均为向量，且代表相同的物理量（距离与速度），因此可以直接用 \(\boldsymbol{z}_1\) 减去 \(\hat{\boldsymbol{x}}_{1,0}\)。

然而并非总是如此。一般情况下，测量与系统状态可能属于不同的物理域。例如，数字温度计测得的是电信号，而系统状态是温度。

因此，需要先将预测状态变换到测量域：

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

矩阵 \(\boldsymbol{H}\) 称为观测矩阵（Observation/Measurement Matrix），用于将状态变量映射到实际测得的量。

在本例中，观测矩阵即为单位矩阵：

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

关于观测矩阵的更多信息，请参见在线教程的测量方程部分以及书籍中的示例 9 与 10。

现在我们可以将状态更新方程改写为：

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

创新项 \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) 表示新信息。

卡尔曼增益决定这条新信息应当在多大程度上修正预测状态，也即我们对预测进行修正的强度。

一维情形

在一维情形下，卡尔曼增益为：

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

其中：

• \(p_{n,\ n-1}\) 为预测状态的方差
• \(r_n\) 为测量的方差

卡尔曼增益的选择使得更新估计的方差 \(p_{n,n}\) 最小化，这正是卡尔曼滤波最优的原因。

如需建立直观理解并查看一维情形下的完整推导，请参见在线教程的一维卡尔曼滤波部分。

多变量情形

在多变量卡尔曼滤波中，卡尔曼增益为矩阵，表达式为：

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

关于多变量卡尔曼增益方程的推导，请参见在线教程的卡尔曼增益部分。

让我们计算 \(t_1\) 时刻的卡尔曼增益：

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

在本例中，\(\boldsymbol{H}=\boldsymbol{I}\)，且 \(\boldsymbol{H}^T=\boldsymbol{I}\)。

代入各矩阵：

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

更新后的状态估计为：

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

在本例中，\(\boldsymbol{H}=\boldsymbol{I}\)，因此创新项为：

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

现在应用修正：

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

最终得到：

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

当我们已经估计出当前状态后，还需要量化该估计的不确定性。

一维情形

在一维情形下，协方差更新方程（Covariance Update Equation）为：

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

推导过程见在线教程的一维卡尔曼滤波部分。

多变量情形

Joseph 形式

在多变量卡尔曼滤波中，协方差更新方程常采用一种数值更稳定的形式，即Joseph 形式，由 Peter Joseph 提出。

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1}(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]

其中：

• \(\boldsymbol{P}_{n,n}\) 为更新（后验）状态估计的协方差
• \(\boldsymbol{P}_{n,n-1}\) 为预测（先验）状态估计的协方差
• \(\boldsymbol{K}_n\) 为卡尔曼增益
• \(\boldsymbol{H}\) 为观测（测量）矩阵
• \(\boldsymbol{R}_n\) 为测量噪声协方差矩阵
• \(\boldsymbol{I}\) 为单位矩阵

推导过程见在线教程的协方差更新方程部分。

简化形式

文献中也常见到协方差更新的简化形式：

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

其推导见协方差更新简化方程部分。

在精确算术下，两种形式给出相同结果。但在计算机实现中，通常更推荐 Joseph 形式，因为其数值更稳定。

仅在本示例中，我们采用简化的协方差更新方程：

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

在本例中，\(\boldsymbol{H}=\boldsymbol{I}\)，因此：

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

现在代入各矩阵：

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

结果分析

更新估计的不确定度低于预测不确定度与测量不确定度：

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

通过融合测量与预测，并使用卡尔曼增益进行加权，我们得到更低不确定度的估计。

即使新信息具有较高不确定度，加入它仍会降低整体估计不确定度。数学证明见书籍中的传感器融合章节以及附录 G 与 H。从理论角度看，新测量不应被忽略。

但在实践中，常常需要拒绝某些测量。关于处理不可靠测量的实用方法，参见书籍中的异常值处理章节。

预测

迭代 1 的预测步骤（从 \(t_1\) 到 \(t_2\)）与 迭代 0 的预测步骤（从 \(t_0\) 到 \(t_1\)）相同，只是现在我们从更新后的估计 \(\hat{\boldsymbol{x}}_{1,1}\) 与 \(\boldsymbol{P}_{1,1}\) 出发。

状态预测

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

协方差预测

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

注意，预测步骤中两个方差再次增加。这是因为随着时间推移而没有新测量，不确定度会自然增长。尤其是速度不确定度会导致额外的距离不确定度，因此距离方差增长更快。

迭代 1 小结

• 更新
  
  • 我们将当前系统状态 \(\hat{\boldsymbol{x}}_{1,1}\) 估计为预测状态 \(\hat{\boldsymbol{x}}_{1,0}\) 与测量 \(\boldsymbol{z}_1\) 的加权组合。
    加权由卡尔曼增益 \(K_1\) 决定。卡尔曼增益由预测协方差 \(\boldsymbol{P}_{1,0}\) 与测量协方差 \(\boldsymbol{R}_1\) 计算得到，并使更新估计的不确定度 \(\boldsymbol{P}_{1,1}\) 最小化。
  • 卡尔曼滤波的更新方程如下：
    
    
    状态更新方程（State Update Equation）
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    协方差更新方程（Joseph 形式）
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    或其简化形式
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    卡尔曼增益方程（Kalman Gain）
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  其中：
  
  • \( \hat{\boldsymbol{x}}_{n,n} \) 为时刻 n 的更新状态估计
  • \( \hat{\boldsymbol{x}}_{n,n-1} \) 为时刻 n 的预测状态（由 n-1 时刻的信息计算）
  • \( \boldsymbol{z}_n \) 为测量向量
  • \( \boldsymbol{P}_{n,n} \) 为更新状态估计的协方差
  • \( \boldsymbol{P}_{n,n-1} \) 为预测状态估计的协方差
  • \( \boldsymbol{K}_n \) 为卡尔曼增益
  • \( \boldsymbol{H} \) 为观测（测量）矩阵
  • \( \boldsymbol{R}_n \) 为测量噪声协方差矩阵
  • \( \boldsymbol{I} \) 为单位矩阵
• 预测
  迭代 1 的预测步骤与迭代 0 相同。
  使用状态转移模型，将当前状态估计及其协方差推进到下一时刻（雷达再次重访飞机时）。

示例小结