点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达
介绍
卡尔曼滤波器是一种复杂的算法，在大多数情况下，我们在没有完全理解其方程的情况下使用它。
当我开始使用卡尔曼滤波器时，我也是这样做的。我阅读了一堆试图直观地解释算法的教程，但在这样做时，忽略了关于协方差矩阵作用的关键部分。
我正在开发的产品是基于检测的对象跟踪器。在其中，卡尔曼滤波器用于预测和更新给定视频流的对象的位置和速度，以及对每一帧的检测。在某些时候，我们还想通过在给定运动角度的情况下调整对象的位置来考虑相机的倾斜和平移。当然，物体的速度应该不受这种相机运动的影响。
但是，当开始实施这种调整时，我意识到我并不真正了解速度是如何估计的。我假设在这个过程中的某个地方，在给定先前和当前位置的情况下会明确计算速度（在我们的例子中，速度永远不会直接测量）。我还假设以与位置相同的方式处理此速度。令我惊讶的是，这两个假设都被证明是错误的。
这使我深入研究了卡尔曼滤波器的方程，试图尽可能直观地弄清楚到底发生了什么，但没有过度简化。本文是我在这个过程中学到的东西的总结。本文中我们需要小伙伴熟悉概率论中的基本概念，例如概率分布函数、高斯和贝叶斯定理。此外，我们将使用矩阵乘法和加法进行一些计算。因此，如果小伙伴也熟悉这一点，那就更好了。
概述
卡尔曼滤波器是一种算法，旨在随着时间的推移估计测量变量的值，给定这些变量的连续测量值以及这些测量值的不确定性。卡尔曼滤波器还考虑了估计变量之间的给定关系。
让我们以在视频中跟踪乒乓球的过程为例，给定球的 2D 边界框检测。检测可能在给定 bbox 的坐标中或由误报/负数引起的坐标中包含一些不准确之处。
为了更顺畅地跟踪球，我们将要跟踪测量的中心坐标 ( x , y )、bbox 的宽度和高度。假设移动球的速度可以在短时间内近似为常数，我们还希望跟踪 x 和 y 速度（v_x，v_y），希望得到这些预测：
通常，当试图解释卡尔曼滤波器时，人们会使用一个例子来跟踪一个物体，使用它的位置和速度的测量值（例如 GPS 和速度计）。在我们的示例中，我们没有直接测量速度。相反，卡尔曼滤波器使用测量的位置和我们注入其方程的先验知识来推断这个速度，这些知识与我们期望的运动学行为有关（在这种情况下是恒定的速度）。
为简单起见，从现在开始我们将只讨论x和v_x 的跟踪（一维跟踪示例）。在了解 1D 中发生了什么之后，逻辑可以很容易地扩展到 2 维和 3 维，因为在我们的示例案例中，每个维度都完全独立于其他维度。
假设
使用卡尔曼滤波器时有两个重要的假设：
传感器有噪声，其输出和噪声可以通过高斯概率分布函数 (PDF)准确建模。
我们对实际状态的初始先验知识也是一个高斯 PDF。
在我们的示例中，传感器输出由一维高斯表示，其中平均值等于实际传感器输出，方差是传感器的不确定性（主要是根据经验测量）。
先验高斯是二维的（一维表示位置，一维表示速度）。可以使用 6 个数字完全描述 2D 高斯：2 表示其中心坐标向量（也称为均值），另外 4 表示 2x2 协方差矩阵。
协方差矩阵的对角线对应于每个维度的方差，2个非对角线元素是位置和速度之间的实际协方差。由于协方差的对称性，非对角线元素总是相同的，即 x 和 y 之间的相关性与 y 和 x 之间的相关性相同（稍后我们将详细介绍该元素）。
卡尔曼滤波器将尝试使用输入检测更新均值和协方差。常用术语是将此分布称为系统状态。
在我们的示例中，我们将使用以下初始状态：
平均位置μ_x取自第一个检测到的 bbox 的 x 位置，我们假设一些初始速度μ_v。我们还假设最初速度和位置之间没有协方差。我们可以用图形绘制这个初始状态，如下所示：
算法步骤
为了发挥它的魔力，卡尔曼滤波器有两个步骤：预测，其中它尝试在给定先前状态和此后经过的时间的情况下预测当前状态。更新，它尝试将预测状态与传入的测量值结合起来。让我们使用我们的示例来看看这是如何发生的。
预测步进方程
为了在给定时间的情况下预测下一个状态，卡尔曼滤波器使用状态转换矩阵 ( F )，（也称为物理模型）将前一个状态与当前状态相关联。在我们的例子中，由于我们假设在很短的时间间隔内（大约几帧）的速度恒定，状态转移矩阵是：
这样，如果我们将它与我们的状态均值向量相乘，我们就会得到预测均值：
预测新的平均值是不够的。我们预计，如果时间过去了，那么协方差矩阵（表示状态的不确定性）也会发生变化。为了预测新的协方差，我们将使用随机变量的线性变换公式，并向其添加噪声对角矩阵。这种噪声称为“过程噪声”，它包含了自我们上次更新状态以来所经过的时间所产生的不确定性，并且在我们的物理模型中没有考虑到。所以在我们的例子中，它可以表示非恒定速度和相机运动。它通常用Q表示。所以协方差预测方程如下：
如果我们为我们的初始状态求解这个方程：
请注意，除了过程噪声增加的额外不确定性外，位置方差也增加了一个因子：
这个因素源于我们在将F（物理模型）定义为非对角矩阵时引入的位置和速度之间的依赖性。可以这样理解：假设预测位置是状态速度的函数，这个预测位置的不确定性是我们之前状态的不确定性加上速度的不确定性乘以它们耦合的因子（平方，单位）。
这里要注意的另一件事是新引入的协方差因子：
这意味着我们的高斯维度不再是独立的。这更容易以图形方式解释，所以让我们这样做。
预测步骤直觉
从图形上看，更新步骤如下所示：
那么这里发生了什么？为什么我们的州突然转向了一边？这是因为位置和速度不是两个独立的变量。状态的速度直接影响预测的位置。直观地说，这意味着更高的速度应该对应于更远的位置，反之亦然。在数学上，我们的状态转移矩阵 (F) 不是对角线这一事实表明了这一点。非对角元素 Δt 将位置和速度耦合在一起。
由于这种耦合，每个更新步骤都会在协方差矩阵中引入非对角元素，这些元素在图形上对应于我们分布的移动，如上面的 GIF 所示。
更新步骤直觉
这一步在数学上有点复杂，但直观上也很简单。在这一步中，卡尔曼滤波器将接收两个可能的状态（定义为高斯 PDF）作为输入，并输出一个结合了两者信息的新状态。
在深入研究实际描述此过程的方程式之前，要点如下：以统计上最优的方式组合来自两个（或多个）PDF 的信息，相当于将它们相乘。由于高斯分布的特殊性质，这种乘积是另一个具有均值和协方差的高斯分布，可以通过（相对）简单的方程获得。
现在了解更多详情。更新过程可以被认为是贝叶斯推理。如果我们将我们正在跟踪的对象的实际位置（我们实际上并不知道）定义为x，将实际测量的位置定义为m，我们将得到以下更新方程：
卡尔曼滤波器中使用的贝叶斯定理让我们在卡尔曼滤波器的上下文中理解这些术语：
后验：给定测量值的实际位置的概率分布。这就是我们想要使用更新步骤推断的内容。
可能性：给定实际位置的测量概率分布。假设这是围绕实际位置的高斯分布（如“假设”部分中所述）。
Prior：在测量之前（也就是预测状态），实际位置的概率分布到我们目前所知的最佳状态。这也被假定为最初的高斯分布。
归一化：测量概率分布，对所有可能的位置求和。在这种情况下，这部分没有计算，因为我们只关心更新均值和协方差，而不关心高斯的幅度。
从这里我们可以推断出更新后的状态将是预测状态和测量状态的乘积。因为我们在这里处理的是高斯分布，所以这种分布的乘积将是另一个高斯分布，其均值和协方差具有封闭方程。
如果我们对位置和速度都有测量（我们没有），那么更新步骤会像这样：
预测状态取自上述过程。测量状态，类似于我们的初始状态，具有轴对齐的椭圆形状。也就是说，测量的位置和测量的速度来自两个独立的传感器，每个传感器的不确定性由高斯方差捕获。
但是，因为在我们的视频跟踪示例中，我们只有位置测量，没有速度测量，所以测量状态的概率只会在位置维度上衰减，但在速度维度上，它会均匀分布，所以看起来有点像像这样：
在这里，小伙伴可以看到一个测量位置的平均值高于预测位置。更新后的状态的位置当然会介于两者之间。但请注意，虽然我们只测量了位置，但更新状态的平均速度也相对于预测状态有所增加。如果我们回想一下预测状态的位置和速度是相关的，就可以直观地理解这种速度变化，这意味着更远的位置与更高的速度相关联。鉴于这一特性，为什么测量某个位置会导致我们对平均速度的信念发生变化就变得很清楚了。
考虑两个高度相关的概率分布：例如，云量概率分布和降雨量概率分布。我们知道，大量的云与更多的降雨高度相关。现在假设我们不得不猜测昨天巴黎下了多少雨。在没有任何先验知识的情况下，小伙伴可能会猜到 3 毫米的降雨量（小伙伴猜测的平均值）。现在，有人告诉你昨天很阴天。使用这些先验知识，小伙伴肯定会将小伙伴对降雨的猜测更新到更高的数量。这正是卡尔曼滤波器中发生的情况。给定远位置测量值，我们还将速度更新为更高的值。
均值更新方程
在本博客中，我们仅展示它们并尝试理解它们。
首先是一些定义：
我们使用R来 表示测量的状态协方差矩阵，因为这是卡尔曼滤波器教科书中常用的表示方式。在这个例子中，由于我们的测量是一维的，测量的状态可以用两个标量表示：
因为我们的测量和状态具有不同的维度，要将两者结合起来，我们需要一个线性变换，将我们从状态空间带到测量空间。我们用矩阵H表示这种变换。在我们的例子中，我们的测量只有一维（位置），而状态有两个（位置和速度），所以我们可以使用：
这样，如果我们将预测均值与H相乘，我们将仅得到均值的测量部分：
现在来看看实际的方程式。我们将首先给出更新状态的均值和协方差的完整方程，然后将它们分解为更有意义的组件，以便我们更好地了解正在发生的事情。
完整更新的平均方程（对于我们的 1D 视频跟踪示例）是：
这是一个相当复杂的方程，所以让我们分解一下。首先，我们将传感器“创新”定义为：
这只是测量位置和平均预测位置之间的差异。它被称为创新，因为它可以被认为是我们从传感器获得的新（创新）信息。
其次，我们将“卡尔曼增益”定义为：
卡尔曼增益是我们将新传感器信息纳入更新状态的因素。
将这些定义代入我们得到的方程：
让我们进一步分解为位置更新和速度更新。对于更新后的位置，我们得到：
这个等式很有意义。更新的平均值将是预测平均值和测量平均值的加权平均值，权重因子与它们中的每一个的相对不确定性成正比。
因此，假设我们有一个零方差的完美传感器。这将导致预测的测量值等于传感器的测量值（应该如此——它是完美的。）。另一方面，如果传感器与预测状态的方差相比非常嘈杂（高不确定性），那么它将是相反的，完全忽略测量并且只使用预测位置。完全如小伙伴所料。
现在平均速度更新：
这种一般形式没有多大意义（至少不是我能想到的），所以让我们尝试将结果插入我们之前推导出的单个协方差预测步骤，看看是否有帮助。如果我们假设零过程噪声（Q=0，这是不现实的，但有利于更简单和更直观的方程），我们得到预测的协方差为：
请记住，“ t ”上标只是表示“上一步的”；插入我们得到的速度更新：
我们将分子和分母乘以 Δt 并得到：
让我们考虑一下这个等式的不同部分以及它们的含义。最后一部分：
是速度创新。就像位置创新一样，这是预测的状态速度与我们实际到达测量位置所需的速度之间的差异。现在，我们希望将多少创新融入到更新的状态中？嗯，它取决于我们先前状态下速度的不确定性相对于测量速度的不确定性。但是等等，我们没有测量速度！它是从位置测量中推断出来的。那么我们如何定义这个推断速度的不确定性呢？
速度是使用先前位置和测量位置推断出来的，因此它的不确定性将是其两个分量的不确定性之和是有道理的。因此，我们可以将推断的速度不确定性定义为：
如果我们回想一下：
我们还可以定义一个广义的预测速度不确定性，它也解释了自上次更新以来经过的时间：
将这些定义代入我们得到的方程：
这个等式更直观。它的意思是更新的速度将等于预测的速度加上速度创新的一部分。当推断的速度不确定性非常大（相对于预测速度）时，该分数将接近零，反之则接近 1。
协方差更新方程
完整的协方差更新方程如下所示：
我们可以插入上面获得的卡尔曼增益和以下事实：
要得到：
我们将相对测量不确定度定义为：
这个常数只是相对于位置总不确定度的测量不确定度。如果我们将其代入等式（在我将省略你的更多代数之后），我们得到：
让我们分解一下：更新后的位置不确定性是预测位置不确定性和测量位置不确定性的调和平均值。调和平均值是一种通过接近两个元素中较小的元素来组合两个元素的平均值。因此，如果：
然后：
反之亦然。
更新后的位置速度协方差也很直观。它是预测协方差的缩小版本，比例因子是相对测量不确定性。因此，再次，对于完美的测量，这些元素将接近于零，而对于嘈杂的测量，它们将保持不变。
现在是更新的速度方差。乍一看，这似乎没有多大意义，因此，就像我们之前所做的那样，让我们插入我们之前推导出的单个协方差预测步骤的结果。
我们可以再次使用我们上面定义的推断速度不确定性和广义速度预测不确定性来获得：
我们再次得到更新的方差是推断速度不确定性和广义速度预测不确定性的调和平均值。所以基本上总是接近两者中较小的一个。
最后，我们有：
让我们总结一下我们可以从这个等式中学到什么。位置方差和速度方差都可以表示为两个不确定性的调和平均值（每个不确定性都与它相关联）。位置速度协方差也随着我们相对于先验的测量确定性而缩小。如果小伙伴返回显示更新过程的图形，这也是非常直观的原因。
好消息！
小白学视觉知识星球
开始面向外开放啦👇👇👇
下载1：OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。
下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。
下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。
交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~