使用补偿算法对陀螺仪和电子罗盘进行数据融合，计算方位角



电子罗盘如  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