卡曼滤波器实现多目标跟踪解析 1

1 定义

只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好的弄清楚究竟发生了什么，找出现象间不易察觉的相关性因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。

2 应用

比如跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

3 卡尔曼滤波眼里的行人跟踪问题

下面是图例公式的描述,初学同学可能有点蒙，建议去查一下卡尔曼滤波相关视频（然后发现还是蒙/坏笑/,但每次的学习都会有新的发现/加油/）

下面是UP学习了不错文章作者Bzarg

一个包含位置信息和速度信息的状 x=（p,v）

卡尔曼滤波假设两个变量（在我们的例子里是位置和速度）都应该是随机的，而且符合高斯分布。如下图

位置和速度是不相关的，这意味着我们不能从一个变量推测另一个变量。那么如果位置和速度相关呢？如下图所示，人前往特定位置的可能性取决于它拥有的速度。

这不难理解，如果基于旧位置估计新位置，我们会产生这两个结论：如果速度很快，人可能移动得更远，所以得到的位置会更远；如果速度很慢，人就走不了那么远。

这种关系对目标跟踪来说非常重要，因为它提供了更多信息：一个可以衡量可能性的标准。这就是卡尔曼滤波的目标：从不确定信息中挤出尽可能多的信息！

为了捕获这种相关性，我们用的是协方差矩阵。简而言之，矩阵的每个值是第i个变量和第j个变量之间的相关程度（由于矩阵是对称的，i和j的位置可以随便交换）。我们用表示协方差矩阵，在这个例子中，就是

。

为了把以上关于状态的信息建模为高斯分布（图中色块），我们还需要k时的两个信息：最佳估计

（均值，也就是

，协方差矩阵

。（虽然还是用了位置和速度两个变量，但只要和问题相关，卡尔曼滤波可以包含任意数量的变量）

接下来，我们要通过查看当前状态（k-1时）来预测下一个状态（k时）。这里我们查看的状态不是真值，但预测函数无视真假，可以给出新分布：

我们可以用矩阵

表示这个预测步骤：

它从原始预测中取每一点，并将其移动到新的预测位置。如果原始预测是正确的，系统就会移动到新位置。这是怎么做到的？为什么我们可以用矩阵来预测人下一刻的位置和速度？下面是个简单公式：

换成矩阵形式：

这是一个预测矩阵，它能给出人的下一个状态，但目前我们还不知道协方差矩阵的更新方法。这也是我们要引出下面这个等式的原因：如果我们将分布中的每个点乘以矩阵A，那么它的协方差矩阵会发生什么变化

把这个式子和上面的最佳估计

结合，可得：

外部影响

但是，除了速度和位置，外因也会对系统造成影响。比如模拟火车运动，除了列车自驾系统，列车操作员可能会手动调速。在我们的机器人示例中，导航软件也可以发出停止指令。对于这些信息，我们把它作为一个向量

，纳入预测系统作为修正。假设油门设置和控制命令是已知的，我们知道火车的预期加速度a。根据运动学基本定理，我们可得：

把它转成矩阵形式：

是控制矩阵，

是控制向量。如果外部环境异常简单，我们可以忽略这部分内容，但是如果添加了外部影响后，模型的准确率还是上不去，这又是为什么呢？

外部不确定性

但是，如果存在我们不知道的力量呢？当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和“世界”相关的所有不确定性：

如上图所示，加上外部不确定性后，

的每个预测状态都可能会移动到另一点，也就是蓝色的高斯分布会移动到紫色高斯分布的位置，并且具有协方差

。换句话说，我们把这些不确定影响视为协方差

的噪声。

这个紫色的高斯分布拥有和原分布相同的均值，但协方差不同。

我们在原式上加入

新的最佳估计是基于原最佳估计和已知外部影响矫正后得到的预测

新的不确定性是基于原不确定性和外部环境不确定性得到的预测