5、Error-state kinematics for IMU-driven systems

IMU驱动系统的误差状态运动学

 5.1 动机

我们希望使用Hamilton四元素表示空间方位或姿态，将具有偏置和噪声读数的加速度计和陀螺仪进行积分，实现惯性系统运动学的误差状态方程。

加速度计和陀螺仪的读数通常来自惯性测量单元（IMU）。将IMU读数积分会导致随时间漂移的惯性导航定位系统。避免漂移的关键是将这些信息与绝对位置读数（如GPS或视觉）进行融合。

误差状态卡尔曼滤波器（ESKF）是我们用于此目的的工具之一。在卡尔曼滤波范式中，ESKF的最显著优点包括：

1）方向误差状态是最小的（即它具有与自由度相同的参数数量），避免了与过度参数化（或冗余）相关的问题，以及由于强制约束而导致涉及协方差矩阵的奇异性的风险。这种奇异性通常是由于强制约束而导致的。

2）误差状态系统始终在靠近原点的位置运行，因此远离可能的参数奇异性、万向锁等问题，从而保证线性化的有效性始终存在。

3）误差状态始终很小，这意味着所有二阶产品都可以忽略不计。这使得雅可比矩阵的计算非常简单和快速。有些雅可比矩阵甚至可以是常数或等于可用状态量。

4）误差动态较慢，因为所有大信号动态都已经在名义状态中被整合了。这意味着我们可以以比预测更低的速率应用KF校正（这是观察误差的唯一手段）。

5.2 误差卡尔曼滤波的解释

在误差状态滤波器的公式中，我们提到真实状态、名义状态和误差状态值，真实状态被表达为名义状态和误差状态的适当组合（线性求和、四元数乘积或矩阵乘积）。这个想法是将名义状态视为大信号（可在非线性方式下积分），将误差状态视为小信号（因此可线性积分并适用于线性高斯滤波）。

误差状态滤波器可以解释如下。一方面，高频IMU数据um被积分到一个名义状态x中。这个名义状态不考虑噪声项w和其他可能的模型不完美性。因此，它会累积误差。这些误差被收集在误差状态δx中，并用误差状态卡尔曼滤波器（ESKF）进行估计，这次包括所有噪声和扰动。误差状态由小信号幅度组成，其演化函数由一个（时变的）线性动态系统正确定义，其动态、控制和测量矩阵从名义状态的值计算而来。与积分名义状态并行的是，ESKF预测误差状态的高斯估计。它只是预测，因为现在没有其他测量可用于校正这些估计。滤波器校正是在除IMU之外的信息到达时进行的（例如GPS、视觉等），这些信息能够使误差可观测，通常比积分阶段低得多。这种校正提供了误差状态的后验高斯估计。在此之后，误差状态的均值被注入到名义状态中，然后重置为零。误差状态的协方差矩阵方便地更新以反映这个重置。系统会一直这样运行下去。

5.3 连续时间下的系统运动学

表格3总结了所有涉及变量的定义。值得一提的是，有两个重要的关于惯例的决定：

1）角速率ω是相对于名义四元数在局部定义的。这使我们能够直接使用陀螺仪测量值ωm，因为它们提供了与机体相关的角速率。

2）角度误差δθ也可以根据名义方向在本地定义。虽然这不一定是最优的方法，但它对于大多数IMU集成工作来说是一种经典的选择。有证据表明，全局定义的角度误差具有更好的性质。本文档的第7节也将探讨这一点，但是这里大部分的发展、示例和算法都是基于本地定义的角度误差。

5.3.1 真值状态运动学

真运动学方程如下：

这里，真实的加速度a_t和角速率ω_t是从一个IMU中以身体坐标系中的噪声传感器读数a_m和ω_m的形式获得的。

变换一下方程，将真值提取出来，

把上面的内容代入，得到的运动学系统是：

，系统状态为xt，u为IMU带有噪声的读数，以及一个高斯噪声w，定义如下

上述公式中需要注意的是，重力向量g_t将由滤波器进行估计。它具有恒定的演化方程式(235f)，因为其大小被认为是恒定的。系统从一个固定且任意已知的初始方向q_t(t=0)=q_0开始，由于通常不在水平平面上，因此初始重力向量通常是未知的。为了简化起见，通常取q_0=(1,0,0,0)，因此R_0=R{q_0}=I。

我们估计在q0坐标系下表达的gt，而不是在水平坐标系下表达的qt，这样可以将初始方向的不确定性转移到重力方向的初始不确定性上。我们这样做是为了提高线性度：事实上，方程（235b）现在是关于g线性的，而g承载了所有的不确定性，初始方向q0是已知的，没有不确定性，因此q从一开始就没有不确定性。一旦估计出重力向量，水平平面就可以恢复，如果需要，整个状态和恢复的运动轨迹可以重新定向以反映估计的水平面。有关进一步的理由，请参见（Lupton和Sukkarieh，2009）。当然，这是可选的，读者可以从系统中删除所有与重力有关的方程，并采用更传统的方法，即将g，（0，0，-9.8xx）视为实验现场重力向量的适当小数位数，并采用不确定的初始方向q0。

