卡尔曼滤波器的工作原理，图片

我必须告诉你们卡尔曼滤波器，因为它的功能非常惊人。

令人惊讶的是，似乎很少有软件工程师和科学家知道它，这让我感到难过，因为它是一个在存在不确定性的情况下组合信息的如此通用和强大的工具。有时，它提取准确信息的能力似乎近乎神奇-如果听起来我夸大其词了，那么看看之前发布的这段视频，我演示了卡尔曼滤波器通过观察自由漂浮体的速度来计算其方向。非常整洁！

你可以在任何你有关于某个动态系统的不确定信息的地方使用卡尔曼滤波器，并且你可以对系统下一步要做什么做出一个有根据的猜测。即使混乱的现实出现，干扰了你猜测的干净运动，卡尔曼滤波器通常也能很好地弄清楚实际发生了什么。它可以利用疯狂现象之间的相互关系，这可能是你可能没有想过要利用的！

卡尔曼滤波器是不断变化的系统的理想选择。它们的优点是内存很少(除了以前的状态不需要保存任何历史记录)，而且速度非常快，这使得它们非常适合实时问题和嵌入式系统。

在谷歌上的大多数地方，实现卡尔曼滤波器的数学看起来都相当可怕和不透明。这是一种糟糕的情况，因为如果你正确地看待卡尔曼滤波器，它实际上是超级简单和容易理解的。因此，它是一个很棒的文章主题，我会尝试用很多清晰、漂亮的图片和颜色来说明它。前提条件很简单，您只需要对概率和矩阵有基本的了解。

我将从一个卡尔曼滤波器可以解决的松散的例子开始，但是如果您想要直接进入闪亮的图片和数学，请随意跳到前面。

让我们来做一个玩具例子：你已经建造了一个可以在树林里漫步的小机器人，机器人需要确切地知道它的位置，这样它才能导航。

我们假设我们的机器人有一个状态\(\vec{x_k}\)，它只是一个位置和一个速度：

请注意，状态只是关于系统底层配置的数字列表；它可以是任何东西。在我们的示例中，它是位置和速度，但它可以是关于油箱中的液量、汽车引擎的温度、用户手指在触摸板上的位置或需要跟踪的任何数量的数据。

我们的机器人还有一个GPS传感器，可以精确到10米左右，这很好，但是它需要知道它的位置比10米更精确。这些树林里有很多沟壑和悬崖，如果机器人错了几英尺以上，它可能会掉下悬崖。所以GPS本身还不够好。

我们可能也知道机器人是如何移动的：它知道发送给车轮马达的命令，它知道如果它朝一个方向前进，没有任何干扰，下一刻它很可能会沿着这个方向走得更远。但当然，它并不完全了解它的运动：它可能会受到风的冲击，轮子可能会稍微打滑，或者在崎岖的地形上滚动；所以轮子转动的量可能不能准确地代表机器人实际行驶了多远，预测也不会很完美。

GPS传感器告诉我们一些关于这个州的信息，但只是间接的，而且有一些不确定或不准确的地方。我们的预测告诉了我们一些关于机器人如何移动的信息，但只是间接的，而且有一些不确定性或不准确性。

但是，如果我们使用所有可用的信息，我们能得到一个比这两个估计本身给我们的更好的答案吗？答案当然是肯定的，这就是卡尔曼滤波器的作用。

让我们来看看我们试图诠释的风景。我们将继续简单的状态，只有位置和速度。$$\vec{x}=\BEGIN{bMatrix}p\\v\end{bMatrix}$$。

我们不知道实际的位置和速度是什么；有一系列可能的位置和速度的组合可能是真的，但其中一些比另一些更有可能：

卡尔曼滤波器假设两个变量(在我们的情况下，位置和速度)都是随机的，并且是高斯分布的。每个变量都有一个平均值\(\µ\)和一个方差\(\sigma^2\)，前者是随机分布(及其最可能的状态)的中心，后者是不确定性：

在上图中，位置和速度是不相关的，这意味着一个变量的状态不会告诉您另一个变量可能是什么。

下面的例子显示了一些更有趣的东西：位置和速度是相关的。观察特定位置的可能性取决于你的速度：

