论文链接：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}}
]
补偿
对陀螺仪漂移的补偿和对磁力计变化的补偿。
对磁力计变化的补偿就是将全局坐标系的正北方向，一直对准磁力计得出的磁北方向。
陀螺仪漂移的补偿看不太懂，我看看再补上。