加速度计、磁力计求初始姿态角

1.几个基本概念。

地理坐标系：北东地。北为X轴、东为Y轴、地为Z轴。

机体坐标系：见下图。

姿态角：我的理解是，roll角是Zb在ZOY平面与Z轴的夹角。pitch角是Zb在ZOX平面与Z轴的夹角。yaw角是Xb在XOY平面与X轴的夹角。

旋转矩阵：地理坐标系到机体坐标系的转换矩阵。旋转矩阵为正交阵，逆即转置。

传感器：都是基于机体坐标系的。加速度计测三轴的加速度，磁力计测三轴的磁感应强度，陀螺仪测三轴的角速度。注意角速度的定义，方向是沿着三轴的。得到姿态角主要靠陀螺仪测得的角速度积分，但存在漂移误差，因此要用加速度计和磁力计随时校正。

很重要的：在地理坐标系下，重力加速度坐标始终为（0,0,1g），磁场坐标为（M,0,D）。在地球表面，磁场始终是沿着磁感应线指北的，某点的磁场强度为磁感应线在该点的切线，它在北方有个分量，在垂直方向有个分量，在Y轴是没有分量的，即（M,0，D）。这里还有一点，地理的北极和磁场的北极是不重合的，有一定的夹角，即磁偏角。根据经纬度是可以查到这个夹角的大小的。我们定义的地理坐标系的北当然是地理的北。但用磁力计求得实际是Xb与地磁的北的夹角，我们最后会加上磁偏角，这样就是Xb与地理北的夹角了，就是真正的航向角啦。