文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第5章

基于误差状态卡尔曼滤波器的四元数运动学

5. 基于IMU驱动系统的误差状态运动学

5.1 运动

我们希望使用汉密尔顿（Hamilton）四元数表示空间中的方向或者attitude（姿态），编写将加速度计和陀螺仪读数与偏置和噪声相结合的惯性系统运动学的误差方程。

加速度计和陀螺仪的读数通常来自惯性测量单元（IMU）。整合IMU读数可以得到随着时间漂移的航位推算定位系统。避免漂移就是将这些信息与GPS或视觉等绝对位置读数融合在一起。

误差状态卡尔曼滤波（ESKF）是我们基于这个目的可能使用的其中的一个工具。在卡尔曼滤波范例中，这些是ESKF最显着的价值(Madyastha et al., 2011)。

• 方向误差状态极小（即，它具有与自由度相同的参数数量），避免了与过度参数化（或冗余）有关的问题以及由此产生的相关协方差矩阵奇异性的风险（通常由强制导致）约束。
• 误差状态系统始终在接近原点的位置运行，因此远离可能的参数奇异性，万向节锁定问题等，从而保证了线性化有效性始终保持不变。
• 误差状态始终很小，这意味着所有二阶乘积都可以忽略不计。这使得Jacobian的计算非常容易和快速。一些雅可比行列甚至可以是恒定的或等于获得的状态的大小。
• 误差动态很慢，因为所有大信号动态都已集成在标称状态。这意味着我们可以以比预测更低的速率来进行KF校正（这是观测误差的唯一方法）。

5.2 误差状态卡尔曼滤波解释

在误差状态滤波器公式中，我们说的是真实状态，标称状态和误差状态的值，真实状态被表示为标称状态和误差状态的适当组合（线性和，四元数乘积或矩阵乘积） 。其思想是将标称状态视为大信号（以非线性方式可积分），将误差状态视为小信号（因此可线性积分并适用于线性高斯滤波）。

    误差状态滤波器可以解释如下。一方面，高频IMU数据${\mathbf{u}}_m$被集成到标称状态$\mathbf{x}$中。该标称状态未考虑噪声项$\mathbf{w}$和其他可能的模型缺陷。结果，它将积累误差。这些误差收集在误差状态$\delta \mathbf{x}$中，并通过误差状态卡尔曼滤波器（ESKF）进行估算，这一次包含了所有的噪声和扰动。误差状态由小幅度的信号组成，其演化函数由（时变的）线性动态系统正确定义，其动态，控制和测量矩阵均由标称状态的值计算得出。在结合标称状态的同时，ESKF预测误差状态的高斯估计。它只是预测，因为目前尚无其他度量可用来校正这些估计。滤波器校正是在IMU以外的信息（例如GPS，视觉等）到达时执行的，该信息能够使误差可观，并且执行频率通常比积分阶段低得多。校准提供了误差状态的后高斯估计。此后，误差状态的平均值被结合到标称状态，然后重置为零。错误状态的协方差矩阵可以方便地更新以反映此重置。系统永远这样运行下去。

表3：误差状态卡尔曼滤波中的所有变量

5.3 持续时间的系统运动学

所有涉及到的变量的定义被总结到表3中。关于约定的两个重要决定值得一提：

• 相对于标称四元数的角速率$\mathbf{\omega }$是局部定义的。这允许我们直接使用陀螺仪测量${\mathbf{\omega }}_m$，因为它们提供了本体参考的角速率。
• 相对于标称四元数的角速率误差$\delta \mathbf{\omega }$也是局部定义的。这不一定是最佳的进行方法，但与大多数IMU集成工作中的选择相对应-我们称之为经典方法。这些存在的证据(Li and Mourikis, 2012)提到全局定义的角速度误差有更好的属性。这在文章的第7章会被讨论，但是这里大部分的开发，例子和算法都是基于局部定义的角误差。

5.3.1 真实状态运动学

真实运动学方程如下
$${\dot{\mathbf{p}}}_t={\mathbf{v}}_t\text{\hspace{1em}(229a)}$$

$${\dot{\mathbf{v}}}_t={\mathbf{a}}_t\text{\hspace{1em}(229b)}$$

