(通过简单直观的推导理解卡尔曼基础)Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

本文提供了卡尔曼滤波器的简单直观的推导，目的是向不需要强大数学背景的学科的学生教授这种有用的工具。理解该推导所需的最复杂的数学是将两个高斯函数相乘并将结果简化为紧凑形式的能力。

卡尔曼滤波器已有50多年的历史，但仍然是当今使用的最重要和最常见的数据融合算法之一。卡尔曼滤波器以鲁道夫·卡尔曼（Rudolf E.Kálmán）的名字命名，其巨大的成功归因于其小的计算需求，优雅的递归特性以及作为具有高斯误差统计量的一维线性系统的最佳估计器的地位¹。卡尔曼滤波器的典型用途包括平滑噪声数据并提供感兴趣参数(parameters of interest)的估计值。应用包括全球定位系统接收器，无线电设备中的锁相环，平滑笔记本电脑触控板的输出等等。

从理论上讲，卡尔曼滤波器是一种允许在线性动力学系统中进行精确推断的算法，该算法是类似于隐马尔可夫模型的贝叶斯模型，但其中latent变量的状态空间是连续的，并且所有latent变量和观测变量具有 高斯分布（通常是多元高斯分布）。本讲义的目的是让对卡尔曼滤波感到困惑或恐惧的人通过简单直观的推导来理解卡尔曼滤波器的基础。

卡尔曼滤波器已有50多年的历史，但仍然是当今使用的最重要和最常见的数据融合算法之一。

RELEVANCE

卡尔曼滤波器²（及其变体，例如扩展卡尔曼滤波器³和 unscented 卡尔曼滤波器⁴）是信息处理领域中最著名和最受欢迎的数据融合算法之一。卡尔曼滤波器通常使用矢量代数作为最小均方估计量⁵推导，该方法适用于对数学充满信心的学生，但不适用于对数学要求不高的学科的学生。卡尔曼滤波器是从第一原理出发，考虑一个利用高斯分布的关键属性的简单物理示例（特别是两个高斯分布的乘积是另一个高斯分布的属性）而得出的。

PREREQUISITES

为了简单起见，本文没有设计成为了刚接触卡尔曼的学生提供详细的教程，而是意在向数学薄弱的学生教授卡尔曼滤波的简单方法教程。希望读者熟悉与状态向量和协方差矩阵等卡尔曼滤波相关的向量符号和术语。本文针对那些需要以简单直观的方式向其他人讲授卡尔曼滤波器的人，或针对那些已经对卡尔曼滤波器有一定经验但可能不完全了解其基础的人。 本文并非旨在作为面向新手的全面而独立的教育工具，因为这将需要一章而不是几页的内容来传达。

PROBLEM STATEMENT

卡尔曼滤波假设一个系统$t$时刻的状态可由它的前一个状态$t-1$时刻演化而来，根据下面的关系
$${\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{w}}_t\text{\hspace{1em}(1)}$$
其中

• ${\mathbf{x}}_t$表示时间$t$时刻包含系统兴趣项(term of interest)的状态向量 ，例如系统的位置，速度，方向。
• ${\mathbf{u}}_t$表示任何控制输入（如转向角，油门设置，制动力）的向量
• ${\mathbf{F}}_t$是状态转换矩阵，它将时间$t-1$系统状态的每个参数的影响应用于时间$t$系统状态（例如，时间$t-1$的位置和速度都影响时间$t$的位置）
• ${\mathbf{B}}_t$是控制输入矩阵，它将${\mathbf{u}}_t$的每个控制输入参数应用到状态向量（例如，将油门门设置的效果应用于系统速度和位置）。
• ${\mathbf{w}}_t$是每一个状态向量参数的过程噪声(process noise)向量。假定过程噪声是从零均值多元正态分布中提取的，并且协方差矩阵${\mathbf{Q}}_t$给出了协方差。