例如，如果我们根据旧的仓位估计新的仓位，可能会出现这种情况。如果我们的速度高，我们可能会移动得更远，所以我们的位置会更远。如果我们走得慢，我们就没走那么远。

跟踪这种关系真的很重要，因为它给了我们更多的信息：一种测量告诉我们其他的可能是什么。这就是卡尔曼滤波器的目标，我们想要从我们不确定的测量中尽可能多地压缩信息！

这种相关性被称为协方差矩阵的东西捕获。简而言之，矩阵\(\sigma_{ij}\)的每个元素是第i个状态变量和第j个状态变量之间的相关度。(您可能会猜到协方差矩阵是对称的，这意味着即使您交换i和j也无关紧要)。协方差矩阵通常被标记为“\(\mathbf{\sigma}\)”，因此我们称其元素为“\(\sigma_{ij}\)”。

我们将关于状态的知识建模为高斯斑点，因此在时间\(K)需要两条信息：我们将最佳估计称为\(\mathbf{\hat{x}_k}\)(平均值，别处称为\(\µ\))，以及它的协方差矩阵\(\mathbf{P_k}\)。$$\BEGIN{公式}\LABEL{eq：statevars}\BEGIN{Aligned}\mathbf{\hat{x}}_k&；=\Begin{bMatrix}\text{position}\text{ocity}\end{bMatrix}\mathbf{P}_k&；=\Begin{bMatrix}\Sigma_{pp}&；\Sigma_{pv}\Sigma_{VP}&；\Sigma_{vv}\End{bMatrix}\End{Alignment}\End{公式}$$。

(当然，我们在这里只使用位置和速度，但请记住，状态可以包含任意数量的变量，并且可以表示您想要的任何内容，这一点很有用)。

接下来，我们需要一些方法来查看当前状态(在时间k-1)并预测在时间k的下一个状态。记住，我们不知道哪个状态是“真实的”状态，但是我们的预测函数并不关心。它只适用于所有这些组件，并为我们提供了一个新的分布：

它获取原始估计中的每个点，并将其移动到新的预测位置，如果原始估计是正确的，则系统将移动到该位置。

让我们把这个贴上。我们如何使用矩阵来预测未来下一个时刻的位置和速度？我们将使用一个非常基本的运动学公式：$$\BEGIN{SPLIT}\COLOR{深粉色}{p_k}&；=\color{royalBlue}{p_{k-1}}+\Delta t&；\color{royalBlue}{v_{k-1}}\color{深粉色}{v_k}&；=&；\color{royalBlue}{v_{k-1}}\end{plit}$$换言之：$$\BEGIN{ALIGN}\COLOR{深粉红色}{\mathbf{\hat{x}}_k}&；=\Begin{bMatrix}1&；\Delta t\\0&；1\end{bMatrix}\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}\\&；=\mathbf{F}_k\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}\Label{statevars}\end{Align}$$。

我们现在有了一个预测矩阵，它给出了我们的下一个状态，但我们仍然不知道如何更新协方差矩阵。

这就是我们需要另一个公式的地方。如果我们将分布中的每个点乘以一个矩阵\(\color{Firerick}{\mathbf{A}}\)，那么它的协方差矩阵\(\sigma\)会发生什么？

嗯，这很简单。我只给您提供身份：$$\BEGIN{SPLIT}CoV(X)&；=\Sigma\\CoV(\color{Firerick}{\mathbf{A}}x)&；=\color{Firerick}{\mathbf{A}}\Sigma\color{Firerick}{\mathbf{A}}^T\end{Split}\Label{covident}\end{equence}$$。

不过，我们并没有捕捉到所有的东西。可能会有一些与国家本身无关的变化--外部世界可能正在影响系统。

例如，如果国家模拟火车的运动，火车操作员可能会推油门，导致火车加速。类似地，在我们的机器人示例中，导航软件可能会发出转动轮子或停止的命令。如果我们知道这些关于世界上正在发生的事情的额外信息，我们可以把它塞进一个叫做\(\color{暗橙色}{\vec{\mathbf{u}_k}}\)的向量中，对它做些什么，然后把它作为修正添加到我们的预测中。

