ROS中的imu数据（sensor_msgs/imu）一般为机体坐标系下，如果后续需要进行卡尔曼滤波等处理，需要先将其转换至世界坐标系中。借助ROS中tf工具里面的转换矩阵功能，可以较为简单的实现这一目的。（tf官方教程中是教用broadcaster与listener，个人感觉在这里有点大材小用且复杂化了）

代码全文

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Vector3.h"
#include "tf/tf.h"

void imu_fix_callback(const sensor_msgs::Imu::ConstPtr& data){
    //将imu传感器中的四元数转换为tf工具中的四元数。顺序为x,y,z,w。
    tf::Quaternion q1={data->orientation.x,data->orientation.y,data->orientation.z,data->orientation.w}; 
    //通过四元数得到旋转矩阵
    tf::Matrix3x3 r1;
    r1.setRotation(q1);

    geometry_msgs::Vector3 acc_world;//构建向量用于储存转换后的数据
    //旋转矩阵右乘列向量获得转换后的向量
    acc_world.x=(r1[0][0]*data->linear_acceleration.x)+(r1[0][1]*data->linear_acceleration.y)+(r1[0][2]*data->linear_acceleration.z);
    acc_world.y=(r1[1][0]*data->linear_acceleration.x)+(r1[1][1]*data->linear_acceleration.y)+(r1[1][2]*data->linear_acceleration.z);
    acc_world.z=(r1[2][0]*data->linear_acceleration.x)+(r1[2][1]*data->linear_acceleration.y)+(r1[2][2]*data->linear_acceleration.z);
}


int main(int argc,char **argv)
{
    ros::init(argc,argv,"test");
    ros::NodeHandle nh;

    ros::Subscriber sub=nh.subscribe("/mavros/imu/data",100,imu_fix_callback);

    ros::spin();
    return 0;
}

代码比较简单，也带有注释，看到这里就看懂的就不需要继续往下看了。下面是代码更详细的解析与参考资料链接。

代码解析

int main(int argc,char **argv)
{
    ros::init(argc,argv,"test");
    ros::NodeHandle nh;

    ros::Subscriber sub=nh.subscribe("/mavros/imu/data",100,imu_fix_callback);

    ros::spin();
    return 0;
}

常规的消息订阅，没什么好讲的，刚入门的话建议去看官方教程。

    //将imu传感器中的四元数转换为tf工具中的四元数。顺序为x,y,z,w。
    tf::Quaternion q1={data->orientation.x,data->orientation.y,data->orientation.z,data->orientation.w}; 
    //通过四元数得到旋转矩阵
    tf::Matrix3x3 r1;
    r1.setRotation(q1);

quaternion是tf工具里面储存四元数的一个类，除直接输入x,y,z,w以外，也可以通过输入欧拉角进行设置（里面的setRPY函数）。tf::Quaternion Class Reference

matrix3x3是tf工具里面的3x3多功能矩阵，这里利用的是其通过四元数转换旋转矩阵的功能（setRotation）。详细官方文档在此：tf::Matrix3x3 Class Reference

//旋转矩阵右乘列向量获得转换后的向量
    acc_world.x=(r1[0][0]*data->linear_acceleration.x)+(r1[0][1]*data->linear_acceleration.y)+(r1[0][2]*data->linear_acceleration.z);
    acc_world.y=(r1[1][0]*data->linear_acceleration.x)+(r1[1][1]*data->linear_acceleration.y)+(r1[1][2]*data->linear_acceleration.z);
    acc_world.z=(r1[2][0]*data->linear_acceleration.x)+(r1[2][1]*data->linear_acceleration.y)+(r1[2][2]*data->linear_acceleration.z);

这里的作用就是旋转矩阵与向量相乘。对应的矩阵运算为：

$\begin{bmatrix} r_{00} &r_{01} &r_{02} \\ r_{10}& r_{11} & r_{12} \\ r_{20}&r_{21} &r_{22} \end{bmatrix}*\begin{bmatrix} x\\ y\\z \end{bmatrix}$