ROS组合导航笔记1：融合传感器数据

使用机器人定位包（robot_localization package）来合并来自不同传感器的数据，以改进机器人定位时的姿态估计。

基本概念

在现实生活中操作机器人时，有时我们需要处理不够准确的传感器数据。如果我们想要实现机器人的高精度定位，我们需要尽可能获得最准确的传感器数据。为此，一个解决方案是将来自不同传感器的数据进行融合。我们拥有的数据越多，机器人就能越好地感知周围的世界。这时候，robot_localization 包就显得非常有用！

robot_localization 包的一个主要优点是能够融合来自多个传感器的数据。实际上，只要你有足够的耐心把它们放在机器人上，你甚至可以融合来自无限多个传感器的数据！

为了融合传感器数据，它使用了卡尔曼滤波器。实际上，它提供了两种不同类型的滤波器：

扩展卡尔曼滤波器 (EKF)
无迹(损)卡尔曼滤波器 (UKF)

在实际操作中，你可以使用其中任何一种，因为它们最终会提供类似的结果。下面的图片简单展示了这两种滤波器的概况：

在蓝色中，你可以看到机器人的真实状态。
在红色中，你可以看到基于传感器读数的机器人状态，这些读数在这个例子中有些噪声。
在绿色中，你可以看到应用卡尔曼滤波器后得到的机器人状态，这个状态与真实状态更为接近。

步骤 0：为我们的传感器添加噪声

当然，这不是一个必需的步骤。我们这样做只是为了模拟一个传感器数据不够可靠的情况，并观察如何通过使用 robot_localization 包来改进读数。

Exercise 1.1

首先，让我们查看当前接收到的里程计数据。为此，我们将使用 RViz。现在，启动它。
rosrun rviz rviz
首先我们给odom设置固定坐标系

现在让我们添加一个显示来可视化里程计数据。

您可能还想添加 RobotModel 显示器来可视化您的机器人。

现在，只需设置您想要从中读取里程计数据的主题。在本例中，它是 /odom。您应该得到类似这样的结果。

现在让我们稍微移动机器人，看看我们的里程计数据有多可靠。您可以使用以下命令让机器人绕圈移动：
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.5
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5"
转到 RViz 屏幕并检查里程表数据。您应该会看到类似这样的内容。

如您所见，在这种情况下，里程计数据非常可靠。那么...让我们对其进行调整！为此，我们将使用以下 Python 脚本：
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
import random

class NoisyOdom():
    
    def __init__(self):
        # 订阅 /odom 话题，接收 Odometry 消息
        self.odom_subscriber = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        # 发布到 /noisy_odom 话题，发送带噪声的 Odometry 消息
        self.odom_publisher = rospy.Publisher('/noisy_odom', Odometry, queue_size=1)
        # 创建 Odometry 消息实例
        self.odom_msg = Odometry()
        # 控制 shutdown 状态的标志
        self.ctrl_c = False
        # 注册 shutdown hook
        rospy.on_shutdown(self.shutdownhook)
        # 设置发布频率为 5 Hz
        self.rate = rospy.Rate(5)
    
    def shutdownhook(self):
        # 在 ROS 关闭时执行的回调函数
        self.ctrl_c = True

    def odom_callback(self, msg):
        # 处理接收到的 Odometry 消息并调用 add_noise() 函数
        self.odom_msg = msg
        self.add_noise()
    
    def add_noise(self):
        # 向 Odometry 消息的 Y 位置值添加噪声
        rand_float = random.uniform(-0.5,0.5)
        self.odom_msg.pose.pose.position.y = self.odom_msg.pose.pose.position.y + rand_float
        
    def publish_noisy_odom(self):
        # 循环发布带噪声的 Odometry 值
        while not rospy.is_shutdown():
            self.odom_publisher.publish(self.odom_msg)
            self.rate.sleep()
            
if __name__ == '__main__':
    # 初始化 ROS 节点
    rospy.init_node('noisy_odom_node', anonymous=True)
    # 创建 NoisyOdom 类的实例
    noisyodom_object = NoisyOdom()
    try:
        # 执行发布带噪声的 Odometry 消息的函数
        noisyodom_object.publish_noisy_odom()
    except rospy.ROSInterruptException:
        # 捕获 ROS 中断异常
        pass
基本上，上述代码会在发布到 /odom 主题的数据中添加一些随机噪声。具体来说，它会添加到位置的 Y 分量中。然后，它将这些新数据发布到一个名为 /noisy_odom 的新主题中。