假设我们知道由于油门设置或控制命令而产生的预期加速度\(\color{暗橙色}{a}\)。从基本运动学出发，我们得到：$$\BEGIN{SPLIT}\COLOR{深红}{p_k}&；=\color{royalBlue}{p_{k-1}}+{\Delta t}&；\color{royalBlue}{v_{k-1}}+&；\frac{1}{2}\color{深橙}{a}{\Delta t}^2\color{深红}{v_k}&；=&。\color{royalBlue}{v_{k-1}}+&；\color{深橙色}{a}{\Delta t}\end{Split}$$矩阵形式：$$\BEGIN{等式}\BEGIN{SPLIT}\COLOR{深粉色}{\mathbf{\hat{x}}_k}&；=\mathbf{F}_k\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}+\Begin{bMatrix}\frac{\Delta t^2}{2}\Delta t\end{bMatrix}\color{深橙色}{a}\\&；=\mathbf{F}_k\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}+\mathbf{B}_k\color{暗橙色}{\vec{\mathbf{u}_k}}\end{分割}\end{等式}$$

\(\mathbf{B}_k\)称为控制矩阵，\(\color{暗橙色}{\vec{\mathbf{u}_k}}\)称为控制向量。(对于没有外部影响的非常简单的系统，您可以省略这些参数)。

让我们再增加一个细节。如果我们的预测对实际发生的事情不是100%准确的模型，会发生什么呢？

如果国家根据自己的属性发展，一切都很好。只要我们知道那些外力是什么，如果国家是基于外力发展的，一切都是好的。

但是我们不知道的力量呢？例如，如果我们正在跟踪一架四轴飞行器，它可能会受到风的冲击。如果我们追踪轮式机器人，轮子可能会打滑，或者地面上的颠簸可能会减慢它的速度。我们无法跟踪这些事情，如果这些事情发生了，我们的预测可能会出错，因为我们没有考虑到这些额外的力量。

通过在每个预测步骤之后添加一些新的不确定性，我们可以对与“世界”(即我们没有跟踪的东西)相关的不确定性进行建模：

在我们最初的估计中，每个州都可以转移到一个范围内的州。因为我们非常喜欢高斯斑点，所以我们会说\(\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}\)中的每个点都被移动到具有协方差的高斯斑点\(\color{mediumaquamarine}{\mathbf{q}_k}\)中的某个位置。另一种说法是，我们将未跟踪的影响视为具有协方差的噪声\(\color{mediumaquamarine}{\mathbf{q}_k}\)。

我们通过简单地添加\({\color{mediumaquamarine}{\mathbf{q}_k}}\)来获得扩展的协方差，给出预测步骤的完整表达式：$$\BEGIN{等式}\BEGIN{SPLIT}\COLOR{深粉}{\mathbf{\hat{x}}_k}&；=\mathbf{F}_k\color{royalBlue}{\mathbf{\hat{x}}_{k-1}}+\mathbf{B}_k\color{暗橙色}{\vec{\mathbf{u}_k}}\color{深粉色}{\mathbf{P}_k}&；=\mathbf{F_k}\color{royalBlue}{\mathbf{P}_{k-1}}\mathbf{F}_k^T+\color{mediumaquamarine}{\mathbf{q}_k}\end{Split}\Label{kalrectfull}\end{等式}$$。

换句话说，新的最佳估计是根据先前的最佳估计作出的预测，加上对已知外部影响的修正。

新的不确定性是从旧的不确定性中预测出来的，另外还有一些来自环境的不确定性。

好了，这已经很简单了。我们对我们的系统可能所在的位置有一个模糊估计，由\(\color{深粉}{\mathbf{\hat{x}}_k}\)和\(\color{深粉}{\mathbf{P}_k}\)给出。当我们从传感器获得一些数据时会发生什么？

我们可能有几个传感器，它们可以给我们提供有关系统状态的信息。就目前而言，他们测量什么并不重要；也许一个人读取位置，另一个人读取速度。每个传感器都间接地告诉我们一些关于状态的信息--换句话说，传感器对一个状态进行操作，并产生一组读数。

