之前博客《 ROS学习笔记之——机器人航向角的求解》介绍了怎么求机器人的航向角，但是该求法均为相对于初始点的求法。若一开始机器人的初始姿态角未知，则需要采用相应的传感器来计算初始姿态角
目录
基本概念
传感器：都是基于机器人坐标系的。加速度计测三轴的加速度，磁力计测三轴的磁感应强度，陀螺仪测三轴的角速度。注意角速度的定义，方向是沿着三轴的。得到姿态角主要靠陀螺仪测得的角速度积分，但存在漂移误差，因此要用加速度计和磁力计随时校正。
在地理坐标系下，重力加速度坐标始终为（0,0,1g），磁场坐标为（M,0,D）。在地球表面，磁场始终是沿着磁感应线指北的，某点的磁场强度为磁感应线在该点的切线，它在北方有个分量，在垂直方向有个分量，即（M,0，D）。这里还有一点，地理的北极和磁场的北极是不重合的，有一定的夹角，即磁偏角。根据经纬度是可以查到这个夹角的大小的。我们定义的地理坐标系的北当然是地理的北。但用磁力计求得实际是Xb与地磁的北的夹角，我们最后会加上磁偏角，这样就是Xb与地理北的夹角了，就是真正的航向角啦。
加速度计求roll，pitch角
机器人静止放在地上。地理坐标系下重力加速度为（0，0，g），机器人坐标系下加速度测量的三轴加速度为（ax,ay,az）。二者的关系如下图所示，（ax,ay,az）是三条边，（0,0，g）是对角线。
这样roll=arctan(ay/az)，pitch =-arctan(ax/az)
磁力计求航向角
地理坐标系下该点的磁场强度为（M,0，D），磁力计测得的三轴磁场强度为（mx,my,mz）。
当我们认为机器人是完全水平放在地上的时候，即Z轴和Zb轴是平行的时候，完全不用考虑垂直方向了。我们只用考虑XOY平面了。
ROS上代码实现
通过下面代码订阅地磁传感器和IMU的topic
ros::Subscriber imu_sub;
ros::Subscriber mag_sub;
imu_sub = nh_.subscribe("imu", 1, &IMAGE_LISTENER_and_LOCATOR::imu_callback, this);
mag_sub = nh_.subscribe("magnetic_field", 1, &IMAGE_LISTENER_and_LOCATOR::mag_callback, this);参考资料
https://blog.csdn.net/waihekor/article/details/103551408
https://www.cnblogs.com/aipan-123456/p/6056814.html