所以，创建一个新的 ROS 包，命名为 noisy_odom，并将上述脚本添加到其中。

现在，启动代码，并在 RViz 中检查现在的里程计数据是什么样的。

注意： 在可视化这些带噪声的里程计数据时，你应该禁用协方差，因为它会快速增长。(不禁用画面看不清)

你应该得到如下结果：

您还可以再次移动机器人来查看现在的里程表数据。

如您所见，里程计现在不太可靠。

太好了！既然里程计数据已经损坏……让我们修复它吧！

END Exercise 1.1

步骤 1：启动 robot_localization 包

现在，我们已经弄乱了里程计数据……是时候修复它了！为此，正如您所知，我们将使用 robot_localization 包。

让我们创建一个新的 ROS 包，我们将称之为 turtlebot_localization。
roscd; cd ../; cd src;
catkin_create_pkg turtlebot_localization rospy
我们将启动 ROS 节点的启动文件添加到我们的包中。您可以查看下面的启动文件：

start_ekf_localization.launch
<launch> 

    
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
        <rosparam command="load" file="$(find turtlebot_localization)/config/ekf_localization.yaml"/>
    </node>

</launch>
如你所见，这个过程非常简单。基本上，我们在这里启动的是名为 ekf_localization_node 的 ROS 程序（它使用扩展卡尔曼滤波器），这个程序来自 robot_localization 包。然后，我们加载一系列参数，这些参数存储在名为 ekf_localization.yaml 的配置文件中。我们将在本章后面填充这个文件。在此之前，让我们先了解几个你需要知道的概念：

步骤 1.1：了解reference frames

robot_localization 节点需要四种不同的坐标系才能正常工作：

base_link_frame：这是机器人自身的坐标系，任何传感器都可以以这个坐标系为参考。它通常位于机器人的中心，随着机器人一起移动。
odom_frame：这是用于报告里程计数据的坐标系。
map_frame：这是用于从已知机器人的位置的系统（例如 AMCL 系统）报告全局位置的坐标系。如果你没有使用任何外部定位系统，这个坐标系可以忽略。
world_frame：这是用于确定在世界坐标系中机器人绝对坐标的两个坐标系中的哪一个。

这些概念可能会有点令人困惑，因此让我们做一个简单的例子来更好地理解这些概念。

首先，再次启动 RViz。接下来，我们将添加一个 TF 显示。

接下来，删除所有坐标系并只留下 base_link 和 odom坐标系。

此外，我建议将 RobotModel 中的 alpha 变量设置为 0.5 之类的值，这样您就可以更好地看到帧。

最后，你应该得到类似这样的结果：

从图中可以看出，两个坐标系的起点相同，即机器人的中心。但是，如果稍微移动机器人，您会看到 odom 坐标系开始分离。

Example

因此，例如，假设您没有任何外部定位系统，机器人的坐标系配置将如下所示：
base_link_frame: base_link
odom_frame: odom
world_frame: odom
步骤 1.2：添加要融合的传感器

现在是时候向 robot_localization 节点指示我们要融合的所有传感器了。该包接受以下类型的消息：

nav_msgs/Odometry
sensor_msgs/Imu
geometry_msgs/PoseWithCovarianceStamped
geometry_msgs/TwistWithCovarianceStamped

任何生成这些格式消息的传感器都可以输入到 robot_localization 包中，以估计机器人的位置。

最重要的是，你可以有多个相同类型的传感器。这意味着，例如，你可以有由两个不同传感器（如轮编码器和视觉里程计）提供的里程计数据，或者两个不同的惯性测量单元（IMU）。

你可以查看下图：

navsat_transform_node: 卫星导航转换节点 (NavSat Transform Node)

Wheel Encoders: 轮式编码器

Barometer: 气压计

如你所见，只要传感器使用上述指定的消息类型，你就可以将任何类型的传感器输入到 robot_localization 节点中。

你输入的每个传感器都必须在参数文件中按类型及其顺序编号进行指示，编号从零开始。此外，你还必须为每个传感器指定它将从哪个主题获取数据。格式如下所示：
odom0: /odom
odom1: /visual_odom
odom2: /laser_odom
    
imu0: /front_imu
imu1: /back_imu
在上述示例中，我们使用了 3 个不同的里程计源和 2 个不同的 IMU 源。

现在，对于每个输入传感器，我们必须指定其消息中的哪些变量将被融合（合成）在卡尔曼滤波器中，以计算最终的状态估计。为此，你需要填写一个 3x5 的矩阵。这个矩阵的含义如下：