请注意，读数的单位和比例可能与我们跟踪的州的单位和比例不同。您可能会猜到这是怎么回事：我们将使用矩阵\(\mathbf{H}_k\)为传感器建模。

我们可以用通常的方式计算出我们期望看到的传感器读数的分布：$$\BEGIN{公式}\BEGIN{ALIGNED}\vec{\mu}_{\text{Expect}}&；=\mathbf{H}_k\color{深粉红色}{\mathbf{\hat{x}}_k}\mathbf{\Sigma}_{\text{Expect}}&；=\mathbf{H}_k\color{深粉红色}{\mathbf{P}_k}\mathbf{H}_k^T\end{aligned}\end{等式}$$。

卡尔曼滤波器最擅长的一件事就是处理传感器噪声。换句话说，我们的传感器至少在某种程度上是不可靠的，我们最初估计的每个状态都可能导致一定范围的传感器读数。

从我们观察到的每一个读数，我们可能会猜测我们的系统处于一种特定的状态。但由于存在不确定性，一些州比其他州更有可能产生我们看到的读数：

我们将这种不确定性的协方差(即传感器噪声的协方差)称为\(\color{mediumaquamarine}{\mathbf{R}_k}\)。分布的平均值等于我们观察到的读数，我们称之为\(\color{Yellowgreen}{\vec{\mathbf{z}_k}}\)。

所以现在我们有两个高斯斑点：一个围绕着我们变换后的预测平均值，另一个围绕着我们得到的实际传感器读数。

我们必须尝试使我们对基于预测状态(粉色)看到的读数的猜测与基于我们实际观察到的传感器读数(绿色)的不同猜测相一致。

那么我们最有可能的新州是什么？对于任何可能的读数\((z_1，z_2)\)，我们有两个相关的概率：(1)我们的传感器读数\(\color{Yellowgreen}{\vec{\mathbf{z}_k}}\)是\(z_1，z_2)\的(误)测量的概率，以及(2)我们先前估计认为\(z_1，z_2)\是我们应该看到的读数的概率。

如果我们有两个概率，我们想知道这两个概率都为真的概率，我们就把它们相乘。因此，我们将这两个高斯斑点相乘：

我们剩下的就是重叠部分，也就是两个斑点都很明亮/可能的区域。而且它比我们之前的任何一个估计都要精确得多。这个分布的平均值是两个估计值都最有可能的配置，因此，在我们拥有所有信息的情况下，它是对真实配置的最佳猜测。

结果是，当您用独立的均值和协方差矩阵乘以两个高斯斑点时，就会得到一个具有其自己的均值和协方差矩阵的新的高斯斑点！也许您可以看到这是怎么回事：一定有一个公式可以从旧参数中获得这些新参数！

让我们找出那个公式。首先从一个维度来看这是最容易的。具有方差和均值的一维高斯钟形曲线定义为：$$开始{方程}\标签{高斯公式}\数学{N}(x，\µ，\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\µ)^2}{2\sigma^2}}\end{方程}$$。

我们想知道当你把两条高斯曲线相乘时会发生什么。下面的蓝色曲线表示两个高斯总体的(未规格化)交集：

$$\BEGIN{公式}\LABEL{gausequiv}\Mathcal{N}(x，\color{紫红色}{\MU_0}，\color{深粉红色}{\sigma_0})\cdot\Mathcal{N}(x，\color{Yellowgreen}{\Mu_1}，\color{mediumaquamarine}{\sigma_1})\stackrel{？}{=}\Mathcal{N}(x，\。\color{mediumBlue}{\sigma‘})\end{公式}$$。

您可以将公式\(\eqref{gaussform}\)代入公式\(\eqref{gausequiv}\)，然后做一些代数运算(注意重新正规化，使总概率为1)，得到：$$\BEGIN{公式}\LABEL{FusionFormal}\BEGIN{ALIGNED}\COLOR{royalBlue}{\mu‘}&；=\MU_0+\FRAC{\sigma_0^2(\MU_1-\MU_0)}{\sigma_0^2+\sigma_1^2}\\color{mediumBlue}{\sigma‘}^2&；=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2}\end{aligned}\end{公式}$。

