电子罗盘如 HMC5883L或者 MPU9250的AK8963测方位角时，经常会发现测量角度抖动很厉害。即使不动，角度也在不停的小范围内变化。这时候就需要使用陀螺仪对方位角进行补偿融合，得出稳定的数据。本文只讨论2Ｄ空间的方位角，就是电子罗盘X, Y两轴数据融合陀螺仪Ｚ轴数据。
1. 电子罗盘和陀螺仪测量物理意义
电子罗盘：０－36０度的方位角，也称为 yaw 。角度为arctan2(y,x)
陀螺仪： 围绕某轴旋转的角速度，这里我们只考虑Ｚ轴，相当于方位角的变化速度速度
2. 融合算法公式
在某个时间点 Tx 上, 电子罗盘值为 YawCompassTx, 陀螺仪值为YawGyroTx, 融合后的数据为 YawFuzeTx
下一个时间点 Tx+1, 陀螺仪计算的角度变化为 delta_Yaw = YawGyroTx+1 * delta_t, 其中 delta_t = Tx+1 - Tx
YawFuzeTx+1 = K * ( YawFuzeTx+ delta_Yaw ) + ( 1 - Ｋ ) * YawCompassTx+1
其中K的取值范围一般为 [0.9, 1), 物理意义就是对陀螺仪的信任程度。
在T0时间点， YawFuzeT0 = YawCompassT0
原文地址：http://blog.csdn.net/crazyquhezheng/article/details/44732191