行代表要融合的变量的类别，通常包括位置（Position）、速度（Velocity）和姿态（Orientation）。
列代表传感器类型或消息的特定部分，比如位置（X, Y, Z）、速度（X, Y, Z）以及姿态（Roll, Pitch, Yaw）。

矩阵的具体格式通常如下：
[  X,        Y,       Z
  roll,    pitch,    yaw
  X/dt,     Y/dt,    Z/dt
 roll/dt, pitch/dt, yaw/dt
  X/dt2,    Y/dt2,   Z/dt2]
上述矩阵的值含义如下：

X, Y, Z：这些是机器人的 [x, y, z] 坐标。
roll, pitch, yaw：这些是机器人姿态的滚转（roll）、俯仰（pitch）和偏航（yaw）角度。
X/dt, Y/dt, Z/dt：这些是机器人的速度。
roll/dt, pitch/dt, yaw/dt：这些是机器人的角速度。
X/dt², Y/dt², Z/dt²：这些是机器人的线性加速度。

你必须为每个传感器指定一个这样的矩阵。矩阵的值是布尔值（true 或 false），表示是否希望卡尔曼滤波器考虑这些特定值。

以下是一个示例矩阵的配置：

Example
odom0_config: [false, false, false,
               false, false, false,
               true,  true, false,
               false, false, true,
               false, false, false]
    
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              true, false, false]
在这种情况下，我们对卡尔曼滤波器考虑的值如下：

对于里程计数据：X 和 Y 方向的线速度，以及 Z 方向的角速度。
对于 IMU 数据：偏航角（yaw）、Z 方向的角速度，以及 X 方向的线性加速度。

但你可能会问……为什么选择这些值？为什么不使用，例如，位姿数据？这是一个很好的问题！我们来尽可能简单地解释一下。

首先，我们来查看一下一个里程计消息，以及它包含哪些数据。为此，你可以执行以下命令：
rostopic echo /odom
header:
  seq: 2560
  stamp:
    secs: 128
    nsecs:  51000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: 0.00114332573697
      y: 3.54915309929e-05
      z: -0.000247248017886
    orientation:
      x: 0.000385212901903
      y: -0.00720416134491
      z: 5.46923776386e-05
      w: 0.999973974001
  covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0,0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
  twist:
    linear:
      x: -5.97116118048e-05
      y: 0.000328280635543
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.000377385608995
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
正如你在消息中看到的，里程计提供了与位姿相关的数据：

位置：X, Y, Z。
姿态：x, y, z, w。正如你所看到的，这个姿态不是以经典的滚转（roll）、俯仰（pitch）、偏航（yaw）单位表示，而是以四元数表示。你也可以查看如何将四元数转换为 rpy，这里有相关的说明：将四元数转换为rpy。

它还提供了与速度相关的数据：

线速度：X, Y, Z。
角速度：X, Y, Z。

所以，通过这些数据，我们覆盖了矩阵中的几乎所有变量，除了加速度。
[  X,        Y,       Z
  roll,    pitch,    yaw
  X/dt,     Y/dt,    Z/dt
 roll/dt, pitch/dt, yaw/dt

]
因此，我们可以做的第一件事就是将所有加速度设置为 false，因为我们没有从里程计中获得有关它们的任何数据。
[  ?,     ?,    ?
   ?,     ?,    ? 
   ?,     ?,    ? 
   ?,     ?,    ?
 false, false, false
]
接下来我们看一下 rpy 轴：

很明显，我们的机器人只能在偏航轴上旋转。因此，我们将横滚角和俯仰角设置为 false。
[  ?,     ?,    ?
 false, false,  ? 
   ?,     ?,    ? 
   ?,     ?,    ?
 false, false, false
]
既然我们知道机器人使用差分驱动系统，并且它在一个 2D 环境中移动（不能像无人机那样上下移动），这意味着机器人只能在 X 轴上进行线性运动，或在 Z 轴上进行旋转运动。因此，唯一真正重要的速度是 X 方向的线性速度和 Z 方向的角速度。

这将导致我们使用以下矩阵：
[  ?,     ?,    ?
 false, false,  ? 
   ?,   false, false 
 false, false,    ?
 false, false, false
]
好的，让我们最后分析一下。在大多数情况下（包括这种情况），里程计数据是通过轮式编码器生成的。这意味着其速度、方向和位置数据都是从同一来源生成的。因此，在这种情况下，我们不希望使用所有的值，因为这会向滤波器输入重复的信息。相反，最好只使用速度数据。

