角度计算

　在只有三轴加速度计的场合下（MARG初始校准的时候，需要先通过加速度计得到水平倾角，才能继续对磁力计数据进行处理）如何求得三轴角度，搜索互联网一般是这以下这两种算法：

　　左图算法是：

　　右图算法是：atan2(Ax,Az)也就是arctan(Ax/Az)。编程时还是推荐使用atan2。

　　图片来自：http://www.geek-workshop.com/thread-2328-1-1.html

　　左图方法中的角度是重力矢量g与3个正交轴的夹角，并不是本体坐标系与地面坐标系的三个欧拉角！！

　　拿俯仰角举例，俯仰角是本体坐标系的Oxb与水平面之间的夹角（前提是欧拉旋转顺序是ZYX，即航偏，俯仰，滚动，这里并不关心航偏，即使有航偏也认为航偏过后的新的坐标系是参考的地面坐标系，只有俯仰和滚动），矢量g先从指向z轴正方向，通过俯仰角、滚动角达到上右图矢量R位置，俯仰角是Axz，滚动角是Ayz。实际用个坐标系演示一下，就会发现俯仰角的大小就是Axz。

　　建立我建模常用的坐标系，也就是坐标系的y轴与上图一致，xz均方向相反，可得到Roll = atan2f(-ay, -az);Pitch = atan2f(ax, -az);