Madgwick IMU Filter

论文链接：http://202.114.96.204/cache/13/03/x-io.co.uk/35c82431852f2aa7d0feede9dc138626/madgwick_internal_report.pdf

IMU 是指六轴传感器，包含陀螺仪和加速度计。MARG 是指九轴传感器，在 IMU 的基础上添加了磁力计。
IMU = gyroscope + accelerometer
MARG(Magnetic, Angular Rate, and Gravity) = gyroscope + accelerometer + magnetometer

为方便起见，下文中，我将 IMU 与 MARG 统称为 IMU。

原理

Madgwick 是一个 Orientation Filter，用于获得精确的姿态数据，并不考虑整个 IMU 的积分过程。

在 IMU 与其他传感器的融合中我们一般都把 IMU 当做是内部（Intrinsic）传感器。内部传感器能够获得精确的加速度、角速度数据，实际使用中将加速度进行二次积分、角速度进行一次积分获得位置与姿态。但是因为与外界缺乏联系，积分能迅速扩大误差的累积，所以内部传感器需要与外部传感器融合使用。

将 IMU 分解成陀螺仪、加速度计、磁力计，加速度计测量的是地球重力场（受力平衡情况下），磁力计测量的是地磁场（忽略人工磁场影响）。

所以在 IMU 受力平衡状态下，陀螺仪是内部传感器，加速度计和磁力计是外部传感器。

Madgwick IMU Update 是使用加速度计或磁力计的数据与陀螺仪融合使用，提供精确的姿态数据。

过程

Madgwick 使用两种方法分别使用内部传感器、外部传感器求出两个姿态（四元数），随后将这两个姿态进行融合。

1. 内部传感器积分的四元数

用陀螺仪测量值与 \(t-1\) 时刻的四元数积分计算出 \(t\) 时刻四元数。

陀螺仪的测量值：

\[\sideset{^S}{}\omega = \begin{bmatrix} 0 & \omega_x & \omega_y & \omega_z\end{bmatrix}\]

（注：左上角 \(S\) 表示在 Sensor 坐标系下的坐标。）

四元数关于时间 \(t\) 的一阶导数：

\[ \sideset{^S_E}{}{\dot{q}} = {1 \over 2} {}\sideset{^S_E}{}{\hat{q}} \otimes \sideset{^S}{}{\omega} \]

（注：左下角 \(E\) 表示 Earth 坐标系，\(\sideset{^S_E}{}{\dot{q}}\) 表示 Earth 坐标系在 Sensor 坐标系下的四元数时间变化率。）

代入时间，从 \(t-1\) 时刻计算到 \(t\) 时刻：

\[ \sideset{^S_E}{_{\omega, t}}{\dot{q}} = {1 \over 2} \sideset{^S_E}{_{est, t-1}}{\hat{q}} \otimes \sideset{^S}{_t}\omega \]

\[ \sideset{^S_E}{_{\omega, t}}{q} = \sideset{^S_E}{_{est, t-1}}{\hat{q}} + \sideset{^S_E}{_{\omega, t}}{\dot{q}} {\Delta t}\]

（注：下标 \(\omega\) 指使用角速度计算得到的四元数，\(est, \hat{}\) 指上一时刻的“后验值”。）

2. 外部传感器优化的四元数

用 \(t\) 时刻加速度计或磁力计的测量值构建最小化问题，求 \(t\) 时刻四元数。

最小化问题：

\[ {min}_{\sideset{^S_E}{}{\hat{q}} \in \mathbb{R}^4} = f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}}) \]

\[ f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}}) = \sideset{^S_E}{^*}{\hat{q}} \otimes \sideset{^E}{}{\hat{d}} \otimes \sideset{^S_E}{}{\hat{q}} - \sideset{^S}{}{\hat{s}} \]

其中：

\[ \sideset{^S_E}{}{\hat{q}} = \begin{bmatrix} q_1 & q_2 & q_3 & q_4\end{bmatrix} \]

\[ \sideset{^E}{}{\hat{d}} = \begin{bmatrix} 0 & d_x & d_y & d_z \end{bmatrix} \]

\[ \sideset{^S}{}{\hat{s}} = \begin{bmatrix} 0 & s_x & s_y & s_z \end{bmatrix} \]