$${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\text{\hspace{1em}(229c)}$$

$${\dot{\mathbf{a}}}_{bt}={\mathbf{a}}_w\text{\hspace{1em}(229d)}$$

$${\dot{\mathbf{\omega }}}_{bt}={\mathbf{\omega }}_w\text{\hspace{1em}(229e)}$$

$${\dot{\mathbf{g}}}_t=0\text{\hspace{1em}(229f)}$$

这里，真实的加速度${\mathbf{a}}_t$和角速率${\mathbf{\omega }}_t$从IMU中获得，在本体坐标系中以带有噪声的读数${\mathbf{a}}_m$和${\mathbf{\omega }}_m$，即【23】，
$${\mathbf{a}}_m={\mathbf{R}}_t^{\top }({\mathbf{a}}_t-{\mathbf{g}}_t)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\text{\hspace{1em}(230)}$$

$${\mathbf{\omega }}_m={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n.\text{\hspace{1em}(231)}$$

    【注23，通常的做法是在（231）中描述的旋转运动学中忽略地球的旋转速度${\mathbf{\omega }}_{\epsilon }$考虑到非零的地球旋转速率，在绝大多数实际情况下过于复杂的。但是，我们注意到，当使用噪声和偏置非常小的高端IMU传感器时，${\mathbf{\omega }}_{\epsilon }=15°/h\approx 7.3\times 10^{-5}rad/s$ 的值可能变得可以直接测量；在这种情况下，为了保持IMU误差模型的有效性，公式中不应忽略比率${\mathbf{\omega }}_{\epsilon }$。】

【注，根据本人学习的关于IMU预积分方面的一些知识，噪声项里面，${\mathbf{a}}_b,{\mathbf{\omega }}_b$用下标b表示的噪声，应该是bias的缩写，即随时间缓慢变化的噪声；以n为下标的噪声应该是指的白噪声】

其中 R t ≜ R { q t } \mathbf{R}_t \triangleq \mathbf{R}\{\mathbf{q}_t\} Rt​≜R{ qt​}。对此，可以分离出真实值（这意味着我们把测量方程反过来了），
$${\mathbf{a}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n)+{\mathbf{g}}_t\text{\hspace{1em}(232)}$$

$${\mathbf{\omega }}_t={\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n.\text{\hspace{1em}(233)}$$

代入上面，得到运动学方程
$${\dot{\mathbf{p}}}_t={\mathbf{v}}_t\text{\hspace{1em}(234a)}$$

$${\dot{\mathbf{v}}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n)+{\mathbf{g}}_t\text{\hspace{1em}(234b)}$$

$${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n)\text{\hspace{1em}(234c)}$$

$${\dot{\mathbf{a}}}_{bt}={\mathbf{a}}_w\text{\hspace{1em}(234d)}$$

$${\dot{\mathbf{\omega }}}_{bt}={\mathbf{\omega }}_w\text{\hspace{1em}(234e)}$$

$${\dot{\mathbf{g}}}_t=0\text{\hspace{1em}(234f)}$$

我们可以称它为${\dot{\mathbf{x}}}_t=f({\mathbf{x}}_t,\mathbf{u},\mathbf{w})$。该方程有状态${\mathbf{x}}_t$，由带有IMU噪声的读数${\mathbf{u}}_m$支配，且由白噪声$\mathbf{w}$扰动，它们都可以被定义成
$${\mathbf{x}}_t=\left[\begin{array}{c}{\mathbf{p}}_t\\ {\mathbf{v}}_t\\ {\mathbf{q}}_t\\ {\mathbf{a}}_{bt}\\ {\mathbf{\omega }}_{bt}\\ {\mathbf{g}}_t\end{array}\right]\mathbf{u}=\left[\begin{array}{c}{\mathbf{a}}_m-{\mathbf{a}}_n\\ {\mathbf{\omega }}_m-{\mathbf{\omega }}_n\end{array}\right]\mathbf{w}=\left[\begin{array}{c}{\mathbf{a}}_w\\ {\mathbf{\omega }}_w\end{array}\right].\text{\hspace{1em}(235)}$$

注意，在以上公式中，重力矢量${\mathbf{g}}_t$将由滤波器估计。它具有一个恒定的演化方程（234f），对应于一个已知为恒定大小的值。系统以固定且任意已知的初始方向${\mathbf{q}}_t(t=0)={\mathbf{q}}_0$开始，它通常不在水平面，使初始的重力向量未知。为了简单，它通常设为${\mathbf{q}}_0=(1,0,0,0)$且因此 R 0 = R { q 0 } =