原标题：状态估计：卡尔曼滤波器

来源：AI 研习社         链接：https://www.yanxishe.com/TextTranslation/1273

距上次发布文章已经有一段时间了，让我们进行这次的学习。

快速回顾上一篇文章，我开始讨论移动机器人和以及怎样用编程使系统能够找到从起点到目标点的轨迹。 使用Dijkstra算法（或任何其他路径规划算法）有助于在黑箱中解决这个问题，但这只解决了一小部分。 另一部分是关于使用控制器操作机器人沿着轨迹移动，然后利用各种传感器来估计机器人在该计划路径的位置。 状态估计不仅对于路径规划和轨迹跟踪重要，而且对于通过SLAM（实时定位和导航）探索新区域和构建地图的智能移动机器人来说非常重要，以便定位移动目标。 例子是跟踪道路上其他车辆的自动驾驶车辆。

卡尔曼滤波器不仅可以跟踪我们在空间中的位置，还可以跟踪环境中的其他移动物体，例如道路上的汽车。

图片来源：https://shaoanlu.wordpress.com/2017/05/07/vihicle-detection-using-ssd-on-floybhub-udacity-self-driving-car-nano-degree/

机器人跟踪感兴趣的物体与空间位置交互（或避免）是一项非常困难的任务，因为机器人必须每分钟从一系列传感器（如 IMU 惯性测量单元，物体跟踪摄像机和车轮编码器）进行多次实时采样操作。捕获器22是没有传感器是理想的，因此每个样本都有累积误差，即噪声，因此导致我们对机器人或其环境的“真实”状态有一个低置信度。这可能导致许多挑战，因为大多数机器人必须无碰撞地导航，同时与复杂的动态环境进行交互。在以前的机器人应用中，例如工厂中的机械臂，必须严格限制机械臂的操作环境来使得执行任务时的错误最小化。现代机器人专家想于生产没有环境约束的机器人，让它在更复杂的环境中能智能地作出反应并且正常运行。

钟形曲线

高斯函数（分布）是统计学中最常见的数学函数之一，并且在数学，科学，商业分析，AI等的其他分支中广泛应用。 大多数人都知道它们是钟形曲线，并且由于一系列原因非常方便：

1. 它们由两个参数组成，均值和方差，非常直观地计算和解释。

2. 它们具有非常有用的数学特性。 任何复杂的高斯分布都可以分解成更简单的高斯分布。

3. 它们是中心极限定理的很良好例子，指出如果样本量足够大且具有有限的方差阈值时，那么所有样本的均值将是整个总体的平均值。 基本上，如果你使用其中一些样本，并且它具有某些特征，这意味着它可能适用于总体。

1维高斯有两个参数，均值和方差，可以用数学方式表达：

                         标准一维高斯分布函数

上式被理解为“x的概率”，它是模型的变量，其中μ是分布的均值，σ²是方差，σ是标准差。 以下是一维高斯的几个例子：

          一维高斯分布的几个示例，说明改变均值和方差将如何移动和拉伸/压缩分布。 

                                                图片来源：Dan Lee

虽然一维高斯有广泛应用，但我们生活在一个多变量的世界中，因此我们需要探索多变量高斯。 多变量高斯遵循与一维高斯相同的钟形曲线形状，但现在想象它与周围有宽边缘的高帽子类似：

2维高斯分布，注意，如果从沿X或Y轴的正交方向观看，

它将看起来像两个单独的1维高斯分布。

多元高斯分布等式描述如下：

也可以写成：

其中 D 是维数（在上图中，它是2），x 是我们感兴趣的变量，μ 是均值向量，Σ 是协方差矩阵。 注意这些值中的一些是粗体的，表示是非标量变量。 作为一个快速练习，建议感兴趣的读者在上面的等式中使用 D 值等于1来简化它。 得到与上面显示的相同的一维等式。

上面有一项是 协方差矩阵 Σ，它包含沿对角线的方差项和非对角线的相关项。 协方差矩阵有一些特征。 协方差矩阵始终：

1. 是正定和对称的。

2. 有一个对角化属性：Σ 可以以 UDUᵀ 的形式分解。（D是对角矩阵）

协方差矩阵示例如下：

注意：

