记录：robot_localization传感器数据融合学习

一、参考资料

1. 官方：

• http://wiki.ros.org/robot_localization
• http://docs.ros.org/en/noetic/api/robot_localization/html/index.html
• 2015 ROSCon 演讲官方网址（youyube上也有这个视频）
• ppt

2. 实践教程

• https://kapernikov.com/the-ros-robot_localization-package/
• https://github.com/methylDragon/ros-sensor-fusion-tutorial?tab=readme-ov-file

二、安装

mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/cra-ros-pkg/robot_localization --branch kinetic-devel
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release
source ./devel/setup.bash

出现CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake报错，执行sudo apt-get install ros-kinetic-geographic-msgs。

三、实践

(1) TurtleSim仿真
代码：https://github.com/Kapernikov/ros_robot_localization_tutorial

source /opt/ros/kinetic/setup.bash
cd /ros_robot_localization_tutorial/ros-ws
catkin_make
source ./devel/setup.bash

根据第一个教程，运行即可。如果报c++11的错误，CMakeLists中添加SET(CMAKE_CXX_FLAGS "-std=c++0x")。

编译通过以后，启动节点，出现TurtleSim界面，可通过键盘操作，结果已经可视化出来了。

roslaunch robot_localization_demo robot_localization_demo.launch

灰色粗线是TurtleSim节点画的轨迹。蓝色细线是虚拟位置传感器，测量乌龟的绝对位置和方向。红色细线是虚拟速度传感器，测量乌龟的线速度和角速度。绿色粗线是robot_location融合的位姿估计结果。
（2）bag实践
robot_localization官方代码中有3个测试bag：

robot_localization/test/test1.bag
robot_localization/test/test2.bag
robot_localization/test/test3.bag

第一个有GPS、里程计、IMU数据，后面两个只有里程计、IMU数据。bag info如下：

用第二个bag测试一下。编译安装robot_localization功能包以后，在ros工作空间下添加yaml和launch配置文件，执行launch文件即可调库。以下是我的工作空间结构：

在your_workspace_path/src/your_package_name/config/下添加ekf_localization_local.yaml文件，内容如下：

#Configuation for robot odometry EKF
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
publish_tf: true

# -------------------------------------
# Wheel odometry:

odom0: /jackal_velocity_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true,  true,  true,
               false, false, false,
               false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false

# Complete the imu0 configuration
# imu configure:

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  false,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

use_control: 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]

在our_workspace_path/src/your_package_name/launch/下创建ekf_localization_local.launch，内容如下：

<?xml version="1.0"?>
<launch>
    <!-- Run the ekf for map to odom config -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local">
        <rosparam command="load" file="$(find iplus_perception)/config/ekf_localization_local.yaml" />
        <remap from="/odometry/filtered" to="/odometry/filtered" />
    </node>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find iplus_perception)/rviz/jl.rviz" />
</launch>

rviz文件没有可以删除，注意功能包的名字，注意odom和imu话题要与bag一直，这里test2.bag是/jackal_velocity_controller/odom和/imu/data。
编译工作空间，编译通过以后，启动launch文件，rostopic echo //odometry/filtered，有输出，则运行成功。

roslaunch your_package_name ekf_localization_local.launch

上图用rqt_multiplot绘制了里程计的xyz和融合后的xyz。
rviz可视化。。。待补充。。。

四、理解

robots_localization 包是用于在 3D（或 2D）空间中移动的机器人的非线性状态估计器的集合。每个状态估计器都可以融合任意数量的传感器（IMU、里程计、GPS ……）来跟踪 15 维（$x$、$y$、$z$、$roll$、$pitch$、$yaw$、$\dot{x}$、$\dot{y}$、$\dot{z}$、$\dot{roll}$、$\dot{pitch}$、$\dot{yaw}$、$\ddot{x}$、$\ddot{y}$、$\ddot{z}$) 机器人的状态。

。。。待写。。。