- 本文主要参考：http://www.olliw.eu/2013/imu-data-fusing/
惯性测量单元(IMU)测量加速度和旋转速率，可能还有地球磁场，以确定物体的姿态。任何认真阅读这篇文章的人都可能熟悉这个主题和数据融合的需求，我不应该在这上面花更多的篇幅。然而，我应该-因为这将是一篇很长的文章-花一些话，这篇文章是关于:
关于这个主题的网络文献非常丰富。然而，它似乎或多或少地基于一些作品，例如Colton [SC]、Premerlani和Bizard [PB]、Starlino [St]、Lauszus [La]、Mahony [RM]和Madgwick [SM]，它们似乎已经成为爱好者的标准参考。这里给出的不同算法和实现细节的数量多少有些令人困惑，但是——尽管使用了不同的术语——它们的不同之处并不总是显而易见的。本文对这些作品中所描述的数据融合滤波器进行了分析和比较，以便更好地理解它们的行为，以及它们之间的异同。作为一个推论，简化和/或改进的算法浮出水面。本文只考虑6自由度的imu。
本文讨论了三种基本的滤波方法:互补滤波、卡尔曼滤波(带常数矩阵)和Mahony&Madgwick滤波。本文以一些我认为相关的初步信息开始。然后考虑单个轴的情况(称为一维或一维)。首先讨论了不估计陀螺偏差的最简单方法(称为一阶方法)。然后进行陀螺偏差估计(称为二阶估计)。最后，考虑三轴(称为三维)的完整情况，并对一些近似和改进进行了评价。
1. 准备知识
1.1 运动学和IMU算法说明
1.2 离散化和实现问题
1.3 带常数矩阵的卡尔曼滤波器
2. 一维IMU数据融合-一阶(忽略漂移估计)
2.1 互补滤波器
2.2 卡尔曼滤波器
2.3 Mahony&Madgwick过滤器
2.4 比较和结论
3.一维 IMU数据融合-二阶(带漂移估计)
3.1 卡尔曼滤波器
3.2 Mahony&Madgwick过滤器
3.3 比较
3.4 互补滤波器
3.5 一维滤波器概述
4. 三维IMU数据与Mahony滤波器融合
4.1 „原始“马赫尼过滤器
4.2 二维Mahony滤波器和简化
4.3 Premerlani & Bizard的IMU滤波器
5. 进一步的3 d过滤器
参考文献
IMU的实现
表示法:离散时间步长记为, 或作为时间步长指标。数量的估计是用帽子表示的，例如，但是为了简单起见，当混淆似乎是不可能的时候，它通常会被去掉。加粗符号表示中的向量或矩阵(例如，状态空间中的向量和矩阵将不加粗)和四元数。
注意: Madgwick的方案在某些方面与Mahony的方案有很大的不同，但是他共享了他的反馈循环思想。此外，Madgwick还为Mahony的过滤器提供了C代码，我觉得这非常有用。这就是为什么我把Mahony的方法称为Mahony&Madgwick过滤器。Madgwick最速下降法将在下面简要介绍。
1.预习知识
1.1 运动学和IMU算法说明
姿态估计的任务相当于对一个物体旋转的运动学方程进行评估(计算):
其中是在基体参考系中测量的旋转速率向量，而旋转表示在地球参考系中观测到的基体参考系的方向。对于任何向量,坐标由相对于地球坐标系转换到基体坐标系的关系为： ,同时可以推导出 (其中的负号在这是因为是在基体坐标系中表达的)
虽然看起来很简单，但方程(1.1)向我们提出了一些基本问题:
(1) 式(1.1)是非线性的。这将极大地复杂化滤波器的设计。
(2) 式(1.1)容易出现数值误差。在微处理器上执行的任何计算都存在数值误差，但在大多数情况下，它们表现良好，因为它们不会累积。但是，对于式(1)，如果不采取相应的措施，误差会不断增加，并且最终不再代表旋转。重要的是，这与旋转的全局不可交换性有关(在三维中)，因此是基本的。在这里，像方向余弦矩阵(DCM)或四元数这样的时髦词进入了游戏。
(3) 旋转可以用几种方法来表示[RO]，每种表示方法都有其各自的优缺点。最著名的是由旋转矩阵或DCM、欧拉角和相关角(Cardan, Tait-Brian)、轴角、四元数表示，但还有更多的存在。显然，算法在很大程度上取决于选择哪种表示。
你可能注意到，还没有在测量噪声和数据融合上花费文字;我还没有将它添加到列表中，因为它并不是真正来源于(1.1)，尽管它是重要的一点，实际上也是本文的主题。
无论如何，由于困难是相似的，上述作者提出的所有算法都表现出类似的结构:
(T1)对DCM、欧拉角、四元数或表示的任何数的变化率进行积分。
对于(T1)使用DCM、四元数和欧拉角表示，或者——如果只需要方向——使用“重力”向量。
(T2)注意{\bf{R}}的数字表示实际上代表“真正的”物理旋转。
文献中提出了若干策略。在[PB]和[St2]中，通过将x和y方向的误差相减到相等的部分来重新规范化DCM，在[RM07]中提出了矩阵指数和罗德里格斯公式，或者采用2阶龙格-库塔公式，在[RM08]和[SM]中采用了四元数正态化的常用方法。在MultiWii代码中(和在[St1]中)，采取了完全忽略这一步的极端方法;也就是说，数据融合是保证姿态估计正确性的重要步骤。
(T3)通过融合加速度计和陀螺仪数据改进姿态估计。
这是至关重要的一步(也是最具挑战性的一步)，因为它根据滤波器的性能来决定滤波的实际质量。任务是根据给定的姿态和测量的加速度向量计算改进的姿态估计，或者正式地确定一个函数。在上面提到的文章中，我们采取了三种方法，这里我们称之为互补滤波器、卡尔曼滤波器 和 Mahony滤波器。
本文是关于task T3的;不讨论T1和T2任务。
1.2 离散化和实现问题
对于一个给定的系统,如连续时间描述传递函数G (s),在计算机代码实现过程中多种不同的实现方式,尽管他们来自同一个函数G (s)，但是性能却不尽相同。
得到不同实现的一个原因是，将连续时间的传递函数G(s)转换为离散时间的传递函数H(z)没有通用的规则，因为它是近似的。因此，从G(s)到H(z)的转换不是唯一的，典型的选择是:
| 后向差分: | |
|---|---|
| 双线性变换, 扩展: | |
| 脉冲不变性变换: |
另一个原因是给定的离散时间传递函数H(z)可以用不同的方式实现，不同的实现可能在稳定性、高频噪声等各个方面表现出不同的行为。这个话题不简单，超出了我的能力范围，但基本原则是明确的。
让我们思考一个很熟悉的例子，PID控制器:
如果使用后向差分的话，可以得到：
在实现时可以先计算：
然后计算：
另外也可以直接求解：
第一种实现方式可以得到位置PID算法：
第二种方式可以得到速度PID算法：
两者都来自于相同的函数H(z)或G(s)，但是关于结束、内部变量溢出、存储元素的数量等都有所不同。
推论#1：在位置PID算法中，Eq.(1.5)，两个方程的顺序可以颠倒：
当参数发生不相关的变化时，它们在代码中的执行或实现顺序是不相关的。
1.3 卡尔曼滤波器和常数矩阵
卡尔曼滤波器通过协方差矩阵将噪声考虑在内，通过相对复杂的公式将其在每个时间梯度内更新。然而如果他们是不随时间变化的，那么卡尔曼滤波器的方程将会简化很多。我不知道在哪些条件下这些矩阵能变成常数，但是似乎在通常系统中，当做常数考虑是比较合理的，在后面的讨论中先假设他们是常数，同样，系统矩阵也当做常数。
离散卡尔曼滤波器在相空间中应用到系统模型如下：
在此，是状态向量，，是控制向量，是测量向量，我去掉了其他量的时间步长指数来强调它们与时间无关。有一个微秒的地方：通常(1.8a)是写做,但是通常情况被写成了。在具体实现中，我会使用最后的控制向量,所以最后卡尔曼滤波器变为：
其中是预测项，是更新的状态估计。
卡尔曼滤波器的一个通用问题是针对同一个系统，通常可以建立几种不同的状态空间模型，因此可能推导出不同的卡尔曼滤波器。
2 一维IMU 数据融合----一阶（忽略漂移估计）
本章主要考虑最简单情况下的IMU数据融合，即通过旋转速率随时间的积分和加速度数据融合单个轴的角度，不显性的考虑陀螺仪的漂移。
在接下来，我们用代表估计的角度，代表利用加速度计测量的角度，代表利用陀螺仪测量的旋转速率。
2.1互补滤波器
互补滤波器将角速度积分通过一阶高通滤波，将加速度计测量值通过一阶低通，然后两项相加融合。传递函数为：
其中决定了滤波器的截止频率，使用后向差分可以得到带入公式整理得到：
其中,这个关系在代码实现时有几种方式，为了更好的和其他案例做比较，在此使用如下方式：
2.2 Kalman Filter
在此种简单的情形下，可以将系统的状态空间模型简化为：
状态向量变成 ,对应的矩阵为,那么卡尔曼方程可以写成：
注意，上面的是预测项，是估计项，如果将预测项带入估计项，这两个方程同样能够被表达为：
其中,和上面的方程（2.3）对比起来是不是很像啊？
2.3 Mahony&Madgwick Filter
在此，数据融合使用了P控制器和积分过程，也就是说加速度计的角度a 变成了控制系统所说的设定点（setpiont）,旋转速率被当做扰动，传递函数即：
其中是P控制器的误差输入，重新整理这个方程可以准确得到互补滤波器的传递函数（方程2.1），从控制理论的标准表达来看，这并不奇怪。
控制器（2.7）通常在实现中先写为,然后分成 且,最后离散化为：
最后得到Mahony&Madgwick滤波器的更新律为：
其中
（备注待添加）
2.4 对比和总结
从以上结果的对比可以得到几个发现：
（1）互补滤波和卡尔曼滤波可以得到相同的更新方程，如公式（2.2）和公（2.6）是相同的，就传递函数而言也是一样的。
（2）互补滤波和Mahony&Madgwick滤波器利用相同的传递函数描述。
（3）从（1）和（2）可以知道忙着三种滤波器在传递函数的层面来说是相同的。
（4）除了（3）以外，Mahony&Madgwick滤波器与互补滤波器和卡尔曼滤波器是不相同的，比较方程（2.9），（2.2），（2.6）。
最后一点值得深入讨论，方便起见，再此重新列写两种更新律：
我们可以从两个方面来分析其中的差别，第一,方程（2.2或2.6）可以理解为，首先通过对旋转速率进行积分得到更新的角度，然后用进行过滤得到改进的角度，从而得到新的角度。这也可以通过一个方括号表达： 。与之对比，方程（2.9）可以理解为角度首先被进行过滤然后对旋转速率进行积分，对应的用括号表现为：。其次以上方程可以重新整合到算法：
| complementary or Kalman filter (1D, 1st order) | Mahony&Madgwick filter (1D, 1st order) |
|---|---|
它们本质上是相同的，除了一个重要的区别，即左边的反馈误差使用的是更新后的角度,但是右边的方程使用了之前的估计角度,这两种算法不能直接相互转换，即使他们都是从相同的传递函数推导出来。
3 一维IMU数据融合--二阶（考虑漂移估计）
在本章，通过显性的将陀螺仪的偏置和漂移纳入考虑之中，我们改进了单轴的滤波器。据我所知，在此之前还没有使用互补滤波器完成这个任务的 相关描述。因此本章讨论滤波器的顺序将相对于前一章有所变化。
3.1Kalman Filter
如文献【】所述，系统的状态空间模型可以扩展为：
其中代表了陀螺仪偏置，我们可以很自然的推出它可以这样修正测量的旋转速率,在此我们可以使用“﹢”号和“-”号，状态向量和矩阵可以变成：
卡尔曼方程可以推导为：
跟之前一样，我们将这些方程重新表达，得到更新律为：
其中 这和一阶的结果中的方程（2.6）很相似，测量的旋转速率被估计的偏置修正，这个偏置反过来又通过对误差的积分得到：
推论#2:应注意将偏差校正中的符号更改为“+”的效果。修正后的转速为，与预期一致，但是方程(3.3)及(3.4)中的偏差更新定律并无更改!因此，结果并不等价于变量替换。
评价：你也可以根据文献【】的建议从状态空间向量触发，不过在我们的这个案例中，这明显会导致一个不恰当的滤波器（也就是说:推到出卡尔曼滤波器是一回事，使之很好的工作又是另一回事）。
3.2. Mahony&Madgwick Filter
使用PI控制器可以简化陀螺仪漂移的估计【文献】，传递函数是根据：
再次根据位置PID算法的标准实现，我们可以将方程（3.5）拆分成：,,,离散化后得到：
然后得到更新律：
其中
可以看出，只要改变一个不重要的参数，方程（3.7）中的两个方程的顺序就可以颠倒了。
3.3 对比
从2.4章节相同的方面对比二阶的卡尔曼和Mahony&Madgwick算法中的更新律：
| Kalman filter (1D, 2nd order) | Mahony&Madgwick filter (1D, 2nd order) |
|---|---|
| ——(3.8) |
——(3.9) |
应该注意到为了得到这些方程，在Kalman滤波器的bias估计的符号变成了“+”，此外，在Mahony&Madgwick滤波器的方程的顺序被反转了，同时引入了
这两个更新定律本质上是相同的，除了一个重要的区别，即Kalman过滤器在误差中使用更新的角度，而Mahony&Madgwick过滤器使用之前的角度估计
，就像之前在一阶过滤器中观察到的(第2.4章)。
3.4互补滤波器
通过解Mahony&Madgwick滤波器的传递函数，可以很容易得到关于的互补滤波器：
很明显，这个互补滤波器是由二阶滤波器构建的。注意，作用于加速度数据的滤波器实际上由低通加带通滤波器组成。
这个结果产生了有趣的结论。作为二阶滤波器，加速度和转速滤波器的频率响应以谐振频率和阻尼系数为特征：
阻尼系数决定了谐振频率的超调。对于高通(和低通)滤波器，在时频率响应是平坦的(而阶跃响应是无振荡的)。这表明了标准：
为了避免在陀螺仪通道中超调。同时为了最小化加速度计通道的超调，阻尼应该稍微大于这个值;在平滑频率响应和快速偏差估计之间，可能是一个很好的折衷方案。因此，根据经验，。
注意，除非被设置为非常小的值，否则交叉频率由(或其平方根的倒数)决定，而不是像一阶情况那样由决定!实际上，可以使用参数或和来代替和;对用户来说，滤波器的调优可能更直观。
这些考虑显然也适用于Mahony&Madgwick滤波器和Kalman滤波器。
互补滤波器可以像(3.6)那样实现，也可以用任何一种对数字滤波器有利的算法来实现。直接形式II是一个典型的选择。
3.5 一维滤波器的总结
尽管篇幅很长，但在第2章和第3章中对单一轴数据融合的不同方法进行了详细讨论，得出了一个非常简短的总结:
- 这三种不同的方法实际上并没有那么不同，即使是在二阶情况下。
- 另外，还得到了选择偏差估计器增益或的准则，以及作为补充滤波器的更简单和/或更灵活的直接实现。
- 在计算误差的方式上，Kalman和Mahony&Madgwick过滤器是有区别的。这可能被解释为概念上不同的“哲学”，但除此之外，我不清楚这是否也有实际的后果，如不同的稳定性或高频噪声。(有谁知道?)
附录一：参考文献
[SC] Fun with the Complementary Filter [link] and The Balance Filter (Jun. ’07) [.pdf] – by Shane Colton
[PB] Direction Cosine Matrix IMU: Theory (May ’09) [.pdf] – by William Premerlani, Paul Bizard,
see also the google repository gentlenav [link] (it hosts also some of Mahony’s papers)
[St1] A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications (Dez. ’09) [link] – by Starlino
[St2] DCM Tutorial – An Introduction to Orientation Kinematics (May ’11) [link] – by Starlino (or as [.pdf])
[La] A practical approach to Kalman filter and how to implement it (Sep. ’12) [link] – by Lauszus, TKJ Electronics
Mahony’s papers:
[RM05] Complementary filter design on the special orthogonal group SO(3) (Dec. ’05) [.pdf] – by Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin
[RM07] Complementary filter design on the Special Euclidean group SO(3) (’07) [.pdf] – by Grant Baldwin, Robert Mahony, Jochen Trumpf, Tarek Hamel, Thibault Cheviron
[RM08] Nonlinear Complementary Filters on the Special Orthogonal Group (Jun. ’08) [link] – by Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin (it is also hosted on gentlenav)
Madgwick’s report and codes:
[SM1] An efficient orientation filter for inertial and inertial/magnetic sensor arrays (Apr.’10) [.pdf] – by Sebastian Madgwick (internal report on his thesis and MARG)
[SM2] Codes and Resources: Open source IMU and AHRS algorithms [link] (original repository imumargalgorithm30042010sohm)
Kalman filter:
[KA1] Kalman Filtering (June ’01) – by Dan Simon
[KA2] An Introduction to the Kalman Filter – by Greg Welch, Gary Bishop (or here)
[KA3] Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation (Sep. ’12) – by Ramsey Faragher
[KA4] What is the Kalman Filter and How can it be used for Data Fusion? (Dec. ’05) – by Sandra Mau (Note: This ref should be considered with caution, I added it because the first two presented filters are of pedagogical value, but otherwise the work shouldn’t be taken seriously)
Rotation representations:
[RO1] Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors – by James Diebel (excellent!)
[RO2] Rotation Representations and Performance Issues – by David Eberly
[RO3] Rotation formalisms in three dimensions [link] – Wikipedia
[RO4] on Euler, Tait-Bryan and Cardan angles see Euler Angles [link] – Wikipedia
[RO5] Application of Quaternions to Computation with Rotations – by Eugene Salamin
Miscellaneous:
[LTB] Other MARG/AHRS/IMU/INS Open Code Projects – by Lewis De Payne (Lew’s Tech Blog)
[SHO] Quaternions – by Ken Shoemake
[WTH] A Comparison of Complementary and Kalman Filtering – by Walter T. Higgins