和上面的1维一样，多元高斯以均值 μ 为中心，μ 是高斯分布的中心和最高峰。 为了便于可视化，将在本节的剩余部分中查看2维高斯分布。 由于我们有一个2维示例，μ 矩阵的大小为2×1，我们的状态也将是2×1。 我们可以将其可视化如下：

2维高斯分布， 资料来源：宾夕法尼亚大学 Dan Lee

与1维分布非常相似，对均值矩阵的任何更改都会使高斯分布变为以空间中的该点为中心，如下图所示。 此图像还显示协方差矩阵将如何影响各成分方向上的高斯宽度。

高斯分布均值的变化会移动整个分布。 资料来源：宾夕法尼亚大学 Dan Lee

注意上面的示例，Σ 矩阵是单位矩阵。 意味着在 x 方向和 y 方向上的宽度都是一个单位。 如果我们将这些sigma值变成大于1，正如预期的那样，分布将在各成分方向上变得更宽。 如果它们变小，分布将变得更窄。 如果你考虑一下，这是有道理的。 因为协方差矩阵实际上只是一个方差矩阵，如果有大量数据，分布就会分散开来。 如果我们在一组数据中的变化很小，方差，反过来，协方差是非常简单的。

 协方差矩阵描述了高斯的宽度。 资料来源：宾夕法尼亚大学 Dan Lee

从上面要注意的是协方差矩阵没有非对角线项。 当非对角线项中有非零变量值时，高斯分布的俯视图不会保持圆形，但事实上，它会变成椭圆形，并且偏斜到一侧或另一侧。

二维协方差矩阵。 资料来源：宾夕法尼亚大学 Dan Lee

我将关注协方差矩阵一些感兴趣的属性：

• 协方差矩阵始终是对称的和正定的。

• 协方差矩阵具有对角化属性。 意味着协方差矩阵可以分解为 UDUᵀ  的形式，其中 D 是对角矩阵。

卡尔曼滤波器

让我们回到机器人技术，看看如何利用足够的统计数据来发挥优势。 机器人是非常复杂的动力系统，它汇集了大量的传感器，机械，算法和电子设备。 幸运的是，为了更容易实现控制并跟踪它们的状态，我们可以在解决问题的同时将机器人抽象化拆分。 采用页面失控理论，我们可以利用状态空间表示将机器人分解为系统状态和观察测量量。

机器人的状态是以动力系统感兴趣的任何测量量来建模的。 这些量就像位置，速度，加速度和空间方向 。注意，这是基于机器人的基础框架或我们正在跟踪的对象。 测量抽象是我们观察到的。 我们的传感器采集了什么，我们测量了什么？ 其中的例子是测量墙壁距离的距离传感器，旋转传感器跟踪角度变化，IMU为我们提供加速度变化，以及我们相机中物体的颜色。 我们使用来自这些传感器的测量值来更新所处的状态。卡尔曼滤波器（KF）真正做的是两件非常简单的事情：它们基于在之前和当前时间步骤进行的测量来更新我们对状态的当前信念  。 KF 还使用收集的数据来预测下一个状态对我们系统的影响。

卡尔曼滤波器视图

注意我如何使用术语“信念”，这是上面介绍的所有统计数据将重新发挥作用的地方。 我们通过使用所讨论系统的均值 μₜ 和协方差 Σₜ，在任何离散时间步 t 表示我们的信念。

卡尔曼滤波器可以应用于任何系统，给出以下三个标准：

1. 状态转换系统必须是线性的。 也就是说：ẋ= Ax + Bu +ε，如上面的链接所述，其中 x 是状态向量，u 是控制向量，A 和 B 分别是大小为nxn 和 nxm 的矩阵，n 是状态向量 x 的维数。，m 是控制向量u 的维数。 最后一项ε是高斯随机向量，它捕获由状态转换引入的不确定性。 它具有与均值为零且协方差为R的状态向量相同的维度。使用上面的多元高斯公式，我们可以说状态转移概率如下：  

   

   
   

2. 测量概率 p（z | x）也必须是线性的，加上高斯噪声：z = Cx +δ，其中 C 是 k×n 矩阵，其中 k 是测量矢量 z 的维数。 增加的传感器噪声向量δ 是多元高斯，其均值为零，协方差为 Q。我们可以再次应用相同的方程，并将测量概率建模如下：

