使用imu_tools对IMU进行滤波并可视化

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消息。

2 安装imu_tools

创建catkin工作空间：

mkdir -p ~/imu_tools_catkin_ws/src

安装git：

sudo apt-get install git-core

下载仓库到catkin工作空间中：

cd ~/imu_tools_catkin_ws/src
git clone -b  https://github.com/ccny-ros-pkg/imu_tools.git

注意：为ROS版本，比如：kinetic。

使用rosdep安装依赖：

rosdep install imu_tools

编译：

cd ..
catkin_make

3 使用

这里，我使用小觅相机深度版的IMU进行测试。

启动ROS驱动：

source ~/MYNT-EYE-D-SDK/wrappers/ros/devel/setup.bash
roslaunch mynteye_wrapper_d mynteye.launch

然后在终端打印IMU数据：

rostopic echo /mynteye/imu/data_raw

从上图可以看出，只输出了加速度（linear_acceleration）和角速度（angular_velocity），没有计算方位（orientation），而且也没有协方差（covariance）。

打开文件：
~/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为/mynteye/imu/data_raw，因此需要修改代码，使topic一致：

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

打开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>

运行imu_tools：

source ~/imu_tools_ws/devel/setup.bash
roslaunch imu_complementary_filter complementary_filter.launch

打开rviz：

rviz

在rviz中，点击【Add】，添加【rviz_imu_plugin】。

设置【Fixed Frame】为odom，设置【IMU】中的【Topic】为/imu/data。

IMU静止时，rviz中的模型也保持静止；转动IMU，rviz中的模型会显示IMU的运动。