大家好，我是学电子的小白白~

上一篇我们讲了陀螺仪与加速度计的数据融合，本篇文章是互补滤波的续篇，接着讲陀螺仪与磁力计的数据融合。

先回忆一下上一篇内容，加速度计是如何修正陀螺仪的呢？

我们是假定了在大部分时间里，载体是静止或者匀速直线运动的，所以可以认为加速度计的测量值，就是重力加速度。但是，在载体水平时，重力加速度的方向与Z轴相同，是无法反映出偏航角yaw的（因为偏航角是绕Z轴旋转，即使偏航角变化，Z轴方向仍然不变，也就是说加速度计无法感知Z轴的旋转）。

而磁力计可以测出地球的磁场，能够得到水平的北向方位，所以引入磁力计的数据，可以用来修正陀螺仪的偏航角。

我们仍然先回忆一下上一篇的内容，陀螺仪与加速度计是怎样融合的，会有助于本篇方法的理解。

陀螺仪与加速度计融合时，我们先是通过当前时刻的姿态转移矩阵，算出了一个重力加速度的方向vx、vy、vz（这个方向包含了陀螺仪的姿态信息）；再将这个方向与加速度计测量到的重力加速度方向ax、ay、az对比，求出误差ex、ey、ez，再用误差去修正下一时刻的陀螺仪角速度gx、gy、gz。

本篇中，陀螺仪与磁力计融合时，我们也用类似的方法，先通过姿态转移矩阵的变换，利用当前的姿态信息求出一个磁场的方向；再用它与磁力计测到的磁场方向比对，求出误差，最后用这个误差去修正下一时刻的角速度。

但是，需要注意的是，利用加速度计姿态融合时，由于重力加速的方向只有z向有，x、y向都为0，所以我们可以认为它（方向）是已知的；而地球的磁场并不是完全的水平指向北极，在竖直方向上也有一定的大小，所以它不是个已知量；因此，具体实现的时候，磁力计数据的融合与加速度计有点不同，主要在利用姿态信息求磁场方向的这一步。

假定我们需要修正的姿态，是已经被加速度计修正过的，那么此时的姿态与真实的载体姿态就只存在偏航角yaw的偏移。

修正的主要步骤如下：

a) 磁力计测到的地球磁场数值为mx、my、mz，是在未修正偏航角的载体坐标系下的，可以通过当前的姿态转移矩阵转换到地理坐标系下（n系），转换后的磁力计数据为hx、hy、hz，这组磁力计的数据，与真实的磁场数据，只存在偏航角的偏移：

b) 在n系中，假定真实的磁场数据为bx、by、bz，由于z向的磁场数据与偏航角不相关，所以有bz=hz；如果n系的x轴指北，则by=0，bx=√(hx2 hy2)，这就是真实的磁场数据。（如果n系y轴指北，则bx=0，by=√(hx2 hy2)）。

c) 我们得到了真实的磁场数据后（就想当于加速度计融合时，已知的重力加速度），再通过未修正偏航角的姿态转移矩阵变回到载体系，得到一组载体系下的磁场数据wx、wy、wz，这组数据中就包含了偏航角的偏差信息：

d) 然后就可以用wx、wy、wz与磁力计测到的载体系下真实的mx、my、mz叉乘（求外积），求出偏差。

e) 最后，使用这组偏差去修正陀螺仪的角速度，仍然使用与加速度计数据融合相同的方法，PI后修正。

具体的实现框图如下：

融合磁力计数据的主要的代码实现如下：

（这段代码里也包括了加速度计的数据融合，计算误差时，将加速计的误差和磁力计的误差相加后，再求PI去修正陀螺仪的角速度）

上述代码在使用时，只要周期性地获取陀螺仪、加速度计、磁力计的数据，调用互补滤波函数，更新四元数，计算姿态角即可（与上一篇加速度计数据融合用法一样）。

测试一下互补滤波的效果，使用的硬件平台是MPU6050和HMC5883L，处理器是stm32f103c8t6。

初始化时，需要平放几秒钟让加速度计和陀螺仪自行校准。平放时俯仰角和滚转角都接近于0，偏航角依据与地磁北向的夹角有一定初始值。经过较长时间后，俯仰角、滚转角、偏航角都可以保持正确，不会产生误差累积和漂移；姿态变化后，在回到初始位置，也基本不会有偏差，说明陀螺仪的数据得到了修正；磁力计对偏航角的修正是有效的：

好了，关于互补滤波姿态融合的知识，就讲到这里了。