3. 由于上述两个标准，我们可以假设信念也是正态分布。 然后我们可以说信念如下：

如果希望获得上述三个方程的完整证明以及卡尔曼滤波器的推导，可以在第3.2.4节中通过Sebastian Thrun在Probabilistic Robotics中找到。

卡尔曼滤波器采用我们的状态和测量的这些高斯模型，并帮助我们分别在均值和协方差 μₜ 和 Σₜ 的某个时间步 t 表示我们的信念。 为了计算当前时间步长的当前估计值，我们将使用先前的时间步长估计值作为输入：t，μₜ₋1 和 Σₜ₋1。 为了更新这些参数，卡尔曼滤波器将需要控制和测量值 uₜ 和 zₜ。 因此，我们可以通过算法将其写出来：

函数采用我们先前对均值和协方差的估计以及新的控制和测量向量。 然后用它来计算我们对所处位置的新估计以及预测我们的目标。

逐行深入，卡尔曼增益指定了我们允许将新测量值添加到状态估计器中的程度。 这非常有用，因为它可以控制滤波器并且不允许任何疯狂的跳跃。 它使事情变得平滑。 在计算卡尔曼增益之后，我们将其纳入状态估计以及实际测量的偏差（zₜ）以及对先前状态和预期测量（Cₜ）的预测。 一旦完成，我们将采用所有这些值并更新我们的协方差。 在算法的预测部分，我们计算出期望下一个时间步长均值和协方差应该是什么（因此是“条形符号”），这是我们填写A和B矩阵来计算它的地方。

在更新步骤期间，从计算的角度来看，卡尔曼滤波器也是一种有效的算法。正如在前面的示例中所看到的，它不是很长，也不是很复杂。这都是相对简单的数学。对于大多数矩阵库，d x d 矩阵的逆大致为 O（d ^ 2.4）。对于滤波器的每个循环，下限将是相似的，O（k ^ 2.4），其中 k是我们的测量矢量 zₜ 的维数。正如 Probabilistic Robotics 所指出的那样，在许多机器人应用中，如导航，这个滤波器的性能将由 O（n²）决定，因为测量空间远低于状态空间。请注意，预测步骤可能非常密集，但幸好有一些方法可以帮助最小化SLAM等情况下的成本，其中协方差需要AΣₜ₋1Aᵀ计算，其中 A 接近同一性。虽然这种特殊情况需要使用诸如稀疏矩阵乘法之类的东西来专门评估 AΣₜ₋1Aᵀ。

卡尔曼滤波器：（a）初始信念，

（b）测量（粗体）与相关的不确定性，

（c）将测量结果整合到一起后使用卡尔曼滤波算法的信念，

（d）向右运动后的信念（其中引入不确定性），

（e）具有相关不确定性的新测量，以及（f）由此产生的信念。

 资料来源：概率机器人，Thrun

在Python中使用卡尔曼

现在想象一个移动机器人在一条直线上移动。 如上所述，不存在理想的传感器，因此总会有一定程度的噪声（高斯类型），这将导致状态估计出错。 为了抵消来自传感器的噪音，我们可以使用卡尔曼滤波器来尝试跟踪机器人在X-Y平面上的实际轨迹，以确保沿着所需的路径移动。 为此，我们将使用一些Python。

如上一节所示，标准卡尔曼滤波器在软件中相对容易实现。 如果你在上一篇文章中遵循我的路径规划示例代码，那么只需使用：

$ git fetch $ git pull

如果没有，你可以通过运行以下命令轻松将其复制到计算机上的本地目录中：

$ git clone https://github.com/atomoclast/realitybytes_blogposts.git

然后它应该引入所有新特征。 然后，你将要导航到：

../realitybytes_blogposts/stateestimation/

运行

$ python kalman_filter.py

生成如下窗口：

示例代码的输出。 可以看到，对于机器人正在采用的示例路径，

卡尔曼滤波器在给定噪声数据的情况下提供其轨迹的合理估计。

在这个例子中，当机器人沿着它的路径移动时，我们进行了几次噪声测量，以及卡尔曼滤波器估计的实际轨迹。 预计的轨迹是相对线性的，这正是我们希望看到的。 代码本身相对简单。 它使用上面显示的相同线性方程和一些其他方程式来确保我们的数据格式正确并正确使用。 与上面相同，我们将与反馈相同的值返回到我们的估算中。 我认为过滤过程真的很酷，因为它使我们对机器人在我们认为的位置有高置信度。

