数据融合

本文主要参考：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的实现

表示法:离散时间步长记为 $\Delta t$ , $n$ 或 $k$ 作为时间步长指标。数量的估计是用帽子表示的，例如 $\hat{x}$ ，但是为了简单起见，当混淆似乎是不可能的时候，它通常会被去掉。加粗符号表示 $\mathbb{R}^3$ 中的向量或矩阵(例如，状态空间中的向量和矩阵将不加粗)和四元数。
注意: Madgwick的方案在某些方面与Mahony的方案有很大的不同，但是他共享了他的反馈循环思想。此外，Madgwick还为Mahony的过滤器提供了C代码，我觉得这非常有用。这就是为什么我把Mahony的方法称为Mahony&Madgwick过滤器。Madgwick最速下降法将在下面简要介绍。

1.预习知识

1.1 运动学和IMU算法说明

姿态估计的任务相当于对一个物体旋转的运动学方程进行评估(计算):
$\dot{\bf{R}} = {\bf{R}} {\boldsymbol{\Omega}}_\times with {\boldsymbol{\Omega}}_\times = {\boldsymbol{\omega}} {\bf{J}} = \left( \begin{array}{ccc} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{array} \right), \tag{1.1}$
其中 ${\boldsymbol{\omega}} = (\omega_x，\omega_y，\omega_z)^T$ 是在基体参考系中测量的旋转速率向量，而旋转 ${\bf{R}}$ 表示在地球参考系中观测到的基体参考系的方向。对于任何向量 ${\bf {v}}$ ,坐标 ${\bf {v}} _{earth}$ 由相对于地球坐标系转换到基体坐标系的关系为： ${\bf{v}}_{body} = {\bf{R}}^T {\bf{v}}_{earth}$ ,同时可以推导出 $\dot{\bf{v}}_{body} = {\boldsymbol{\Omega}}^T_\times {\bf{v}}_{body} = - ({\boldsymbol{\omega}} \times {\bf{v}}_{body})$ (其中的负号在这是因为 $\omega$ 是在基体坐标系中表达的)

虽然看起来很简单，但方程(1.1)向我们提出了一些基本问题:
(1) 式(1.1)是非线性的。这将极大地复杂化滤波器的设计。
(2) 式(1.1)容易出现数值误差。在微处理器上执行的任何计算都存在数值误差，但在大多数情况下，它们表现良好，因为它们不会累积。但是，对于式(1)，如果不采取相应的措施，误差会不断增加，并且 ${\bf{R}}$ 最终不再代表旋转。重要的是，这与旋转的全局不可交换性有关(在三维中)，因此是基本的。在这里，像方向余弦矩阵(DCM)或四元数这样的时髦词进入了游戏。
(3) 旋转 ${\bf{R}}$ 可以用几种方法来表示[RO]，每种表示方法都有其各自的优缺点。最著名的是由旋转矩阵或DCM、欧拉角和相关角(Cardan, Tait-Brian)、轴角、四元数表示，但还有更多的存在。显然，算法在很大程度上取决于选择哪种表示。

你可能注意到，还没有在测量噪声和数据融合上花费文字;我还没有将它添加到列表中，因为它并不是真正来源于(1.1)，尽管它是重要的一点，实际上也是本文的主题。
无论如何，由于困难是相似的，上述作者提出的所有算法都表现出类似的结构:

(T1)对DCM、欧拉角、四元数或表示 ${\bf{R}}$ 的任何数的变化率进行积分。
对于(T1)使用DCM、四元数和欧拉角表示，或者——如果只需要方向——使用“重力”向量 ${\bf{d}} = - {\bf{R}}^T {\bf{e}}_z$ 。

(T2)注意{\bf{R}}的数字表示实际上代表“真正的”物理旋转。
文献中提出了若干策略。在[PB]和[St2]中，通过将x和y方向的误差相减到相等的部分来重新规范化DCM，在[RM07]中提出了矩阵指数和罗德里格斯公式，或者采用2阶龙格-库塔公式，在[RM08]和[SM]中采用了四元数正态化的常用方法。在MultiWii代码中(和在[St1]中)，采取了完全忽略这一步的极端方法;也就是说，数据融合是保证姿态估计正确性的重要步骤。

