【运动传感器】Madgwick算法（下）

在上篇中，讲解了分别从角速度计、加速度计、磁力计估计姿态的方法。本篇文章讲解他们的融合，对误差的处理，以及标定实验。

融合

这一部分在文章中称为filter。
在上一篇文章中，我们能够通过角速度计的读数$\omega$，加速度计或磁力计的读数$d_S$在每个采样间隔更新姿态四元数$q$：
$$q_{t+1}=q_t+\frac{1}{2}{q}_t\times {\omega }_t\Delta t$$

$$q_{t+1}=q_t-\mu \frac{\nabla f}{||\nabla f||}$$

其中$f$是姿态拟合误差，可以通过当前姿态$q$和读数$d_S$计算得到。
两者融合有：

$$q_{t+1}=q_t+\frac{1}{2}{q}_t\times {\omega }_t\Delta t-\beta \frac{\nabla f}{||\nabla f||}{\Delta }_t$$

增量由两部分构成：

第一部分$\frac{1}{2}{q}_t\times {\omega }_t$为通过角速度计获取的结果。
如果角速度计是完美的，则只需这部分即可。

第二部分$\frac{\nabla f}{||\nabla f||}$为通过加速度计/磁力计获取的结果。其权重$\beta$表示角速度计的误差。
如果误差为0，则不需要此项；否则，因为角速度计不完美，当前姿态$q$有误差，导致拟合误差$f$没有达到极小， ∇ f \nabla f