系统的测量还可以根据下面的模型完成
$${\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t\text{\hspace{1em}(2)}$$
其中

• ${\mathbf{z}}_t$是测量向量
• ${\mathbf{H}}_t$是变换矩阵，它将状态向量参数映射到测量域
• ${\mathbf{v}}_t$是包含测量噪声项，它来源于测量向量中每次观察。像过程噪声一样，假定测量噪声为具有协方差${\mathbf{R}}_t$的零均值高斯白噪声。

在下面的推导中，我们将考虑一个简单的一维跟踪问题，尤其是沿铁路线行驶的火车的一维跟踪问题（参见FIG1）。我们可以思考一下这个问题中的一些向量和矩阵的例子，如状态向量${\mathbf{x}}_t$包含小车的位置和速度
$${\mathbf{x}}_t=\left[\begin{array}{c}{x}_t\\ {\text{x˙}}_t\end{array}\right]$$
小车司机可以向系统施加制动或者加速的输入，在此我们将其认为$f_t$和小车质量$m$的一个函数, 例如该控制信息用向量${\mathbf{u}}_t$表示为
$${\mathbf{u}}_t=\frac{f_t}{m}$$
在时间段$\Delta t$（时间点$t-1$和$t$之间经过的时间）中通过制动器或油门施加的力与小车的位置和速度之间的关系由以下公式给出：
$$\begin{array}{rl}{x}_t& =x_{t-1}+({\text{x˙}}_{t-1}\times \Delta t)+\frac{f_t(\Delta t{)}^2}{2m}\\ {\text{x˙}}_t& ={\text{x˙}}_{t-1}+\frac{f_t\Delta t}{m}\end{array}$$
这些线性等式可以写成矩阵形式：
$$\left[\begin{array}{c}{x}_t\\ \text{x˙}\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ {\text{x˙}}_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]\frac{f_t}{m}$$
通过与（1）比较，对于该例子我们得到
$$\begin{array}{rl}{\mathbf{F}}_t& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\\ {\mathbf{B}}_t& =\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]\end{array}$$
${\mathbf{x}}_t$的真实状态不可能直接观察，卡尔曼滤波提供了一个算法来估计一个${\hat{\mathbf{x}}}_t$，它通过组合系统模型和某些参数或者参数的线性函数的噪声测量得到。因此，现在通过概率密度函数（pdfs）而不是离散值来提供状态向量中感兴趣参数的估计值。卡尔曼滤波以高斯pdfs为基础，这使得接下来的“Solutions”部分的推导变得很清晰。为了完整的描述高斯函数，我们需要知道它的方差和协方差，这些值被存储在协方差矩阵${\mathbf{P}}_t$，${\mathbf{P}}_t$主对角线上的量是状态向量对应参数项的方差，非对角线上的量对应状态向量参数项之间的协方差。在建模良好的一维线性系统中，其测量误差来自零均值高斯分布，卡尔曼滤波器已被证明是最佳估计器¹。在本文的其余部分中，我们将推导卡尔曼滤波器方程，使我们能够通过结合先验知识，系统模型的预测和噪声测量来递归计算${\hat{\mathbf{x}}}_t$。
卡尔曼滤波算法包括两个阶段：预测和测量更新。预测阶段的标准卡尔曼滤波方程为
$$\begin{array}{rlr}{\hat{\mathbf{x}}}_{t|t-1}& ={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{B}}_t{\mathbf{u}}_t& (3)\\ {\mathbf{P}}_{t|t-1}& ={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^T+{\mathbf{Q}}_t& (4)\end{array}$$
其中${\mathbf{Q}}_t$是带有控制输入噪声的过程噪声协方差矩阵。等式（3）在上面已经推导，接下来我们推导（4）。未知状态参数${\mathbf{x}}_t$与预测${\hat{\mathbf{x}}}_{t|t-1}$的方差由
$${\mathbf{P}}_{t|t-1}=\mathrm{E}[({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1}{)}^T]$$
给出。由（1）和（3）可知
$$\begin{array}{rl}{\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1}& ={\mathbf{F}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})+{\mathbf{w}}_t\\ ⟹{\mathbf{P}}_{t|t-1}& =\mathrm{E}[({\mathbf{F}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})+{\mathbf{w}}_t)({\mathbf{F}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})+{\mathbf{w}}_t{)}^T]\\ & ={\mathbf{F}}_t\mathrm{E}[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}{)}^T]{\mathbf{F}}_t^T\\ & +{\mathbf{F}}_t\mathrm{E}[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}){\mathbf{w}}_t^T]\\ & +\mathrm{E}[{\mathbf{w}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}{)}^T]{\mathbf{F}}_t^T\\ & +\mathrm{E}[\mathbf{w}{\mathbf{w}}_t^T]\end{array}$$
注意状态误差与过程噪声无关
$$\begin{array}{rl}\mathrm{E}[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}){\mathbf{w}}_t^T]& =\mathrm{E}[{\mathbf{w}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}{)}^T]=0\\ ⟹{\mathbf{P}}_{t|t-1}& ={\mathbf{F}}_t\mathrm{E}[({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1}{)}^T]{\mathbf{F}}_t^T+\mathrm{E}[\mathbf{w}{\mathbf{w}}_t^T]\\ {\mathbf{P}}_{t|t-1}& ={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^T+{\mathbf{Q}}_t\end{array}$$
测量更新方程式为
$$\begin{array}{rlr}{\hat{\mathbf{x}}}_{t|t}& ={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_{\mathbf{t}}{\hat{\mathbf{x}}}_{\mathbf{t}|\mathbf{t}-1})& (5)\\ {\mathbf{P}}_{t|t}& ={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{\mathbf{t}|\mathbf{t}-1}& (6)\end{array}$$
其中
$${\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T+{\mathbf{R}}_t{)}^{-1}\text{\hspace{1em}(7)}$$
本文的剩余部分，我们将从第一原理推导测量更新方程[(5)-(7)]。

SOLUTIONS

我们将考虑一个简单的一维跟踪问题，尤其是沿铁路线行驶的火（小）车的一维跟踪问题来推导卡尔曼滤波。在每次测量时刻我们想要知道小车最可能的位置估计（或更准确地说，是小车车顶上的无线电天线的位置）。可以从两个源获取的信息：

• 预测 ,以小车前一次（或者最近的一次，或者上一次）的位置和速度为基础的预测
• 测量， 部署在小车轨道一侧的无线电系统的测量

通过结合预测和测量中的知识，可以提供对小车位置的最佳估计

组合预测和测量的信息以提供列车位置的最佳可能估计。 该系统FIG1所示。

已知系统的初始状态（在时间$t=0$ 秒时）具有合理的精度，如FIG2所示。通过高斯概率密度函数给第小车的位置（即小车的为止 符合高斯分布），接下来的时刻($t=1$秒)，我们可以根据$t=0$时的位置、速度，以及最大可能的加速度和减速度等的约束来估计小车新的位置。实际情况中，我们可能知道小车的制动器或者油门的控制输入。无论如何，我们都可以预测火车的新位置，在图3中用新的高斯密度函数表示，并带有新的均值和方差。数学上，这一步可由（1）表示，方差增加了，表示与$t=0$时相比，位置估计的精确性降低，这是由于从$t=0$到$t=1$进行的加速或者减速产生的过程噪声（process noise）的不确定。

$t=1$时，我们使用无线电系统测量小车的位置，在FIG4中使用蓝色高斯概率密度函数表示。通过结合预测和测量中的知识，可以提供对小车位置的最佳估计。这可以通过将对应的高斯概率密度函数的相乘获取，在FIG5中使用绿色概率密度函数表示。

高斯函数的重要特性是：两个高斯函数的乘积是另一个高斯函数。这一点很关键，因为它允许随着时间的增长而增加无数的高斯pdf，但是结果函数不会增加复杂度或项数； 在每个时间段之后，新的pdf都完全由高斯函数表示。 这是卡尔曼滤波器优雅的递归特性的关键。

上图中描述的步骤，我们再一次从数学上推导卡尔曼滤波测量更新等式，在FIG3中预测的红色高斯pdf(概率密度函数)可表示为
$$y_1(r;{\mu }_1,{\sigma }_1)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}}\text{\hspace{1em}(8)}$$
在FIG4中测量的蓝色高斯pdf(概率密度函数)可表示为
$$y_2(r;{\mu }_2,{\sigma }_2)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2}}\text{\hspace{1em}(9)}$$
这两个pdf提供的信息通过将两者相乘得到融合，即同时考虑预测和度量（请参见FIG5）。 因此，新的pdf表示来自预测和测量的信息融合，以及我们对系统的最佳当前估计，由这两个高斯函数的乘积给出
$$\begin{array}{rl}{y}_{fused}(r;{\mu }_1,{\sigma }_1,{\mu }_2,{\sigma }_2)& \\ & =\frac{1}{\sqrt{2\pi {\sigma }_1^2}}{e}^{-\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}}\times \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2}}\\ & =\frac{1}{2\pi \sqrt{{\sigma }_1^2{\sigma }_2^2}}{e}^{-(\frac{(r-{\mu }_1{)}^2}{2{\sigma }_1^2}+\frac{(r-{\mu }_2{)}^2}{2{\sigma }_2^2})}\end{array}\text{\hspace{1em}(10)}$$
可以扩展此新函数中的二次项，然后以高斯形式重写整个表达式
$$y_{fused}(r;{\mu }_{fused},{\sigma }_{fused})\triangleq \frac{1}{\sqrt{2\pi {\sigma }_{fuesed}^2}}{e}^{-\frac{(r-{\mu }_{fused}{)}^2}{2{\sigma }_{fused}^2}}\text{\hspace{1em}(11)}$$
其中
$$\begin{array}{rl}{\mu }_{fused}& =\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}\\ & ={\mu }_1+\frac{{\mu }_1^2({\mu }_2-{\mu }_1)}{{\mu }_1^2+{\mu }_2^2}\end{array}\text{\hspace{1em}(12)}$$
以及
$${\sigma }_{fused}^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}\text{\hspace{1em}(13)}$$
最后两个方程式代表卡尔曼滤波器算法的测量更新步骤，如下所示。但是，为了呈现更一般的情况，我们需要考虑对该示例进行扩展。