(T3)通过融合加速度计和陀螺仪数据改进姿态估计。
这是至关重要的一步(也是最具挑战性的一步)，因为它根据滤波器的性能来决定滤波的实际质量。任务是根据给定的姿态 ${\bf{R}}$ 和测量的加速度向量 ${\bf{a}}$ 计算改进的姿态估计 ${\bf{R}}^{improved}$ ，或者正式地确定一个函数 ${\bf{R}}^{improved}= f({\bf{R}}， {\bf{a}})$ 。在上面提到的文章中，我们采取了三种方法，这里我们称之为互补滤波器、卡尔曼滤波器和 Mahony滤波器。
本文是关于task T3的;不讨论T1和T2任务。

1.2 离散化和实现问题

对于一个给定的系统,如连续时间描述传递函数G (s),在计算机代码实现过程中多种不同的实现方式,尽管他们来自同一个函数G (s)，但是性能却不尽相同。
得到不同实现的一个原因是，将连续时间的传递函数G(s)转换为离散时间的传递函数H(z)没有通用的规则，因为它是近似的。因此，从G(s)到H(z)的转换不是唯一的，典型的选择是:

后向差分:	$s = \dfrac{1}{\Delta t} ( 1 - z^{-1} )\tag{1.2}$

双线性变换, 扩展 $ln(z)$ :	$s = \dfrac{2}{\Delta t} \dfrac{ 1 - z^{-1} }{ 1 + z^{-1} } \tag{1.3}$
脉冲不变性变换:	$\dfrac{ G(z) }{1 - z^{-1}} = Z\left( \dfrac{G(s)}{s} \right), \tag{1.4}$

另一个原因是给定的离散时间传递函数H(z)可以用不同的方式实现，不同的实现可能在稳定性、高频噪声等各个方面表现出不同的行为。这个话题不简单，超出了我的能力范围，但基本原则是明确的。
让我们思考一个很熟悉的例子，PID控制器:
$G(s) = K_p + \frac{1}{s} K_i + s K_d$
如果使用后向差分的话，可以得到：
$H(z) = K_p + \frac{K_i \Delta t}{1 - z^{-1}} + \frac{K_d}{\Delta t}(1 - z^{-1})$
在实现时可以先计算：
$I_n = \frac{K_i \Delta t}{1 - z^{-1}} x_n$
然后计算：
$y_n = K_p x_n + I_n + \frac{K_d}{\Delta t}(1 - z^{-1}) x_n$
另外也可以直接求解：
$y_n= H(z) x_n$
第一种实现方式可以得到位置PID算法：
$I_n = I_{n-1} + K_i \Delta t x_n, \tag {1.5a}$
$y_n = K_p x_n + I_n + \dfrac{K_d}{\Delta t}( x_n - x_{n-1} ), \tag{1.5b}$
第二种方式可以得到速度PID算法：
$y_n = y_{n-1} + K_p ( x_n - x_{n-1} ) + K_i \Delta t x_n + \dfrac{K_d}{\Delta t}( x_n + 2x_{n-1} - x_{n-2} ). \tag{1.6}$
两者都来自于相同的函数H(z)或G(s)，但是关于结束、内部变量溢出、存储元素的数量等都有所不同。
推论#1：在位置PID算法中，Eq.(1.5)，两个方程的顺序可以颠倒：
$y_n = K_p x_n + I_{n-1} + \dfrac{K_d}{\Delta t}( x_n - x_{n-1} )\tag{1.7a}$
$I_n = I_{n-1} + K_i \Delta t x_n,\tag{1.7b}$
当参数 $K_p \rightarrow K_p + K_i \Delta t$ 发生不相关的变化时，它们在代码中的执行或实现顺序是不相关的。

1.3 卡尔曼滤波器和常数矩阵

卡尔曼滤波器通过协方差矩阵将噪声考虑在内，通过相对复杂的公式将其在每个时间梯度内更新。然而如果他们是不随时间变化的，那么卡尔曼滤波器的方程将会简化很多。我不知道在哪些条件下这些矩阵能变成常数，但是似乎在通常系统中，当做常数考虑是比较合理的，在后面的讨论中先假设他们是常数，同样，系统矩阵也当做常数。

