1 前言

imu_filter_madgwick：一种滤波器，可将来自常规IMU设备的角速度，加速度和磁力计读数（可选）融合到一个方向中。基于工作：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

imu_complementary_filter：一种滤波器，它使用一种基于互补融合的新颖方法，将来自通用IMU设备的角速度，加速度和磁力计读数（可选）融合到方向四元数中。基于文献：http://www.mdpi.com/1424-8220/15/8/19302

rviz_imu_plugin：rviz插件，可显示sensor_msgs::Imu消息。

ROS org imu_tools官方介绍地址：http://wiki.ros.org/imu_tools

github源代码地址：https://github.com/ccny-ros-pkg/imu_tools

2 安装

From binaries
This repo has been released into all current ROS1 and ROS2 distros. To install, simply:

sudo apt-get install ros-<YOUR_ROSDISTO>-imu-tools

From source (ROS1)
Create a catkin workspace (e.g., ~/catkin_ws/) and source the devel/setup.bash file.

Make sure you have git installed:

sudo apt-get install git

Clone this repository into your catkin workspace (e.g., ~/catin_ws/src; use the proper branch for your distro, e.g., melodic, noetic, …):

git clone -b <YOUR_ROSDISTO> https://github.com/CCNYRoboticsLab/imu_tools.git

Install any dependencies using rosdep.

rosdep install imu_tools

Compile the stack:

cd ~/catkin_ws
catkin_make

3 查找自己的IMU话题

rostopic list

4 imu_tools滤波

修改imu_tools文件

打开文件：
~/imu_tools_ws/src/imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp，有如下代码：

// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));

可以看出，imu_tools订阅的topic为imu/data_raw，而IMU发布的topic为/imu_data，因此需要修改代码，使topic一致：

将imu订阅的话题改为自定义的话题名称即可：

// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, "/imu_data", queue_size));

然后，修改launch文件

打开launch文件：~/imu_tools_ws/src/imu_tools/imu_complementary_filter/launch/complementary_filter.launch，进行一些修改：

前半部分已省略
 
重点修改以下部分的内容
 
<!-- ComplementaryFilter launch file -->
<launch>
  #### Complementary filter
  <node pkg="imu_complementary_filter" type="complementary_filter_node"
      name="complementary_filter_gain_node" output="screen">
    <param name="do_bias_estimation" value="true"/>
    <param name="do_adaptive_gain" value="true"/>
    <param name="use_mag" value="false"/>
    <param name="gain_acc" value="0.01"/>
    <param name="gain_mag" value="0.01"/>
    <param name="publish_debug_topics" value="false"/>
    <param name="publish_tf" value="true"/>
  </node>
</launch>

重新编译：catkin_make, 然后 source ~/.bashrc

开一个终端运行imu_tools中的launch文件，若正常运行不报错则已经开始了滤波操作

roslaunch imu_complementary_filter complementary_filter.launch

在rqt中进行可视化

PS:在rqt_plot中添加想要可视化的数据时，格式为话题名称+对应的数据名称，例如/imu/data/orientation