进一步掌握误差状态卡尔曼滤波（ESKF）

ESKF（Error-State Kalman Filter, ESKF）误差状态卡尔曼滤波，本质上仍是卡尔曼滤波，但核心思想与传统卡尔曼滤波有所不同。

1. 核心原理 - 状态分离

ESKF的核心思想是将状态分解为两个部分：

1. 名义状态: 用 x 表示一个简化的状态，通常通过IMU等传感器的积分（如加速度计积分得到速度，速度积分得到位置）来直接预测，这个非线性的积分过程，忽略了各种误差源（如 bias、噪声）。名义状态可以快速推算，但精度低，会随着时间发散。
2. 误差状态 : 用 δx 表示名义状态与真实状态xt 之间的差值：δx=xt−x，ESKF并不直接估计系统的完整状态，而是专注于估计这个误差状态 δx。

这种误差状态的建模方式带来了几个关键优势：

• 线性化效果更好： 误差状态 δx通常是一个很小的量（因为名义状态 x 已经接近真实状态），在卡尔曼滤波的更新步骤中，需要对观测模型进行线性化，对一个在零点附近（误差很小）的函数进行线性化，其近似精度远高于对一个可能在任何地方的函数（状态值可能很大）进行线性化，这使得ESKF对非线性系统有更好的表现。
• 误差动态更简单： 误差状态的动态变化（即误差如何传播）通常比完整状态的动态变化更简单、更接近线性。
• 与IMU工作机制完美契合： IMU提供的是加速度和角速度的增量/误差信息，而不是绝对信息，ESKF的“预测-更新”循环天然地匹配了这个过程，在预测阶段用IMU数据积分得到名义状态，在更新阶段用GPS、视觉等绝对观测来估计这个积分过程的误差 δx。将估计出的误差补偿给名义状态，得到一个修正后的、更精确的最优估计，同时将误差状态重置为零。

2. 主要步骤

ESKF的运行过程也遵循卡尔曼滤波的“预测-更新”循环，但步骤上更为精细，以一个融合IMU和GPS的系统为例。

状态定义：

步骤一：预测 - 由IMU的高频数据驱动

• 名义状态预测（非线性）： 使用IMU的原始测量值 ω~, a~和当前的偏离估计值，对名义状态进行积分。

这里 R(q)是从四元数到旋转矩阵的转换，g是重力向量，这个过程是确定性的（忽略了噪声），会产生漂移。

• 误差状态协方差预测（线性）： 推导出误差状态的线性化连续时间动态方程： δx˙=Fδx+Gi 其中：F是误差状态方程关于误差状态 δx的雅可比矩阵（状态转移矩阵）。 G是噪声驱动矩阵。 i{i}i 是输入噪声向量（IMU的测量白噪声）。 使用 F和 G来预测误差状态的协方差矩阵 P：

其中 Φk≈exp⁡(FΔt) 是离散时间的状态转移矩阵，Qd是离散化的过程噪声协方差矩阵。

在预测步骤中，只更新了误差状态的协方差 P，而误差状态 δx 的均值（估计值）被重置为零，因为还没有进行观测更新，所以最佳误差估计就是零。

步骤二：更新 - 由GPS、视觉里程计等低频绝对观测数据驱动。

1. 计算卡尔曼增益：

H是观测矩阵，将误差状态 δx映射到观测空间，需要通过求导得到：观测值对误差状态的雅可比矩阵。 R是观测噪声协方差矩阵。

1. 观测误差状态： 计算观测的残差：

z 是实际的传感器观测值（如GPS给出的位置） h(x)是观测模型，使用当前的名义状态 x来预测观测值。 这个残差本质上就是观测到的“误差”，正是需要用误差状态来解释的量。

1. 更新误差状态估计和协方差：

现在，得到了误差状态的最佳估计 δx。

步骤三：注入和重置 ：ESKF独有的步骤

1. 注入：将估计出的误差状态 δx注入到名义状态 x{x}x 中，从而修正名义状态，得到最优估计： x←x⊕δx 这里的 ⊕是一个广义的加法，对于位置、速度就是普通加法，对于姿态四元数，是四元数乘法（对应旋转向量的指数映射）。
2. 重置：由于已经将误差补偿给了名义状态，此时名义状态是最优估计，误差状态就应该归零： δx←0 同时，误差状态的协方差矩阵 P 也需要根据注入操作进行相应的更新（因为误差状态的定义基准发生了变化），以确保一致性。

之后，循环继续，用新的名义状态和重置后的误差状态进行下一轮的预测。

3. 对比

EKF：是基准方法，思想直接，在非线性程度不高、模型简单的情况下是一个快速有效的选择，但当系统非线性强时，其精度和稳定性问题突出。

UKF：是EKF的一个强大替代品，当面对一个强非线性系统，且状态维数较低，同时又不想或难以计算复杂的雅可比矩阵时，UKF是首选。

ESKF：专用于导航、机器人、自动驾驶领域，是为IMU（惯性测量单元） 量身定做的，如果系统核心是高频的IMU（提供预测），并辅以低频的绝对观测（如GPS、相机、激光雷达提供更新），那么ESKF几乎是不二之选。