离散卡尔曼滤波器在相空间中应用到系统模型如下：
$X_{k}= AX_{k-1}+ Bu_{k}+w \tag {1.8a}$
$Z_{k}= HX_{k}+v \tag{1.8b}$
在此， $x_{k}$ 是状态向量，， $u_{k}$ 是控制向量， $z_{k}$ 是测量向量，我去掉了其他量的时间步长指数来强调它们与时间无关。有一个微秒的地方：通常(1.8a)是写做 $u_{k}$ ,但是通常情况被写成了 $u_{k-1}$ 。在具体实现中，我会使用最后的控制向量 $u_{k}$ ,所以最后卡尔曼滤波器变为：
$\hat{x}_{k}^{-} = A\hat{x}_{k-1} + Bu_{k} \tag{1.9a}$
$\hat{x}_{k} = \hat{x}_{k}^{-} + K(z_{k}-H\hat{x}_{k}^{-}) \tag{1.9b}$
其中 $\hat{x}_{k}^{-}$ 是预测项， $\hat{x}_{k}$ 是更新的状态估计。
卡尔曼滤波器的一个通用问题是针对同一个系统，通常可以建立几种不同的状态空间模型，因此可能推导出不同的卡尔曼滤波器。

2 一维IMU 数据融合----一阶（忽略漂移估计）

本章主要考虑最简单情况下的IMU数据融合，即通过旋转速率随时间的积分和加速度数据融合单个轴的角度，不显性的考虑陀螺仪的漂移。
在接下来，我们用 $\theta$ 代表估计的角度， $a$ 代表利用加速度计测量的角度， $\omega$ 代表利用陀螺仪测量的旋转速率。

2.1互补滤波器

互补滤波器将角速度积分通过一阶高通滤波，将加速度计测量值通过一阶低通，然后两项相加融合。传递函数为：
$\theta =\frac{1}{1+Ts}a + \frac{Ts}{1+Ts}\frac{1}{s}\omega = \frac{a + T\omega}{1+Ts} \tag{2.1}$
其中 $T$ 决定了滤波器的截止频率，使用后向差分可以得到 $1+Ts = (1+\frac{T}{\Delta t})-\frac{T}{\Delta t}z^{-1}$ 带入公式 $（2.1）$ 整理得到：
$\theta _{k} = \alpha(\theta_{k-1} + \omega_{k}\Delta t) + (1 - \alpha)a \tag{2.2}$
其中 $\alpha = \frac{T}{\Delta t}/(1+ \frac{T}{\Delta t})$ ,这个关系在代码实现时有几种方式，为了更好的和其他案例做比较，在此使用如下方式：
$\theta _{k} = \alpha\theta_{k-1} + \alpha\omega_{k}\Delta t + (1 - \alpha)a_{k} \tag{2.2}$

2.2 Kalman Filter

在此种简单的情形下，可以将系统的状态空间模型简化为：
$\theta _{k} = \theta_{k-1} + \omega_{k}\Delta t$
$z_{k} = a_{k} \tag{2.4}$
状态向量变成 $x = (\theta),u = (\omega)$ ,对应的矩阵为 $A = (1),B = (\Delta t), H = (1),K = (K_{0})$ ,那么卡尔曼方程可以写成：
$\hat{\theta}^{-}_k = \hat{\theta}_{k-1} + \omega_k \Delta t$
$\hat{\theta}_{k} = (1-K_0) \hat{\theta}^{-}_k + K_0 a_k \tag{2.5}$
注意，上面的 $\hat{\theta}_k^-$ 是预测项， $\hat{\theta}_k$ 是估计项，如果将预测项带入估计项，这两个方程同样能够被表达为：
$\hat{\theta}_k = \alpha \hat{\theta}_{k-1} + (1-\alpha) a_k + \alpha \omega_k \Delta t \tag{2.6}$
其中 $\alpha = 1-K_0$ ,和上面的方程（2.3）对比起来是不是很像啊？

2.3 Mahony&Madgwick Filter

