上一节，介绍了卡尔曼滤波的基本原理，但在SLAM中却使用ESKF，让我们一起看看具体的原因是什么吧

一、误差卡尔曼滤波器ESKF(Error State Kalman Filter)

1.1动机

在常规的卡尔曼滤波器中，需要假定系统的状态服从高斯分布，这要求系统的状态是定义在向量空间中，满足向量空间中的运算法则。但SLAM中的一些状态变量是位于流形长的，例如旋转矩阵、四元数、和李群。直接在欧几里得空间进行线性化和状态更新可能导致无效的状态，且这些数据不适合欧几里得空间的计算。
例如如果简单的将两个四元数相加，或者在四元数上乘上一个标量常数，其结果都不满足模长为一的约束条件。也就是说两个四元数相加后不再是一个能表示旋转的四元数了。对于旋转矩阵也有着相同的问题。如果用欧拉角和角轴来表示旋转，会出现“奇点”的问题。所以需要一种新的方法来传递非线性误差。

1.2 流形

1.2.1 动机

• 数据冗余
  我们所能观察到的数据实际上是由一个低维流形映射到高维空间上的。由于数据内部特征的限制，一些高维中的数据会产生维度上的冗余，实际上只需要比较低的维度就能唯一地表示。
  例子：表达一个圆
  如果使用二维坐标系上的一堆点去表达一个圆，二维坐标系有许多没有用到的点，这是冗余的表达方式，完全可以使用一维流形来表示
  
• 数据本质
  我们使用三维空间表达一个地球仪，现在让你测量南京到纽约的距离，你一定会从地球仪的外部测量而不是直接穿过地球仪去测量直线距离，三维球体其实是一个二维流形，流形可以反映一个数据的本质。
  
  
  
  如果我们观察到的数据是三维的，但其本质是一个二维流形。图上所标注的两个圈圈，在流形（把卷展开）上本距离非常远，但是用三维空间的欧氏距离来计算则它们的距离要近得多。
  
  

  1.2.2 概念

• 流形
  流形表示提供了一种更灵活和高效的方式来处理高维数据，能够更好地保留数据的局部结构和拓扑特征，从而在某些情况下比传统的几何空间表示更适用。在数学中，流形是局部类似于欧几里得空间的空间，但在全局上可能具有更复杂的拓扑结构的对象。简单来说，流形就是一种局部上类似于欧几里得空间的空间。对于流形上的每个点，都存在一个领域，可以与欧几里得空间中的一个开集建立一一对应的关系，使得在这个对应下，拓扑结构相同、连续映射也相同。流形可以是各种维度的，比如曲线、曲面、高维空间等。
• 正切空间
  在流形上的每个点，我们可以定义一个与该点相切的线性空间，这个空间就是正切空间。正切空间是局部近似流形的一个线性空间，它包含了所有可能的在该点的切向量。正切空间可以看作是流形在某一点的“平坦”近似。例如，对于一个二维球面（例如地球表面）上的一点，正切空间就是过这一点的平面。我们可以在这个平面上进行线性运算，例如向量加法和标量乘法。

  1.2.3 应用

1. 非线性降维：降维的过程中不但考虑到了距离，更考虑到了生成数据的拓扑结构。
2. 流形能够刻画数据的本质：将数据从高维空间降维到低维空间，还能不损失信息。
3. GAN网络
   前面提到了高维数据其实是由低维流形生成的。如果我们能模拟这个生成过程，再通过对低维流形的微调，应该能得到对应的“有意义且有道理”的高维数据。利用生成对抗网络（GAN）生成的人脸。
   
4. 计算机信息存储
   身份证号。我们的身份证号是18位的。但是身份证号有严格的格式：3位省3位市8位生日4位其他编码。这18位，最多能出现 10的18次方个不同的编码。但由于格式的限制，很多编码是不可能产生的。例如“999888777766554321”这就不可能出现。但我们可以给每一个新出生的人一个连续的编码。只需要10位数字，就足够代表100亿人了。所以当我们有某种方法，能够找到一个从18维空间映射到10维空间的方法。那么这个10维空间就是数据“最本质”的特征了。

   1.3 ESKF概念

   1.3.1核心思想

   ESKF 是一种特别适用于处理状态空间位于流形上的扩展卡尔曼滤波方法。它通过引入误差状态，将状态的更新和传播都限制在流形上，从而更准确地处理非线性。基于误差状态的扩展卡尔曼滤波器（ESKF）通过将系统状态分解为流形中的状态量和正切空间中的误差量，并结合误差状态的预测和更新，提供了一种高效且准确的状态估计方法。具体步骤包括预测、更新和线性化操作，所有步骤都以误差状态为核心，使得估计更加精确和稳定。
   ESKF的核心在于将系统状态分解为两个组成部分：将状态建模为流型中的状态量（非随机变量）加上正切空间中的误差量（均值为零的高斯分布）它是一种间接优化方法。
   
   

   1.3.2 步骤

   设系统的真实状态为 ，被分解为估计状态x ̂_k和误差状态δx_k
   
   
   
   x ̂_k是在流形上的状态估计。状态变量位于一个非线性空间（流形）上，例如旋转矩阵或四元数。
   δx_k误差量位于正切空间中，可以看作是一个线性化的小扰动。这些误差量服从均值为零的高斯分布。
   ⊕广义加，流形上的状态更新操作。它用于将误差状态加到估计状态上，从而得到新的估计状态。

• 预测
  预测状态
  
  
  预测误差状态
  
  
  
  预测协方差矩阵
  
  
• 更新
  计算卡尔曼增益
  
  
  误差更新
  
  
  
  状态更新
  
  
  
  更新协方差矩阵
  
  
• 优势

1. 处理非线性系统
   很多实际系统是非线性的，例如机器人导航、无人机飞行控制和计算机视觉中的姿态估计等。传统的扩展卡尔曼滤波器（EKF）通过线性化处理非线性，但这种线性化在某些情况下可能导致估计不准确或不稳定。
2. 流形上的状态估计
   一些状态变量自然地位于流形上，例如旋转矩阵、四元数、和李群。直接在欧几里得空间进行线性化和状态更新可能导致无效的状态（例如，旋转矩阵不再是正交的）。ESKF通过在流形上表示和处理状态，使得状态更新更加合理。
3. 减少线性化误差
   ESKF通过引入误差状态，避免在状态本身上进行直接的线性化。误差状态通常表示为小的增量（在李代数上），这使得线性化误差更小，更准确。
4. 提高数值稳定性
   在处理高度非线性和高维系统时，传统EKF可能会遇到数值稳定性问题。ESKF通过在流形上进行误差传播和状态更新，可以提高数值稳定性，减少由于线性化带来的数值误差。
5. 更好的处理高维和复杂动态系统
   对于复杂的高维系统，直接在状态空间进行卡尔曼滤波可能会导致计算复杂度高且效果不理想。ESKF通过在误差状态空间进行处理，能够更高效地处理复杂系统。