加速度计实时输出机体坐标系下的三轴线加速度，磁力计实时输出机体坐标系下的三轴地磁强度，加速度计能解算出俯仰角与横滚角，由磁力计计算出航向角，两者相互配合可以解算三个姿态角信息。
计算roll和pitch
当多旋翼无人机在地理坐标系中静止时， 加速度计的量测输出为
g 为重力加速度。
当多旋翼无人机处于任意姿态时，加速度计在机体坐标系中的量测输出为
我们知道地理坐标系的速度可以通过旋转矩阵得到机体坐标系下的数据：
所以：
计算yaw
多旋翼无人机在地理坐标系（ 导航坐标系） 下磁感应强度表示为，
地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。
如果保持电子罗盘和当地的水平面平行，那么罗盘中磁力计的三个轴就和这三个分量对应起来,即罗盘水平状态下，测量的三个轴数据就是机体系下的地磁分量。
对水平方向的两个分量来说，他们的矢量和总是指向磁北的，所以我们只需要水平的磁力分量即可算出传感器x轴与磁北的夹角：
如果我们认为磁北是0度，那我们就可以得到当前的偏航角。
但是我们不能保证当前的机体绝对水平，所以我们需要把传感器的数据转换到水平面。
怎么转换呢？当然是使用旋转矩阵啦。但是我们只需要旋转roll,和pitch。
所以令旋转矩阵，yaw为0，即yaw不旋转。
磁力计的输出是在机体坐标系下的磁感应强度，表示为。
可知 mn与 mb关系式为：
带入已知条件可以得:
通过这个旋转，我们就把机体坐标系的数据，变成了水平机体坐标系，也就是机体水平的情况下，磁力计测量的数据值。
主意z轴的正负（原始数据是z轴朝上为正）
用同样的方法求得x轴与磁北的夹角：
磁力计所解算的航向角是机体坐标x轴相对于磁北而言的，而真北与磁北之间存在一个磁偏角
所以机体坐标纵轴相对于真北的航向角为：
这里有几个隐藏添加条件要注意
1.我们使用的是zyx顺序旋转。
2.传感器x轴,对齐机体x轴，传感器y轴,对齐机体y轴时yaw为0度。
3.加计和陀螺仪的坐标系是完全重合的。
提问：计算出来的夹角是正还是负？
和机体坐标系有关，如果z轴朝下，那么机体的旋转正方向为顺时针，那么得到的偏航角就是负的。
