FA：formulas and algorithm, FF：fusion and filtering，ESKF：（Error State Kalman Filter）

经典文献：Joan Solà 《Quaternion kinematics for the error-state Kalman filter》，VIO / LIO / IMU+GNSS 组合导航的标准框架，也就是基于误差模型 / 误差状态的卡尔曼滤波。

一、核心思想：名义状态 + 误差状态分离

ESKF 不直接估计真值，把真值拆成两部分：

$${\mathbf{x}}_{true}={\mathbf{x}}_{nominal}\oplus \delta \mathbf{x}$$

• ${\mathbf{x}}_{nominal}$：名义状态，非线性积分传播（IMU 机械编排，四元数积分），不带噪声；
• $\delta \mathbf{x}$：误差状态（小量，切空间向量），是卡尔曼滤波真正估计的变量，线性；
• $\oplus$：状态叠加，旋转是四元数乘法，位置 / 速度 / 偏置是普通加法。

对比传统 EKF：EKF 直接对真值状态线性化；ESKF 在误差空间线性化，误差始终是小量，线性化误差更小，SO (3) 旋转用 3 维微小旋转向量$\delta \theta$，无奇异性，数值稳定性远优于直接 EKF。

IMU 组合导航标准 15 维误差状态（最常用）

$$\delta \mathbf{x}=\left[\begin{array}{c}\delta {\mathbf{p}}_{3\times 1}\\ \delta {\mathbf{v}}_{3\times 1}\\ \delta {\mathbf{\theta }}_{3\times 1}\\ \delta {\mathbf{b}}_{a,3\times 1}\\ \delta {\mathbf{b}}_{g,3\times 1}\end{array}\right]$$

• $\delta \mathbf{p}$：位置误差
• $\delta \mathbf{v}$：速度误差
• $\delta \theta$：姿态微小旋转（SO(3) 切空间，3 维）
• $\delta {\mathbf{b}}_a$：加速度计偏置误差
• $\delta {\mathbf{b}}_g$：陀螺仪偏置误差

名义状态：${\mathbf{x}}_n=[\mathbf{p},\mathbf{v},\mathbf{q},{\mathbf{b}}_a,{\mathbf{b}}_g]$，其中$\mathbf{q}$是四元数。

二、ESKF 完整流程（两大阶段：Predict 预测 + Update 更新 + Reset 重置）

1. Predict：IMU 积分传播名义状态 + 传播误差协方差 P

误差状态$\delta \mathbf{x}$ 预测阶段理论上是 0（只传播协方差）

1.1 名义状态离散传播（IMU 机械编排，dt 为 IMU 采样间隔）

$$\begin{array}{rl}{\mathbf{p}}_{n,k+1}& ={\mathbf{p}}_{n,k}+{\mathbf{v}}_{n,k}\cdot dt+\frac{1}{2}(R({\mathbf{q}}_k)({\mathbf{a}}_m-{\mathbf{b}}_{a,k})+\mathbf{g})d{t}^2\\ {\mathbf{v}}_{n,k+1}& ={\mathbf{v}}_{n,k}+(R({\mathbf{q}}_k)({\mathbf{a}}_m-{\mathbf{b}}_{a,k})+\mathbf{g})dt\\ {\mathbf{q}}_{k+1}& ={\mathbf{q}}_k\otimes \exp \left(\frac{1}{2}({\omega }_m-{\mathbf{b}}_{g,k})dt\right)\\ {\mathbf{b}}_{a,k+1}& ={\mathbf{b}}_{a,k}\\ {\mathbf{b}}_{g,k+1}& ={\mathbf{b}}_{g,k}\end{array}$$

${\mathbf{a}}_m,{\omega }_m$：IMU 测量的加速度、角速度；$R(\mathbf{q})$：四元数转旋转矩阵；$\mathbf{g}$：重力。

1.2 误差状态连续动力学，离散化得到状态转移矩阵(\boldsymbol{F})

