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

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

6. 用将IMU与互补的感知数据融合

在除了IMU之外的其他信息（例如GPS或视觉）到达时，我们将进行ESKF校正。在设计良好的系统中，这应使IMU偏差可观察到，并允许ESKF正确估计它们。有无数种可能性，最受欢迎的是GPS + IMU，单目视觉+ IMU和立体视觉+ IMU。近年来，视觉传感器与IMU的结合引起了很多关注，并因此引起了很多科学研究。这些视觉+ IMU设置非常适合在GPS受限的环境中使用，并且可以在移动设备（通常是智能手机）上实现，也可以在无人飞行器和其他小型敏捷平台上实现。

    到目前为止，IMU信息已用于对ESKF进行预测，其他信息可用于校正滤波器，从而观察IMU偏置误差。校正包括三个步骤：
1，通过滤波校正观测误差状态
2，将观测误差引入标称状态
3，重置误差状态
这些步骤将在下面的章节中展开。

6.1 通过滤波校准的误差状态观测

像往常一样，假设我们有一个传感器来传递由状态决定的信息，例如
$$\mathbf{y}=h({\mathbf{x}}_t)+v,\text{\hspace{1em}(271)}$$

其中，$h()$是系统状态（真实状态）的一个常用的非线性函数，而$v$是一个协方差为$\mathbf{V}$的白高斯噪声，
v ∼ N { 0 , V }   . (272) v \sim \mathcal{N}\{0, \mathbf{V}\}~.\tag{272} v∼N{ 0,V} .(272)

    我们的滤波器估计误差状态，因此滤波校正方程【26】，

    【注26，我们这里给出协方差更新的最简单的形式，$\mathbf{P}\leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}$。已知这种形式的数值稳定性差，因为其结果既不能保证是对称的，也不能保证是正定的。读者可以使用更稳定的形式。例如1），对称的形式$\mathbf{P}\leftarrow \mathbf{P}-\mathbf{K}(\mathbf{H}\mathbf{P}{\mathbf{H}}^{\top }+\mathbf{V}){\mathbf{K}}^{\top }$和2）对称且正定的Joseph形式 $\mathbf{P}\leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}{\mathbf{H}}^{\top })+\mathbf{K}\mathbf{V}{\mathbf{K}}^{\top }$】

$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^{\top }({\mathbf{H}\mathbf{P}\mathbf{H}}^{\top }+\mathbf{V}{)}^{-1}\text{\hspace{1em}(273)}$$

$$\widehat{\delta \mathbf{x}}\leftarrow \mathbf{K}(\mathbf{y}-\mathbf{h}({\hat{\mathbf{x}}}_{\mathbf{t}}))\text{\hspace{1em}(274)}$$

$$\mathbf{P}\leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}\text{\hspace{1em}(275)}$$

需要针对误差状态$\delta \mathbf{x}$定义雅可比矩阵$\mathbf{H}$，并以最好的真实状态${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \widehat{\delta \mathbf{x}}$进行评估。因为在这一阶段的误差状态均值为0（因为我们尚没有观测），我们有${\hat{\mathbf{x}}}_t=\mathbf{x}$且我们可以使用标称误差$\mathbf{x}$作为评估点，得到
$$\mathbf{H}\equiv \frac{\partial h}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}\text{\hspace{1em}(276)}$$

6.1.1 用于滤波器校正的雅可比计算

上面的雅可比行列式可以通过多种方式来计算。最有说明性的是通过使用链式规则，
$$\mathbf{H}\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}=\frac{\partial h}{\partial {\mathbf{x}}_t}{|}_{\mathbf{x}}\frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}={\mathbf{H}}_{\mathbf{x}}{\mathbf{X}}_{\delta \mathbf{x}}.\text{\hspace{1em}(277)}$$

这里，${\mathbf{H}}_x\triangleq \frac{\partial h}{\partial {\mathbf{x}}_t}{|}_{\mathbf{x}}$是$h()$相对于自己的参数的最标准的雅可比（即，将在正规的EKF中使用的雅可比）。链式法则的第一部分取决于所使用的特定传感器的测量函数，此处未介绍。

    第二部分，${\mathbf{X}}_{\delta \mathbf{x}}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}$，是真实状态相对于误差状态的雅可比。这一部分可以被提取出来，因为它只取决于状态的ESKF的组成。我们有，
X δ x = [ ∂ ( p + δ p ) ∂ δ p ∂ ( v + δ v ) ∂ δ v 0 ∂ ( q ⊗ δ q ) ∂ δ θ ∂ ( a b + δ a b ) ∂