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 = egin{bmatrix} 0 & omega_x & omega_y & omega_zend{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}} = egin{bmatrix} q_1 & q_2 & q_3 & q_4end{bmatrix} ]

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

[sideset{^S}{}{hat{s}} = egin{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 {{ abla f(sideset{^S_E}{}{hat{q}}, sideset{^E}{}{hat{d}}, sideset{^S}{}{hat{s}})}over{leftVert abla f(sideset{^S_E}{}{hat{q}}, sideset{^E}{}{hat{d}}, sideset{^S}{}{hat{s}}) ightVert}}, k = 0,1,2,dots,n ]

[ abla 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}{_{ abla, t}}{q} = sideset{^S_E}{_{est, t-1}}{hat{q}} - mu_t {{ abla f}over{leftVert abla f ightVert}} ]

[ abla f = left{ egin{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} ight. ]

[mu_t = alpha leftVert sideset{^S_E}{}{dot{q}_{omega, t}} ightVert 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_{ abla, t}}) 的下标 ( abla) 指通过梯度方法得到的四元数，与前面通过角速度计算得到的四元数 (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}{_{ abla, t}}{q} + (1 - gamma_t) sideset{^S_E}{_{omega, t}}{q}, 0 le gamma_t le 1 ]

对于控制 (sideset{^S_E}{_{ abla, 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}{_{ abla}}{q}).

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

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

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

[eta = leftVert {1 over 2} hat{q} otimes egin{bmatrix} 0 & ilde{omega}_eta & ilde{omega}_eta & ilde{omega}_eta end{bmatrix} ightVert = sqrt{3 over 4} ilde{omega}_eta ]

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

[egin{cases} sideset{^S_E}{_{ abla, t}}{q} = sideset{^S_E}{_{est, t-1}}{hat{q}} - alpha leftVert sideset{^S_E}{}{dot{q}_{omega, t}} ightVert Delta t {{ abla f}over{leftVert abla f ightVert}} \ 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} ]

所以

[eta = leftVert sideset{^S_E}{_{omega, t}}{dot{q}} ightVert ]

继续推导 (gamma_t)：

[gamma_t = {eta over {{mu_t over Delta t} + eta}} ]

因为 (mu_t) 很大，所以：

[gamma_t simeq {eta Delta t over mu_t} simeq 0 ]

[sideset{^S_E}{_{ abla, t}}{q} simeq - mu_t {{ abla f}over{leftVert abla f ightVert}} ]

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

[sideset{^S_E}{_{est, t}}{q} = {eta Delta t over mu_t} (-mu_t { abla f over leftVert abla f ightVert}) + (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}} - eta sideset{^S_E}{_{epsilon, t}}{dot{hat q}} ]

[sideset{^S_E}{_{epsilon, t}}{dot{hat q}} = {{ abla f}over{leftVert abla f ightVert}} ]

补偿

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

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

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