因此，我们得到的最终矩阵如下：
[false, false, false
 false, false, false
   ?,   false, false 
 false, false,    ?
 false, false, false
]
然后，将左值设置为 true，我们将得到以下矩阵：
[false, false, false
 false, false, false
 true,  false, false 
 false, false, true
 false, false, false
]
重要说明

太棒了！但还有一件重要的事情需要讨论。如果里程计消息报告 Y 方向的线性速度为 0（并且其协方差值没有被设置为一个很大的值），那么最好将这个值输入到滤波器中。因为在这种情况下，0 值表示机器人在 Y 方向上无法移动，这作为一个测量值是完全有效的。

从我们里程计消息的 twist 数据中可以看到，这正是我们的情况：
twist:
    linear:
      x: -5.97116118048e-05
      y: 0.000328280635543
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.000377385608995
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
因此，即使它总是报告 0 值，我们也会将该值报告给过滤器。所以，最后，我们将得到一个如下所示的矩阵：
[false, false, false
 false, false, false
 true,  true,  false 
 false, false, true
 false, false, false
]
不过，您现在可能会问，为什么我们不对 Z 轴上的线速度做同样的事情呢？对吧？好吧，那是因为我们实际上将使用另一个名为 two_d_mode 的变量来执行此操作，您稍后会看到它。

Exercise 1.2

按照相同的流程填充与 IMU 数据相关的矩阵。

在此模拟中，IMU 数据发布在以下主题中：
/imu/data
End Exercise 1.2

步骤 1.3：转换

如果传感器未在 world_frame 的 base_link坐标系中发布数据，则必须注意在传感器坐标和 base_link 坐标之间提供有效的 tf 变换。

在我们的示例中，我们需要 Imu 传感器的变换。在这种情况下，通常在所有模拟中，此变换由 robot_state_publisher 提供。要检查变换是否确实存在，您可以执行以下命令：
rostopic echo -n1 /imu/data
这将给我们如下的输出：
header:
  seq: 0
  stamp:
    secs: 16
    nsecs: 434000000
  frame_id: "base_footprint"
orientation:
  x: 0.000384902845417
  y: -0.00720917811173
  z: 5.93260650974e-07
  w: 0.999973939461
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: -0.000515799661041
  y: -0.00184439123128
  z: 7.34538395099e-06
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: 1.17275409288e-05
  y: -1.72326132328e-06
  z: -3.71401775708e-05
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
从这里我们可以看到我们的 Imu 传感器的参考系是 base_footprint。现在，我们可以使用以下命令检查机器人的 Frames Tree：
roscd; cd ../; cd src;
rosrun tf view_frames
此命令将在我们的工作区中生成一个包含机器人坐标系树的 PDF 文件。您可以下载它并将其可视化。您将得到类似以下内容的内容：

正如你所看到的，我们确实有从 base_footprint 坐标系到 base_link 坐标系的变换，这由 robot_state_publisher 提供。因此，请记住，如果你的传感器没有提供这种变换，你需要自己提供。

太棒了！现在我们已经解释了设置 robot_localization 节点所需了解的主要部分，让我们实际完成我们的配置文件。

Exercise 1.3

既然我们已经正确解释了您需要为 robot_localization 节点配置的所有细节，让我们实际创建配置文件。首先，在 turtlebot_localization 节点内创建一个名为 config 的新文件夹。
roscd turtlebot_localization
mkdir config
在此文件夹中，创建名为 ekf_localization.yaml 的配置文件。在此文件中，您将放置以下配置。完成所有带有问号 (?) 的参数。
#Configuation for robot odometry EKF
#
frequency: 50
    
two_d_mode: true
    
publish_tf: false

# Complete the frames section 
odom_frame: ?
base_link_frame: ?
world_frame: ?
map_frame: ?

# Complete the odom0 configuration
odom0: ?
odom0_config: [?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,
               ?, ?, ?,]
odom0_differential: false

# Complete the imu0 configuration
imu0: ?
imu0_config: [?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,
              ?, ?, ?,]
imu0_differential: false

process_noise_covariance": [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]


initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                      0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]
让我们解释一下与上述配置文件相关的一些参数，这些参数我们还没有看到：