我们可以通过分解一小块并将其命名为\(\color{紫色}{\mathbf{k}}\)来简化：$$\BEGIN{公式}\Label{增益公式}\color{紫色}{\mathbf{k}}=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}\end{公式}$\Begin{公式}\Begin{Split}\color{royalBlue}{。\COLOR{紫色}{\mathbf{k}}(\MU_1-\MU_0)\COLOR{mediumBlue}{\sigma‘}^2&；=\sigma_0^2-&；\color{紫色}{\mathbf{k}}\sigma_0^2\end{Split}\Label{更新}\end{等式}$$。

记下你如何才能接受你以前的估计，并添加一些东西来做出新的估计。看看这个公式是多么简单！

但是矩阵版本呢？好，让我们用矩阵形式重写公式\(\eqref{收益公式}\)和\(\eqref{update}\)。如果\(\Sigma\)是高斯斑点的协方差矩阵，且\(\vec{\µ}\)其沿每个轴的平均值，则：$$\BEGIN{公式}\LABEL{矩阵增益}\COLOR{紫色}{\mathbf{K}}=\SIGMA_0(\SIGMA_0+\SIGMA_1)^{-1}\END{等式}$\BEGIN{等式}\BEGIN{SPLIT}\COLOR{roc。\COLOR{紫色}{\mathbf{K}}(\vec{\MU_1}-\VEC{\MU_0})\COLOR{mediumBlue}{\Sigma‘}&；=\Sigma_0-&；\COLOR{紫色}{\mathbf{K}}\Sigma_0\End{Split}\Label{matrixupdate}\end{公式}$$。

\(颜色{紫色}{\mathbf{K}}\)是一个称为卡尔曼增益的矩阵，我们稍后会用到它。

我们有两个分布：预测测量值为\((\color{FUHSIA}{\MU_0}，\COLOR{深粉红色}{\Sigma_0})=(\COLOR{FUHHIA}{\mathbf{H}_k\mathbf{\hat{x}}_k}，\color{深粉红色}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T})\)。Color{mediumaquamarine}{\sigma_1})=(color{yellowgreen}{\vec{\mathbf{z}_k}}，\color{mediumaquamarine}{\mathbf{R}_k})\)。我们只需将它们插入公式\(\eqref{matrixupdate}\)中即可找到它们的重叠部分：$$\BEGIN{EQREQUDE}\BEGIN{ALIGNED}\mathbf{H}_k\color{royalBlue}{\mathbf{\hat{x}}_k‘}&；=\color{FUHSIA}{\mathbf{H}_k\mathbf{\hat{x}}_k}&；\color{紫色}{\mathbf{K}}(\color{Yellowgreen}{\vec{\mathbf{z}_k}}-\color{紫红色}{\mathbf{H}_k\mathbf{\hat{x}}_k})\mathbf{H}_k\color{royalBlue}{\mathbf{P}_k‘}\mathbf{H}_k^。=\color{深粉红色}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T}&；-&；\color{紫色}{\mathbf{K}}\color{深粉色}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T}\end{aligned}\label{kalunsimplify}\end{等式}$$和来自\(\eqref{matrixain}\)，卡尔曼增益为：$$\BEGIN{等式}\LABEL{eq：kalgainunimplify}\color{紫色}{\mathbf{K}}=\color{深粉}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T}(\color{深粉}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^。我们可以将\(\eqref{kalunsimplfied}\)和\(\eqref{eq：kalgainunsimplfied}\)中的每个项的前面去掉\(\mathbf{H}_k\)(请注意，其中一个隐藏在\(\color{紫色}{\mathbf{K}}\)中)，和公式中所有项末尾的{\mathbf{P}_k‘}}(\color{royalBlue}{\mathbf{P}_k’}\)之间的一个\(\mathbf{H}_k^T\)。$$\BEGIN{公式}\BEGIN{SPLIT}\COLOR{royalBlue}{\mathbf{\hat{x}}_k‘}&；=\color{Firhsia}{\mathbf{\hat{x}}_k}&；+&；\color{紫色}{\mathbf{K}‘}(\color{Yellowgreen}{\vec{\mathbf{z}_k}}-\color{紫红色}{\mathbf{H}_k\mathbf{\hat{x}}_k})\color{ro。

.