$\delta \dot{\mathbf{x}}={\mathbf{F}}_c\delta \mathbf{x}+{\mathbf{G}}_c\mathbf{w}$
离散：
$\delta {\mathbf{x}}_{k+1}=\mathbf{F}\delta {\mathbf{x}}_k+\mathbf{G}{\mathbf{w}}_k$

协方差传播：
${\mathbf{P}}_{k+1|k}=\mathbf{F}{\mathbf{P}}_{k|k}{\mathbf{F}}^T+\mathbf{GQ}{\mathbf{G}}^T$

$\mathbf{Q}$：IMU 噪声对角阵（加速度噪声、陀螺噪声、偏置随机游走）

2. Update：观测到来（GNSS 位置 / 视觉 / 激光里程计），卡尔曼更新误差

2.1、计算观测残差：$\mathbf{r}=\mathbf{z}-h({\mathbf{x}}_{nominal})$

2.2、观测雅可比矩阵$\mathbf{H}$：观测对误差状态$\delta x$求导（不是对名义状态）

2.3、卡尔曼增益：
$$\mathbf{K}={\mathbf{P}}_{k|k-1}{\mathbf{H}}^T{\left(\mathbf{H}{\mathbf{P}}_{k|k-1}{\mathbf{H}}^T+\mathbf{R}\right)}^{-1}$$
$\mathbf{R}$：观测噪声协方差

2.4、 更新误差状态：

$$\delta \hat{\mathbf{x}}=\mathbf{Kr}$$

2.5、更新误差协方差（Joseph 形式保证正定）：
$${\mathbf{P}}_{k|k}=(\mathbf{I}-\mathbf{KH}){\mathbf{P}}_{k|k-1}(\mathbf{I}-\mathbf{KH}{)}^T+\mathbf{KR}{\mathbf{K}}^T$$

3. Reset（ESKF 特有！）：把误差$\delta \hat{x}$回馈到名义状态，重置误差状态为 0

$$\begin{array}{rl}{\mathbf{p}}_n& \leftarrow {\mathbf{p}}_n+\delta \mathbf{p}\\ {\mathbf{v}}_n& \leftarrow {\mathbf{v}}_n+\delta \mathbf{v}\\ {\mathbf{q}}_n& \leftarrow {\mathbf{q}}_n\otimes \exp (\delta \theta /2)\\ {\mathbf{b}}_a& \leftarrow {\mathbf{b}}_a+\delta {\mathbf{b}}_a\\ {\mathbf{b}}_g& \leftarrow {\mathbf{b}}_g+\delta {\mathbf{b}}_g\end{array}$$

误差状态清零：$\delta \mathbf{x}\leftarrow 0$

协方差做重置映射：$\mathbf{P}\leftarrow {\mathbf{G}}_{reset}\mathbf{P}{\mathbf{G}}_{reset}^T$；小角度下${\mathbf{G}}_{reset}\approx I$，工程常近似单位阵。

✅ ESKF 关键区别：每次更新完，误差被打进名义状态，误差状态归零，下一轮继续用小量假设。

三、ESKF 优点总结

1. 旋转用 3 维微小旋转向量，无欧拉角奇异性；四元数仅用于名义状态；
2. 误差始终是小量，雅可比简单，线性化误差小，一致性更好；
3. 分离 IMU 积分（非线性）与滤波更新（线性误差空间），工程模块化；
4. 广泛用于 VIO、LIO、组合导航、机器人定位。

四、ESKF 与 EKF、IEKF 对比

表格

算法	估计对象	旋转表达	线性化位置	特点
EKF	直接估计真值状态	四元数 / 欧拉角	真值空间	容易线性化误差大，奇异
ESKF	估计误差状态 δx	名义四元数，误差 3 维微小旋转	误差切空间	小量、稳定、VIO/LIO 标配
InEKF	不变误差	李群	不变流形	一致性更强，更复杂