frequency：这个值表示滤波器产生状态估计的频率，以赫兹（Hz）为单位。
two_d_mode：这个变量表示你的机器人是否在 2D 环境中工作。如果设置为 true，所有的 3D 位姿变量将被设置为 0。
publish_tf：这个变量，如果设置为 true，将会发布从 world_frame 到由 base_link_frame 指定的坐标系的变换。在我们的例子中，这个变换已经由 robot_state_publisher 发布，所以我们将其设置为 false。
sensor_differential：这个变量表示我们是否想使用速度而不是位姿。因此，它只适用于与位姿相关的矩阵中的值 [X, Y, Z, roll, pitch, yaw]。如果我们想使用位姿值，那么我们将其设置为 false。
process_noise_covariance：这个参数用于建模滤波算法预测阶段的不确定性。什么！！？基本上，它用于改善滤波器产生的结果。对角线上的值是状态向量的方差，包括位姿，然后是速度，最后是线性加速度。虽然设置它不是强制性的，但通过调整它可以获得更好的结果。总之，除非你是这个领域的专家，否则很难设置。因此，最好的方法是测试不同的值，看看它们如何改善或降低结果。
initial_state_covariance：估计协方差定义了当前状态估计中的误差。这个参数允许你设置矩阵的初始值，这将影响滤波器的收敛速度。你应该遵循的规则是：如果你正在测量一个变量，则 initial_estimate_covariance 中的对角线值应大于该测量值的协方差。例如，如果你测量的变量的协方差值为 1e-6，则将 initial_estimate_covariance 的对角线值设置为 1e-3 或类似的值。

如果你想了解更多关于这些参数的信息，你可以查看官方文档：官方文档

太好了！现在，我们已经准备好测试我们的卡尔曼滤波器的表现了。首先，确保我们已经运行了 noisy_odom 节点。
rosrun noisy_odom noisy_odom.py
接下来我们执行 EKF 定位节点：
roslaunch turtlebot_localization start_ekf_localization.launch
现在，让我们再次回到 RViz，以便直观地了解正在发生的事情。首先，我们将可视化我们生成的噪声里程表，就像您在本章开头所做的那样：

现在，让我们将过滤后的里程计可视化。如果你在 Shell 上执行 rostopic list，你会看到出现了一个新的里程计主题，名为 odometry/filtered。
rostopic list | grep odom
你会得到类似这样的结果：
/noisy_odom
/odom
/odometry/filtered
这个新的 odometry/filtered 是我们的 robot_localization 节点产生的滤波后里程计数据。让我们在 RViz 中可视化它！

为此，在 RViz 中添加另一个里程计元素，并执行以下操作：

首先，缩短由 /noisy_odom 主题产生的箭头的距离。你可以在 Shape -> Shaft Length 中调整。

现在，在新的里程表显示上，将箭头的颜色更改为蓝色，以便我们可以将其与嘈杂的里程表区分开来。最后你应该得到类似这样的结果：

如您所见，robot_localization 节点正在过滤 Odometry 主题的所有噪声，并且它正在生成更好的里程计数据。

我们还可以移动机器人以查看其性能：

如您所见，虽然噪声里程表（/noisy_odom）显示的数据非常不稳定，但过滤里程表（/odometry/filtered）更加精确和稳定。

END Exercise 1.3

重要说明

如果在机器人不位于世界的中心位置 ([0,0] 位置) 时启动 EKF 本地化节点，噪声里程计和滤波后的里程计将不会重合，你会看到如下情况：

这发生是因为当你启动滤波器时，机器人不在世界的 [0,0] 位置（中心）。由于你没有将机器人的 [X,Y] 位姿数据提供给滤波器（你只使用了速度数据），滤波后的里程计将始终从 [0,0] 位置开始，并且没有办法知道初始位置是否发生了变化。

记住你在里程计配置文件中的设置，其中你没有提供 [X,Y] 数据，只提供了线性速度：
odom0: /noisy_odom
odom0_config: [false, false, false, # X,Y,Z
               false, false, false,
               true,  true, false, # Linear velocities
               false, false, true,
               false, false, false]
如果你从 [0,0] 位置开始移动机器人，这种情况不会发生，因为滤波器将能够通过线性速度数据更新机器人的位姿。

另一种选择是手动更新初始位姿。你可以在滤波器配置文件 ekf_localization.yaml 的末尾添加以下内容：
initial_state: [1.0, 1.0, 0.0,
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0]
例如，如果你使用上述初始位姿配置，那么滤波器的初始位姿将是 [1, 1] 而不是 [0,0]。