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 <distro> https://github.com/ccny-ros-pkg/imu_tools.git
注意:<distro>为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的运动。