准确的状态信息至关重要，因为我们试图将机器人从受控环境带入动态环境中我们无法控制或模拟的区域。 在这些新设置中，机器人必须通过探索和地图构建智能地本地化它们在世界中的位置，并返回到已知区域以执行各种任务。

我强烈建议你使用诸如起始状态和起始协方差矩阵之类的数据，以及各种轨迹，并了解滤波器如何反应。 你是否可以进行一些调整，使其对来回曲折的图案更加健壮？ 如果你最终用它做了什么，我很想看看你有什么发现？

扩展卡尔曼滤波器和ROS   

我之前曾说过，卡尔曼滤波算法的一个标准是我们要跟踪和估计的系统是线性的。不幸的是，这并不总是一种选择，因为我们的系统变得越来越复杂。线性系统的约束导致卡尔曼滤波器产生另外两种变化的发展：扩展卡尔曼滤波器和Unscented卡尔曼滤波器。 

扩展卡尔曼滤波器试图通过在当前时间步长处对均值和协方差的估计进行线性化来解决该问题。它在导航系统中非常流行，因此它是一种非常成熟的算法。如果您对某些基础数学感兴趣，我建议您仔细阅读上面的链接。目前，我将向您展示如何利用已有的ROS软件包来充分利用其功能。  

robot_pose_ekf是一个catkin包，是ROS导航堆栈的一部分。该软件包的真正强大之处在于它可以估算机器人在3D中的位置和方向，IMU和视觉测距的融合数据。对于像SLAM这样的机器人应用来说，这是非常有吸引力的，因为探测和地图建设在很大程度上依赖于我们对空间中的位置的估计。

               来自robot_pose_ekf包的实验结果。

               资料来源：http：//wiki.ros.org/robot_pose_ekf

在本节中，我们将使用Gazebo Simulation和TurtleBot来绘制Willow Garage的模拟世界（开发PR2的人和使ROS成为一种东西）。 如果你按照上面的例子，你已经克隆了我创建的名为turtlebot_ekf的Catkin包。我为ROS和Gazebo采用了几个现有的Turtlebot软件包，并将它们全部编译成几个简单的启动文件，以便于使用。 我建议将上面的git链接克隆到catkin工作区，然后构建和获取工作区。从那里开放四个终端并在前三个终端启动：  

$ roslaunch turtlebot_ekf spawn_turtlebot_willow_world.launch $ roslaunch turtlebot_ekf mapping.launch $ roslaunch turtlebot_teleop keyboard_teleop.launch

然后，您应该在其中一个房间中使用Turtlebot机器人为Willow Garage世界提供一个Gazebo模拟窗口。您还应该在下面打开一个RViz窗口，该窗口已经加载了正确的配置，以便可视化机器人以及它正在构建的地图。  

Gazebo Simulation和RVIZ窗口显示Turtlebot探索环境并构建地图。右下方的终端显示了robot_pose_ekf包，估计其相对于起点的空间位置。 在最终终端中，您将要运行：

$ rostopic echo /robot_pose_ekf/odom_combined

这将显示我们机器人姿势的直接流。重要的是要注意这是相对于机器人的起始位置。如果机器人要返回之前探索过的感兴趣的地图，那么它的参考测距帧将会有所不同。机器人将不得不使用来自环境的各种关键点将自己置于世界之中。虽然这是相关的，但它超出了这篇文章的范围。 我建议你使用第三个窗口并绕过机器人。您不仅会看到地图如何建立起来，而且当您多次通过区域时，它会建立信任，因为它相信特定的障碍存在。它还将显示如何使用EKF窗口中的相关协方差跟踪机器人的姿势：  

我鼓励你跑一下整个流程，并使用一些映射功能进行更多操作。如果你是特别好奇的读者，你应该查看一些源代码，看看在这个链接上如何实现算法。 

祝你玩的愉快！感谢你的阅读！

一THE END一

免责声明：本文来自互联网新闻客户端自媒体，不代表本网的观点和立场。

合作及投稿邮箱：E-mail:editor@tusaishared.com