$ \sideset{^S_E}{}{\hat{q}} $ 表示待求的姿态四元数。
$ \sideset{^E}{}{\hat{d}} $ 表示在地球坐标系下的一个向量，即重力场或磁力场；$ \sideset{^S_E}{^*}{\hat{q}} \otimes \sideset{^E}{}{\hat{d}} \otimes \sideset{^S_E}{}{\hat{q}} $ 表示将 $ \sideset{^E}{}{\hat{d}} $ 旋转到 Sensor 坐标系下。
$ \sideset{^S}{}{\hat{s}} $ 是重力场或磁力场在 Sensor 坐标系下的测量值。

使用梯度下降法求解：

\[ \sideset{^S_E}{}{q_{k+1}} = \sideset{^S_E}{}{\hat{q}_k} - \mu {{\nabla f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}})}\over{\left\Vert \nabla f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}}) \right\Vert}}, k = 0,1,2,\dots,n\]

\[ \nabla f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}}) = J^T(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}) f(\sideset{^S_E}{}{\hat{q}}, \sideset{^E}{}{\hat{d}}, \sideset{^S}{}{\hat{s}}) \]

$ J $ 是 $ f $ 对 $ q $ 求一阶偏导，即 Jacobian 矩阵。

然而梯度下降的方法不行，迭代消耗大量的计算资源，与 IMU 的频率不匹配。

Madgwick 使用了替代方法：

\[ \sideset{^S_E}{_{\nabla, t}}{q} = \sideset{^S_E}{_{est, t-1}}{\hat{q}} - \mu_t {{\nabla f}\over{\left\Vert \nabla f \right\Vert}} \]