在此，数据融合使用了P控制器和积分过程，也就是说加速度计的角度a 变成了控制系统所说的设定点（setpiont）,旋转速率 $\omega$ 被当做扰动，传递函数即：
$\theta = K_p \dfrac{1}{s} ( a - \theta ) + \dfrac{1}{s} \omega \tag{2.7}$
其中 $a-\theta$ 是P控制器的误差输入，重新整理这个方程可以准确得到互补滤波器的传递函数（方程2.1），从控制理论的标准表达来看，这并不奇怪。
控制器（2.7）通常在实现中先写为 $\theta = \dfrac{1}{s}[K_p( a - \theta ) + \dfrac{1}{s} \omega]$ ,然后分成 $e= a-\theta$ 且 $\theta = \dfrac{1}{s}(K_p + \omega)$ ,最后离散化为：
$e_k = a_k - \theta_{k-1} \\[0.5em] \theta_k = \theta_{k-1} + (K_p e_k + \omega_k)\Delta t \tag{2.8}$
最后得到Mahony&Madgwick滤波器的更新律为：

$\theta_k = \alpha \theta_{k-1} + (1-\alpha) a_k + \omega_k \Delta t \tag{ 2.9}$
其中 $\alpha = 1-K_p\Delta t$
（备注待添加）

2.4 对比和总结

从以上结果的对比可以得到几个发现：
（1）互补滤波和卡尔曼滤波可以得到相同的更新方程，如公式（2.2）和公（2.6）是相同的，就传递函数而言也是一样的。
（2）互补滤波和Mahony&Madgwick滤波器利用相同的传递函数描述。
（3）从（1）和（2）可以知道忙着三种滤波器在传递函数的层面来说是相同的。
（4）除了（3）以外，Mahony&Madgwick滤波器与互补滤波器和卡尔曼滤波器是不相同的，比较方程（2.9），（2.2），（2.6）。
最后一点值得深入讨论，方便起见，再此重新列写两种更新律：
$\theta_k = \alpha \theta_{k-1} + (1-\alpha) a_k + \alpha \omega_k \Delta t, \tag{2.2, 2.6}$
$\theta_k = \alpha \theta_{k-1} + (1-\alpha) a_k + \omega_k \Delta t. \tag{2.9}$
我们可以从两个方面来分析其中的差别，第一,方程（2.2或2.6）可以理解为，首先通过对旋转速率进行积分得到更新的角度，然后用 $a_k$ 进行过滤得到改进的角度，从而得到新的角度。这也可以通过一个方括号表达： $\theta_k = \alpha [ \theta_{k-1} + \omega_k \Delta t ] + (1-\alpha) a_k$ 。与之对比，方程（2.9）可以理解为角度首先被 $a_k$ 进行过滤然后对旋转速率进行积分，对应的用括号表现为： $\theta_k = [\alpha \theta_{k-1} + (1-\alpha) a_k] + \omega_k \Delta t$ 。其次以上方程可以重新整合到算法：

complementary or Kalman filter (1D, 1^st order)	Mahony&Madgwick filter (1D, 1^st order)
$\theta^{-}_k = \theta_{k-1} + \omega_k \Delta t \\[0.5em] e^{-}_k = a_k -\theta^{-}_k \\[0.5em] \theta_k = \theta^{-}_k + K_0 e^{-}_k \tag{2.12}$	$\theta^{-}_k = \theta_{k-1} + \omega_k \Delta t \\[0.5em] e_k = a_k - \theta_{k-1}\\[0.5em]\theta_k = \theta^{-}_k + K_p \Delta t e_k \tag{2.13}$

它们本质上是相同的，除了一个重要的区别，即左边的反馈误差使用的是更新后的角度 $\theta_k^-$ ,但是右边的方程使用了之前的估计角度 $\theta_{k-1}$ ,这两种算法不能直接相互转换，即使他们都是从相同的传递函数推导出来。

3 一维IMU数据融合--二阶（考虑漂移估计）

在本章，通过显性的将陀螺仪的偏置和漂移纳入考虑之中，我们改进了单轴的滤波器。据我所知，在此之前还没有使用互补滤波器完成这个任务的相关描述。因此本章讨论滤波器的顺序将相对于前一章有所变化。

3.1Kalman Filter