5.3.2 名义状态运动学

名义状态运动学对应于没有噪声或扰动的模型系统，

5.3.3 误差状态运动学

目标是确定误差状态的线性化动态。对于每个状态方程，我们编写其组合（在表3中），解决误差状态并简化所有二阶无穷小量。我们在此给出完整的误差状态动态系统，然后进行评论和证明。

方程（238a）、（238d）、（238e）和（238f）分别表示位置、两个偏差和重力误差，它们是从线性方程推导出来的，它们的误差状态动态是微不足道的。

作为一个例子，考虑表3中的真实位置方程(235a)和名义位置方程(237a)，它们的组合公式pt = p + δp，解出δp得到(238a)。

方程（238b）和（238c）给出了速度和方向误差的表达式，需要对非线性方程（235b）和（235c）进行一些非平凡的变换才能得到线性化动力学方程。它们的证明在接下来的两个部分中进行。

方程（238b）：线性速度误差。我们希望决定δv^.的导数,速度误差的运动学，从以下关系开始

在公式（240）中，我们将（237b）进行了重写，引入了aB和δaB，它们分别定义为机体坐标系下的大信号加速度和小信号加速度。其中，（239）是Rt的小信号近似值。

为了将惯性参考系中的真实加速度写成大信号和小信号项的组合。

我们可以通过写出v̇t的表达式(235b)的两种不同形式（左侧和右侧展开式）来进行推导，忽略了O(||δθ||²)项。

消除二阶项以及重新组织叉乘

最后得到，

为了进一步简化这个表达式，我们通常可以假设加速度计的噪声是白噪声、不相关和各向同性的24。

下面证明噪声乘以旋转矩阵还是等于噪声

最后得到，

Equation (238c):方向误差。希望去决定δθ的导数，角度误差的运动学。从以下关系开始

四元素导数的真值和名义定义的导数。

与加速度一样，为了清晰起见，我们将角速率中的大信号和小信号项分组。

wt可以写为一个名义部分和误差部分，

我们通过两种不同的方法（左侧和右侧展开）计算 q̇t。

移项简化q并分离δq的导数，我们得到：

(257a)式子是一个二阶无穷小量，不是很重要，第二个方程，在忽略二阶小量之后，得到，

5.4 离散时间下的系统运动学

上述微分方程需要被积分成差分方程，以考虑离散时间间隔∆t>0。积分方法可能会有所不同。在某些情况下，可以使用精确的闭式解。在其他情况下，可以采用不同精度的数值积分方法。请参考附录中有关积分方法的相关细节。

需要对以下子系统进行积分：

1、名义状态。

2、误差状态。

a）确定性部分：状态动力学和控制。

b）随机部分：噪声和扰动

5.4.1 名义状态运动学

我们可以将名义状态的差分方程写成以下形式：

5.4.2 误差状态运动学

确定性部分通常是正常积分的（在这种情况下，我们遵循附录C.2中的方法），而随机部分的积分会导致随机脉冲（请参见附录E）。

这里，v i，θ i，a i和ω i是应用于速度、方向和偏差估计的随机脉冲，由白色高斯过程建模。它们的平均值为零，它们的协方差矩阵是通过将 a n，ω n，a w和ω w的协方差积分到步长时间∆t中获得的（见附录E）。

5.4.3 误差状态的雅可比矩阵和扰动矩阵

雅可比矩阵可以通过对前一节中的误差状态差分方程进行简单的观察得到。

为了以紧凑的形式写出这些方程，我们考虑名义状态向量x、误差状态向量δx、输入向量um和扰动脉冲向量i，具体如下（有关详细信息和理论基础，请参见附录E.1）。

 

现在误差状态系统如下

ESKF预测方程如下：

上面的雅可比矩阵和协方差矩阵的表达式如下所述。这里出现的所有与状态相关的值都直接从名义状态中提取。

请特别注意，F x是系统的转移矩阵，可以通过多种方式以不同的精度进行近似。我们在这里展示了其中最简单的形式（欧拉形式）。请参见附录B至D以获取更多参考资料。

 请注意，由于误差δx的平均值初始化为零，线性方程（268）始终返回零。当然，您应该在代码中跳过第268行。我建议您编写它，但是将其注释掉，以确保您没有遗漏任何内容。

 最后，请注意，您不应跳过协方差预测（269）！！实际上，Fi Qi Fi^T这个术语不为零，因此这个协方差会不断增长 - 正如在任何预测步骤中必须的那样。