在上面的例子中我们假设预测和更新的高斯pdf在相同的坐标系和单位融合的，这导致了一对特别简洁的方程式，代表预测和测量更新阶段。然而，重要的是要注意，实际上，通常需要一个函数将预测和测量值映射到同一域中。 在我们的示例的更现实的扩展中，小车的位置将直接预测为沿铁路线的新距离，以米为单位，但是飞行时间以秒为单位记录时间。为了使预测和测量pdf相乘，必须将一个pdf转换为另一个pdf的域，并且标准做法是通过转换矩阵${\mathbf{H}}_t$将预测映射到测量域。

现在，我们重新讨论（8）和（9），而不是让y 1和y 2都代表沿铁路轨道的米数，我们考虑分布$y_2$代表无线电信号传播的飞行时间（以秒为单位），表示从$x=0$位置发射到$x_i$的小车顶部的天线位置的时间。通过将函数除以光速$c$将空间预测pdf $y_1$转换到测量域，因此重写公式(8)和(9)为
$$y_1(s;{\mu }_1,{\sigma }_1,c)\triangleq \frac{1}{\sqrt{2\pi (\frac{{\sigma }_1}{c}{)}^2}}{e}^{-\frac{(s-\frac{{\mu }_1}{c}{)}^2}{2(\frac{{\sigma }_1}{c}{)}^2}}\text{\hspace{1em}(14)}$$
$$y_2(s;{\mu }_2,{\sigma }_2)\triangleq \frac{1}{\sqrt{2\pi {\sigma }_2^2}}{e}^{-\frac{(s-{\mu }_2{)}^2}{2{\sigma }_2^2}}\text{\hspace{1em}(15)}$$
现在两个分布都在测量域中定义，无线电信号沿时间“ s”轴传播，而测量单位（秒）是第二个。接下来跟之前的推导一样，我们现在发现
$$\begin{gathered} \frac{{\mu }_{fused}}{c}=\frac{{\mu }_1}{c}+\frac{(\frac{{\sigma }_1}{c}{)}^2({\mu }_2-\frac{{\mu }_1}{c})}{(\frac{{\mu }_1}{c}{)}^2+{\mu }_2^2} \\ ⟹{\mu }_{fused}={\mu }_1+\left(\frac{(\frac{{\mu }_1}{c}{)}^2}{(\frac{{\mu }_1}{c}{)}^2+{\mu }_2^2}\right)({\mu }_2-\frac{{\mu }_1}{c})\text{\hspace{1em}(16)} \end{gathered}$$
代换$H=1/c$, $K=(H{\sigma }_1^2)/(H^2{\sigma }_1^2+{\sigma }_2^2)$得
$${\mu }_{fused}={\mu }_1+K({\mu }_2-H{\mu }_1)\text{\hspace{1em}(17)}$$
类似融合的方差估计为
$$\begin{gathered} \frac{{\sigma }_{fused}^2}{c^2}=(\frac{{\sigma }_1}{c}{)}^2-\frac{(\frac{{\sigma }_1}{c}{)}^4}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2} \\ ⟹\begin{array}{rl}{\sigma }_{fused}^2& ={\sigma }_1^2-\left(\frac{\frac{{\sigma }_1^2}{c}}{(\frac{{\sigma }_1}{c}{)}^2+{\sigma }_2^2}\right)\frac{{\sigma }_1^2}{c}\\ & ={\sigma }_1^2-KH{\sigma }_1^2\end{array}\text{\hspace{1em}(18)} \end{gathered}$$
现在，我们可以将标量推导得出的某些项与卡尔曼滤波算法中使用的标准向量和矩阵进行比较：