如文献【】所述，系统的状态空间模型可以扩展为：
$\theta_k = \theta_{k-1} + ( \omega_k \mp b_{k-1})\Delta t \\[0.5em] b_k = b_{k-1} \\[0.5em] z_k = a_k \tag {3.1}$
其中 $b$ 代表了陀螺仪偏置，我们可以很自然的推出它可以这样修正测量的旋转速率 $\omega_k - b_{k-1}$ ,在此我们可以使用“﹢”号和“-”号，状态向量和矩阵可以变成：
$x = \begin{pmatrix} \theta \\ b \end{pmatrix}, u = (\omega), A = \begin{pmatrix} 1 & \mp \Delta t \\ 0 & 1 \end{pmatrix}, B = \begin{pmatrix} \Delta t \\ 0 \end{pmatrix}, z = (a), H = \begin{pmatrix} 1 & 0 \end{pmatrix}, K = \begin{pmatrix} K_0 \\ K_1 \end{pmatrix}, \tag{3.2}$
卡尔曼方程可以推导为：
$\hat{\theta}^{-}_k = \hat{\theta}_{k-1} + (\omega_k \mp \hat{b}_{k-1})\Delta t \\[0.5em] \hat{\theta}_k = (1-K_0) \hat{\theta}^{-}_k + K_0 a_k \\[0.5em] \hat{b}_k = \hat{b}_{k-1} + K_1 ( a_k - \hat{\theta}^{-}_k ). \tag{3.3}$
跟之前一样，我们将这些方程重新表达，得到更新律为：
$\hat{\theta}_k = \alpha \hat{\theta}_{k-1} + (1-\alpha) a_k + \alpha (\omega_k \mp \hat{b}_{k-1})\Delta t \\[0.5em] \hat{b}_k = \hat{b}_{k-1} + K_1 ( a_k - \hat{\theta}^{-}_k ) \tag{3.4}$
其中 $\alpha = 1-K_0$ 这和一阶的结果中的方程（2.6）很相似，测量的旋转速率被估计的偏置修正，这个偏置反过来又通过对误差的积分得到： $a_k -\hat{ \theta }_k^-$
推论#2:应注意将偏差校正中的符号更改为“+”的效果。修正后的转速为 $\omega_k + \hat{b}_{k-1}$ ，与预期一致，但是方程(3.3)及(3.4)中的偏差更新定律并无更改!因此，结果并不等价于变量替换 ${b}_{k} \rightarrow -\hat{b}_{k}$ 。
评价：你也可以根据文献【】的建议从状态空间向量 $x = (\theta, \omega)^T$ 触发，不过在我们的这个案例中，这明显会导致一个不恰当的滤波器（也就是说:推到出卡尔曼滤波器是一回事，使之很好的工作又是另一回事）。

3.2. Mahony&Madgwick Filter

使用PI控制器可以简化陀螺仪漂移的估计【文献】，传递函数是根据：
$\theta = \left( K_p + K_i \dfrac{1}{s}\right)\dfrac{1}{s} ( a - \theta ) + \dfrac{1}{s} \omega \tag{3.5}$
再次根据位置PID算法的标准实现，我们可以将方程（3.5）拆分成： $e= a-\theta$ , $I = K_i\frac{1}{s}e$ , $\theta = \frac{1}{s}( K_p e + I + \omega )$ ,离散化后得到：
$e_k = a_k - \theta_{k-1} \\[0.5em] I_k = I_{k-1} + K_i \Delta t e_k \\[0.5em] \theta_k = \theta_{k-1} + (K_p e_k + I_k + \omega_k)\Delta t \tag{3.6}$
然后得到更新律：
$I_k = I_{k-1} + K_i \Delta t (a_k - \theta_{k-1}) \\[0.5em] \theta_k = \alpha \theta_{k-1} + (1-\alpha) a_k + (\omega_k + I_k)\Delta t \tag{3.7}$
其中 $\alpha = 1 - K_p \Delta t$
可以看出，只要改变一个不重要的参数，方程（3.7）中的两个方程的顺序就可以颠倒了。

3.3 对比

从2.4章节相同的方面对比二阶的卡尔曼和Mahony&Madgwick算法中 $\theta_k$ 的更新律：

Kalman filter (1D, 2nd order)	Mahony&Madgwick filter (1D, 2nd order)
$\theta^{-}_k = \theta_{k-1} + (\omega_k + b_{k-1}) \Delta t$ $e^{-}_k = a_k - \theta^{-}_k$ $\theta_k = \theta^{-}_k + K_0 e^{-}_k$ $b_k = b_{k-1} + K_1 e^{-}_k$ ——(3.8)	$\theta^{-}_k = \theta_{k-1} + (\omega_k + I_{k-1}) \Delta t$ $e_k = a_k - \theta_{k-1}$ $\theta_k = \theta^{-}_k + K'_p \Delta t e_k$ $I_k = I_{k-1} + K_i \Delta t e_k$ ——(3.9)