\[ \nabla f =
\left\{
\begin{array}{c}
J^T_g(\sideset{^S_E}{_{est, t-1}}{\hat{q}})f_g(\sideset{^S_E}{_{est, t-1}}{\hat{q}}, \sideset{^S}{_t}{\hat{a}}) \\
J^T_{g,b}(\sideset{^S_E}{_{est, t-1}}{\hat{q}}, \sideset{^E}{}{\hat{b}})f_{g,b}(\sideset{^S_E}{_{est, t-1}}{\hat{q}}, \sideset{^S}{_t}{\hat{a}}, \sideset{^E}{}{\hat{b}}, \sideset{^S}{}{\hat{m}})
\end{array}
\right. \]

\[ \mu_t = \alpha \left\Vert \sideset{^S_E}{}{\dot{q}_{\omega, t}}\right\Vert \Delta t , \alpha > 1\]

论文中又有一句话说到

the convergence rate governed by \(\mu_t\) is equal or greater than the physical rate of change of orientation

这是一个 convergence 收敛的过程，这个 convergence 不能很好理解，但是看了论文后面的内容，就知道 convergence 是指四元数关于 \(\Delta t\) 的一阶导数。

所谓的 equal or greater than 对应着 \(\alpha > 1\) （不是应该是 greater than 吗？？？）。

这个方法在 \(\sideset{^S_E}{}{\hat{q}_{est, t-1}}\) 处求一次偏导，得到梯度方向，随后在这个梯度方向上前进 \(\mu_t\) 的步长。而前面的梯度下降方法要求很多次偏导，前进的路线是曲线，得到的最终结果会更好，但是相应的计算消耗很大。这个方法只向最初的方向前进了比较大的步长，得到的不是最优点，整体方向是对的。

（注：\(\sideset{^S_E}{}{q_{\nabla, t}}\) 的下标 \(\nabla\) 指通过梯度方法得到的四元数，与前面通过角速度计算得到的四元数 \(\sideset{^S_E}{_{\omega, t}}{q}\) 相对应。）

个人想法，如果将 \(\sideset{^S_E}{}{\hat{q}_{est, t-1}}\) 换成 \(\sideset{^S_E}{_{\omega, t}}{q}\) 会更合理一些。或许因为这两个四元数需要并行计算，不方便使用 \(\sideset{^S_E}{_{\omega, t}}{q}\)。

3. 融合

融合前面两个四元数：

\[ \sideset{^S_E}{_{est, t}}{q} = \gamma_t \sideset{^S_E}{_{\nabla, t}}{q} + (1 - \gamma_t) \sideset{^S_E}{_{\omega, t}}{q}, 0 \le \gamma_t \le 1\]

对于控制 \(\sideset{^S_E}{_{\nabla, t}}{q}\) 与 \(\sideset{^S_E}{_{\omega, t}}{q}\) 比例的 \(\gamma_t\) 文中说了一句：

An optimal value of \(\gamma_t\) can be defined as that which ensures the weighted divergence
of \(\sideset{^S_E}{_{\omega}}{q}\) is equal to the weighted convergence of \(\sideset{^S_E}{_{\nabla}}{q}\).

于是得出了下面这个方程：

\[ (1 - \gamma_t)\beta = \gamma_t {\mu_t \over \Delta t} \]

\(\beta\) 在论文后面有提到（这个公式有点写得混乱了）：

\[ \beta = \left\Vert {1 \over 2} \hat{q} \otimes \begin{bmatrix} 0 & \tilde{\omega}_\beta & \tilde{\omega}_\beta & \tilde{\omega}_\beta \end{bmatrix} \right\Vert = \sqrt{3 \over 4} \tilde{\omega}_\beta \]

推导一下就知道 \(\gamma_t\) 是将 \(\sideset{^S_E}{_{\nabla, t}}{q}\) 与 \(\sideset{^S_E}{_{\omega, t}}{q}\) 分别对 \(\Delta t\) 求一阶偏导，并且让导数的模互为相反数的结果。（\(\left\Vert{{\nabla f}\over{\left\Vert \nabla f \right\Vert}}\right\Vert = 1\)）

\[
\begin{cases}
\sideset{^S_E}{_{\nabla, t}}{q} = \sideset{^S_E}{_{est, t-1}}{\hat{q}} - \alpha \left\Vert \sideset{^S_E}{}{\dot{q}_{\omega, t}}\right\Vert \Delta t {{\nabla f}\over{\left\Vert \nabla f \right\Vert}} \\
\sideset{^S_E}{_{\omega, t}}{q} = \sideset{^S_E}{_{est, t-1}}{\hat{q}} + \sideset{^S_E}{_{\omega, t}}{\dot{q}} {\Delta t}
\end{cases} \]

所以

\[ \beta = \left\Vert \sideset{^S_E}{_{\omega, t}}{\dot{q}} \right\Vert \]

继续推导 \(\gamma_t\)：

\[ \gamma_t = {\beta \over {{\mu_t \over \Delta t} + \beta}} \]

因为 \(\mu_t\) 很大，所以：

\[ \gamma_t \simeq {\beta \Delta t \over \mu_t} \simeq 0 \]

\[ \sideset{^S_E}{_{\nabla, t}}{q} \simeq - \mu_t {{\nabla f}\over{\left\Vert \nabla f \right\Vert}} \]

将上式代回 $ \sideset{^S_E}{_{est, t}}{q} $：

\[ \sideset{^S_E}{_{est, t}}{q} = {\beta \Delta t \over \mu_t} (-\mu_t {\nabla f \over \left\Vert \nabla f \right\Vert}) + (1-0)(\sideset{^S_E}{_{est, t-1}}{\hat{q}} + \sideset{^S_E}{_{\omega, t}}{\dot{q}} {\Delta t}) \]

整理一下就有最终结果：

\[ \sideset{^S_E}{_{est, t}}{q} = \sideset{^S_E}{_{est, t-1}}{\hat{q}} + \sideset{^S_E}{_{est, t}}{\dot{q}} {\Delta t} \]

\[ \sideset{^S_E}{_{est, t}}{\dot{q}} = \sideset{^S_E}{_{\omega, t}}{\dot{q}} - \beta \sideset{^S_E}{_{\epsilon, t}}{\dot{\hat q}} \]

\[ \sideset{^S_E}{_{\epsilon, t}}{\dot{\hat q}} = {{\nabla f}\over{\left\Vert \nabla f \right\Vert}} \]

补偿

对陀螺仪漂移的补偿和对磁力计变化的补偿。

对磁力计变化的补偿就是将全局坐标系的正北方向，一直对准磁力计得出的磁北方向。

陀螺仪漂移的补偿看不太懂，我看看再补上。