• ${\mu }_{fused}\to {\hat{\mathbf{x}}}_{t|t}$ ：数据融合后的状态向量
• ${\mu }_1\to {\hat{\mathbf{x}}}_{t|t-1}$： 数据融合之前的状态向量，也就是预测
• ${\sigma }_{fused}^2\to {\mathbf{P}}_{t|t}$：数据融合以后的协方差矩阵（置信度）
• ${\sigma }_1^2\to {\mathbf{P}}_{t|t-1}$：数据融合之前的协方差矩阵
• ${\mu }_2\to {\mathbf{z}}_t$：测量向量
• ${\sigma }_2^2\to {\mathbf{R}}_t$：与测量噪声相关的不确定矩阵
• $H\to {\mathbf{H}}_t$：映射状态向量参数到测量域的变换矩阵
• $$\begin{array}{rl}K& =\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}\to {\mathbf{K}}_t\\ & ={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^T+{\mathbf{R}}_t{)}^{-1}\end{array}$$
  是卡尔曼增益

同理由（17），（18）我们很容易得到：
$$\begin{array}{rl}{\mu }_{fused}& ={\mu }_1+\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}({\mu }_2-H{\mu }_1)\\ \to {\hat{\mathbf{x}}}_{t|t}& ={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})\\ {\sigma }_{fused}^2& ={\sigma }_1^2-\left(\frac{H{\sigma }_1^2}{H^2{\sigma }_1^2+{\sigma }_2^2}\right)H{\sigma }_1^2\\ \to {\mathbf{P}}_{t|t}& ={\mathbf{P}}_{t|t-1}-{\mathbf{K}}_t{\mathbf{H}}_t{\mathbf{P}}_{t|t-1}\end{array}$$

REFERENCE

---

1. B. D. O. Anderson and J. B. Moore, Optimal Filtering. New York: Dover, 2005. ↩︎ ↩︎

2. R. E. Kalman, “A new approach to linear filtering and prediction problems,” J. Basic Eng., vol. 82, no.1, pp. 35–45, Mar. 1960. ↩︎

3. P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Norwood, MA: Artech House, 2008. ↩︎

4. S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proc. IEEE, vol. 92, no. 3, pp. 401–422, 2004. ↩︎

5. J. Bibby and H. Toutenburg, Prediction and Improved Estimation in Linear Models. New York: Wiley, 1977. ↩︎