应该注意到为了得到这些方程，在Kalman滤波器的bias估计的符号变成了“+”，此外，在Mahony&Madgwick滤波器的方程的顺序被反转了，同时引入了 $K'_p = K_p + K_i \Delta t$
这两个更新定律本质上是相同的，除了一个重要的区别，即Kalman过滤器在误差中使用更新的角度 $\theta^{-}_k$ ，而Mahony&Madgwick过滤器使用之前的角度估计
$\theta_{k-1}$ ，就像之前在一阶过滤器中观察到的(第2.4章)。

3.4互补滤波器

通过解Mahony&Madgwick滤波器的传递函数，可以很容易得到关于 $\theta$ 的互补滤波器：
$\theta = \dfrac{1 + \frac{K_p}{K_i} s}{1 + \frac{K_p}{K_i} s + \frac{1}{K_i} s^2} a + \dfrac{ \frac{1}{K_i} s^2}{1 + \frac{K_p}{K_i} s + \frac{1}{K_i} s^2} \dfrac{1}{s} \omega. \tag{3.10}$
很明显，这个互补滤波器是由二阶滤波器构建的。注意，作用于加速度数据的滤波器实际上由低通加带通滤波器组成。
这个结果产生了有趣的结论。作为二阶滤波器，加速度和转速滤波器的频率响应以谐振频率和阻尼系数为特征：
$\omega_0 = \sqrt K_i, \xi = \dfrac{ K_p }{ 2 \sqrt K_i } \tag{3.11}$
阻尼系数决定了谐振频率的超调。对于高通(和低通)滤波器，在 $\xi >=1$ 时频率响应是平坦的(而阶跃响应是无振荡的)。这表明了标准：
$K_i \le \dfrac{ 1 }{ 4 } K_p^2 \tag{3.12}$
为了避免在陀螺仪通道中超调。同时为了最小化加速度计通道的超调，阻尼应该稍微大于这个值;在平滑频率响应和快速偏差估计之间， $\xi \approx 2$ 可能是一个很好的折衷方案。因此，根据经验， $K_i \approx 0.05...0.1K^2_p$ 。
注意，除非 $K_i$ 被设置为非常小的值，否则交叉频率由 $K_i$ (或其平方根的倒数)决定，而不是像一阶情况那样由 $K_p$ 决定!实际上，可以使用参数 $\omega_0$ 或 $T = 2 \pi / \sqrt{K_i}$ 和 $\xi$ 来代替 $K_p$ 和 $K_i$ ;对用户来说，滤波器的调优可能更直观。
这些考虑显然也适用于Mahony&Madgwick滤波器和Kalman滤波器。
互补滤波器可以像(3.6)那样实现，也可以用任何一种对数字滤波器有利的算法来实现。直接形式II是一个典型的选择。

3.5 一维滤波器的总结

尽管篇幅很长，但在第2章和第3章中对单一轴数据融合的不同方法进行了详细讨论，得出了一个非常简短的总结:

这三种不同的方法实际上并没有那么不同，即使是在二阶情况下。
另外，还得到了选择偏差估计器增益 $K_i$ 或 $K_1$ 的准则，以及作为补充滤波器的更简单和/或更灵活的直接实现。
在计算误差的方式上，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

附录二：IMU实现

最后编辑于：2020.01.15 10:35:48

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 221,635评论 6赞 515
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 94,543评论 3赞 399
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 168,083评论 0赞 360
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 59,640评论 1赞 296
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 68,640评论 6赞 397
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 52,262评论 1赞 308
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 40,833评论 3赞 421
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 39,736评论 0赞 276
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 46,280评论 1赞 319
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 38,369评论 3赞 340
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 40,503评论 1赞 352
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 36,185评论 5赞 350
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 41,870评论 3赞 333
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 32,340评论 0赞 24
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 33,460评论 1赞 272
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 48,909评论 3赞 376
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 45,512评论 2赞 359