姿态检测 树莓派_树莓派小车-06-IMU姿态解算 imu_complementray_filter_ros

由于内容较长，姿态解算将会分为2部分，本篇文章介绍其ros接口，下一篇文章介绍互补滤波器算法。

这篇文章要开始根据ROS的imu-tools中的互补滤波器的代码和论文，详细分析一下其设计思路和姿态解算的过程，为了简化过程，这里我只考虑使用加速度计和陀螺仪来进行解算的过程，并且未考虑零飘和误差模型，仅仅从滤波器本身的设计上进行分析。这一部分由于涉及的内容自己并不熟悉，因此也是边学习边记录，如果其中有任何错误的地方还请各位前辈多多指点。

首先我们先从代码入手，由apt install ros-melodic-imu-tools可以直接安装这个工具包，其中包含了2中滤波器以及一个imu显示的rivz插件，但是因为我们想从源码上查看因此可以在github上找到https://github.com/ccny-ros-pkg/imu_tools。其中互补滤波的代码在imu_complementary_filter文件夹中，而对应的论文可以在reference中找到http://www.mdpi.com/1424-8220/15/8/19302。

imu_complementray_filter中的代码也算简洁，不考虑include文件只看源文件，一共有3个：complementary_filter_node.cpp 这是最终封装好的ros node启动文件，内容非常简单因此不展开介绍了。

complementary_filter_ros.cpp 这是利用互补滤波器和ros topic进行数据交换的层面，内容主要是数据的转换，这里可以作为我们的入口开始研究。

complementary_filter.cpp 这是互补滤波器的具体实现，也就是和论文相关的部分。我们会在完成第二条的分析之后再进行。这一部分将在后续文章中详解。

本篇文章只介绍 complementary_filter_ros.cpp

下面进入正题，第一部分是对complementary_filter_ros.cpp的分析。首先是构造函数，其中定义了两个ros node，一个nh和一个nh_private。这个private的nodehandle恕我才疏学浅没有太理解，查看了一些资料也没有找到很明确地工程应用方面的原因，大致理解下来就是可以通过private nodehandle来获取roslaunch的参数而不需要管我这一个node里面到底使用了多少个nodehandle，可现在这个应用里只有一个nodehandle所以我理解应该是可以不考虑这个private node的了吧。

initializeParams()是解析roslaunch当中的参数；

该节点会发布名为/imu/data的话题；

由publish_debug_topics_参数来决定是否还要把一些用于调试的中间变量发布出去；

该节点会订阅一个名为/imu/data_raw的话题，同时根据是否使用地磁计use_mag_这个参数来决定是否会订阅/imu/mag话题。见下图。

ComplementaryFilterROS::ComplementaryFilterROS(

const ros::NodeHandle& nh,

const ros::NodeHandle& nh_private):

nh_(nh),

nh_private_(nh_private),

initialized_filter_(false)

{

ROS_INFO("Starting ComplementaryFilterROS");