（1）robot_pose_ekf扩展卡尔曼滤波功能包的使用方法

robot_pose_ekf功能包的编译安装

ros wiki社区有对该功能包的具体安装与使用说明，可通过该链接移步http://wiki.ros.org/robot_pose_ekf

Robot_Pose_EKF软件包用于基于来自不同来源的（部分）姿势测量值来估计机器人的3D姿势。它使用带有6D模型（3D位置和3D方向）的扩展卡尔曼滤波器，以结合车轮里程计，IMU传感器和视觉里程计的测量结果。基本思想是提供与不同传感器的松散耦合集成，其中传感器信号以ROS消息的形式接收。

如何使用机器人姿势EKF

可以在robot_pose_ekf软件包目录中找到EKF节点的默认启动文件。启动文件包含许多可配置的参数：

freq：过滤器的更新和发布频率。请注意，较高的频率会随时间推移为您提供更多的机器人姿态，但不会提高每个估计的机器人姿态的准确性。

sensor_timeout：当传感器停止向过滤器发送信息时，在没有该传感器的情况下，过滤器应等待多长时间才能继续运行。

odom_used，imu_used，vo_used：启用或禁用输入。

可以在启动文件中修改配置，如下所示：

<launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="true"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
 </launch>

编译运行robot_pose_ekf

 rosdep install robot_pose_ekf

 roscd robot_pose_ekf

rosmake

cd catkin_ekf

catkin_make

rosdep install robot_pose_ekf 自动安装该ros包的依赖

一般常见报错：找不到工作空间路径

解决方案：将路径添加到.bashrc文件中

source ~/catkin_ekf/devel/setup.bash

运行robot_pose_ekf launch文件

roslaunch robot_pose_ekf.launch

订阅的话题

odom（nav_msgs / Odometry）

2D姿态（用于车轮里程计）：2D姿态包含机器人在地平面中的位置和方向以及此姿态的协方差。发送此2D姿势的消息实际上代表3D姿势，但是z，roll和pitch都被忽略了。

imu_data（sensor_msgs / Imu）

3D方向（由IMU使用）：3D方向提供有关机器人基础框架相对于世界参考框架的横滚，俯仰和偏航角的信息。横滚角和俯仰角被解释为绝对角（因为IMU传感器具有重力参考），偏航角被解释为相对角。协方差矩阵指定方向测量的不确定性。仅接收到有关此主题的消息时，robotpose ekf不会启动；它还期望有关“ vo”或“ odom”主题的消息。

vo（nav_msgs / Odometry）

3D姿势（用于视觉Odometry）：3D姿势表示机器人的完整位置和方向以及该姿势的协方差。当传感器仅测量3D姿势的一部分时（例如，车轮里程表仅测量2D姿势），只需在3D姿势的未实际测量的部分上指定较大的协方差。

该robot_pose_ekf节点并不需要所有的三个传感器源可所有的时间。每个源都给出一个姿势估计和一个协方差。这些源以不同的速率和不同的延迟运行。源会随时间出现或消失，节点将自动检测并使用可用的传感器。要添加自己的传感器输入，请查看“添加GPS传感器”教程

发布的话题

过滤器的输出（估计的3D机器人姿态）
robot_pose_ekf/odom_combined(geometry_msgs/PoseWithCovarianceStamped)

提供的TF变换 rosrun rqt_tf_tree rqt_tf_tree 指令查看TF变换
odom_combined → base_footprint

　参数output_frame和base_footprint_frame默认为odom_combined和base_footprint，也可以根据需要在launch文件中进行更改，比如分别改为odom和base_link，再次用rqt_tf_tree命令查看，如下图所示：

　robot_pose_ekf节点默认会从odom、imu_data、vo这三个topic上订阅消息，可以使用remap将其映射到新名称的topic上。重映射是基于替换的思想，每个重映射包含一个原始名称和一个新名称。每当节点使用重映射中的原始名称时，ROS客户端库就会将它默默地替换成其对应的新名称。

 <launch>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom"/>
        <param name="base_footprint_frame" value="base_link"/>
        <param name="freq" value="30.0"/>
        <param name="sensor_timeout" value="1.0"/>
        <param name="odom_used" value="true"/>
        <param name="imu_used" value="true"/>
        <param name="vo_used" value="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>

        <remap from="imu_data" to="imu" />      <!-- 将节点订阅的imu_data话题改名为imu -->
    </node>
 </launch>

机器人姿态ekf如何工作

姿势解释

将信息发送到过滤器节点的所有传感器源都可以拥有自己的世界参考框架，并且这些世界参考框架中的每一个都可以随时间漂移。因此，不能将不同传感器发送的绝对姿势进行相互比较。该节点使用每个传感器的相对姿势差异来更新扩展的卡尔曼滤波器。

协方差解释

随着机器人的移动，在世界基准中其姿势的不确定性越来越大。随着时间的流逝，协方差将无限增长。因此，在姿态本身上发布协方差是没有用的，而是传感器源发布协方差如何随时间变化，即速度的协方差。请注意，使用对世界的观察（例如，测量到已知墙壁的距离）将减少机器人姿势的不确定性；但是，这是定位，而不是里程计。

定时

想象一下，机器人姿态过滤器是在时间t_0上一次更新的。直到每个传感器的至少一个测量值以晚于t_0的时间戳到达时，该节点才会更新机器人姿态过滤器。当如收到一条消息在Odom话题时间戳T_1> T_0，并在imu_data与话题时间戳T_2> T_1> T_0，过滤器现在将更新到其所有传感器的信息是可用的，在这种情况下，最晚时间到时间t_1。直接给出在t_1处的姿态，通过在t_0和t_2之间对imu姿态进行线性插值来获得t_1处的imu姿态。在t_0和t_1之间，用odom和imu的相对姿势更新了机器人姿势过滤器。

参考文章

robot_pose_ekf

robot_pose_ekf 使用说明

ekf pose使用方法 ros_使用robot_pose_ekf融合odom和imu数据