Hello! 欢迎来到我的博客 今天的内容关于机器人中常用的传感器IMU，我们用它来实现机器人姿态、速度、位置的估计。 今天将会介绍使用低成本IMU进行机器人运动估计的一个常用方法——ESKF。
 

3 Error State Kalman Filter（ESKF介绍）

3.1 动机与流程

使用IMU的初衷是为了求解载体的位置、速度、姿态等系统状态，然而由于IMU测量数据（包括加速度计和陀螺仪的测量数据）都带有大量噪声，直接利用IMU运动方程进行积分会随着时间发生显著的漂移。因此，为了避免漂移我们需要融合其他传感器的信息（如GPS、视觉），对IMU运动积分的结果进行修正。   Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：
  • true state：实际状态，系统实际的运行状态
  • nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
  • error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）
  基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。   ESKF的全过程可以描述为如下步骤：
  • 对高频率的IMU数据um\mathbf{u}_m进行积分，获得nomimal state x\mathbf{x}。（注意：nominal state里面不考虑噪声，因此势必会累积误差。这部分误差可依据error state进行修正）
  • 利用Kalman Filter估计error state，这里同样也包括时间更新和量测更新两部分。（注意：这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波）
  • 利用error state修正nominal state，获取true state
  • 重置error state
  使用ESKF的优势：
  1. error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）引起的协方差矩阵奇异的风险。
  2. error state 总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。
  3. error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。
  4. error state 的变化平缓，因此KF修正的频率不需要太高。

3.2 状态定义

所有在ESKF中用到的变量定义如下：     图片中定义了所以状态，包括true state，nominal state以及error state。以及error state和nominal state组合获得true state的方式。   另外，图中的各个运算符在上一篇文章中均已有了定义。除了一个叉乘矩阵的符号，其定义如下：    
图片来自于参考文献[1]的P31。
 

3.3 Nominal State 积分更新（每个时间步都要执行）

3.3.1 Nominal state 方程（连续时间）

    其中，R=R{q}是基于q成的旋转矩阵，表示从IMU系到惯性系的旋转。   注意到，这组方程完全没有考虑噪声。这组方程的推导是显而意见的，只需要在IMU运动方程的基础上，假设噪声项全部为0即可。  

3.3.2 Nominal state 方程（离散时间，用于写程序）

    其中，定义 ，所以：     其中，  
实际上，上面这个操作就是把角度增量改造成单位四元数，两个单位四元数相乘仍然是单位四元数，这样才能保证 q 始终为单位四元数，保证迭代是收敛的。
 

3.4 Error State 时间更新（每个时间步都要执行）

3.4.1 Error state 方程（连续时间）：

    这个误差方程，考虑了噪声等不确定性。其形式看起来似乎不太直观，但实际上推导也很简单。   比如，以位置为例：     所以其导数为：     再比如以速度为例：     所以其导数为：     其中，用到了。此外，还用到了两个小量相乘可以忽略的假设。  
其它方程的推导，就不在此说明了，可以参考文献[1]的P33页。
 

3.4.2 Error state 方程（离散时间，用于写程序）：

  定义状态向量：     于是误差状态方程可以写作：     其中:     中，vi,θi,ai,ωi 代表随机脉冲，他们的均值为 0，协方差矩阵为对角阵。   另外，定义：     这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。  

3.4.3 对于Error state的卡尔曼滤波器的时间更新（用于写程序）

  误差状态更新     协方差矩阵更新     这个就是很基础的卡尔曼滤波方程了，不了解的可以移步我的博客：机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（上）  

3.5 Error State 量测更新（只有测量值到来的时间步需要执行）

由于IMU的数据中夹带了大量噪声，我们需要利用更多的传感器的信息，来对状态进行修正。   而这些传感器中常用的组合包括：GPS+IMU，单目视觉+IMU，双目视觉+IMU等。  

3.5.1 量测方程

  通常，量测方程的基本形式如下：     其中 h( ) 是量测函数，与传感器有关。v是高斯白噪声，为   为了实现量测更新，我们需要求 h 对 δx 的导数，所以有：     其中是量测函数的雅可比矩阵，根据传感器的不同而具有不同形式。   而则可以表示为：    
关于上式的详细推导，可以参考文献[1]的P39页。
 

3.5.2 对于Error state的卡尔曼滤波器的量测更新（用于写程序）

  卡尔曼增益的计算     误差状态量测更新     协方差矩阵更新     这是卡尔曼滤波的基础方程，就不做介绍了。

3.6 状态合并以及Error State 重置（在量测更新过后执行）

3.6.1 状态合并

  这一步是要把error state与nominal state合并，即修正nominal state随时间的漂移。     即：    

3.6.2 Error state 重置

  由于nominal state的误差已经修正了，所以error state要置零，同时协方差矩阵也要相应的修改。   通常来说，重置的方法如下：    

参考文献

[1] Joan Sol`a. Quaternion kinematics for the error-state KF. Sep 12, 2016.  
关于<IMU运动方程与ESKF原理介绍>的上下两篇文章主要内容出自上面这篇参考文献，我只是进行了翻译和重新梳理逻辑顺序的工作。因此，大家如果使用到本博客内容，也烦请引用上述参考文献。谢谢~