← 返回博客首页

误差状态卡尔曼滤波：从名义状态到误差注入

这是 ESKF 系列的第一篇。读者不需要先了解 SfM、SLAM、因子图或 IMU 预积分。文章从“如何估计一个看不见的状态”讲起，最后把 Error-State Kalman Filter 的基本结构串起来。

摘要

机器人无法直接读出自己的真实运动状态。通常先用运动模型预测，再用带噪声的传感器观测修正。位置和速度可以用普通向量表示，姿态则属于旋转空间，不能把一个三维向量直接加到旋转矩阵上。

误差状态卡尔曼滤波（Error-State Kalman Filter，ESKF）维护一个按完整非线性模型传播的名义状态，只估计名义状态附近的小误差状态。滤波器更新的是误差，更新结果再注入名义状态，随后重新定义局部误差。这样，非线性运动和局部线性估计各自处理自己擅长的部分，旋转状态也能保持合法。

下面只追踪一条闭环：先定义状态，再传播误差、利用观测更新误差，最后把误差注入名义状态并完成 reset。IMU 噪声标定、Allan 方差、预积分和完整视觉观测模型不在本文范围内，后续会分别处理。

关键词： 误差状态卡尔曼滤波；ESKF；扩展卡尔曼滤波；状态估计；旋转误差；名义状态

1. 引言：我们到底在估计什么

考虑一个正在运动的机器人。我们想知道它在某个时刻的状态，例如：

• 它位于什么位置；
• 它正在以多快的速度运动；
• 它的朝向是什么；
• 传感器的零偏是多少。

这些量不能直接从传感器读出。不同传感器给出的信息，在估计器中扮演的角色不同。陀螺仪给出角速度，加速度计给出比力，轮速计给出轮速或相对位移；相机给出像素，LiDAR 给出距离和点云的几何关系，卫星定位给出位置约束。

IMU、轮速计等传感器测量的是与运动直接相关的量。结合上一时刻的状态和运动模型，滤波器利用这些数据把状态向前推进，得到当前状态的先验估计。这个过程称为状态传播，也常被称为预测（prediction）。

视觉、LiDAR 等传感器读到的是运动与环境相互作用后留下的结果。它们通常通过像素、距离或几何关系给状态施加约束，无法直接给出机器人位置。这类传感器输出通常作为观测（measurement/observation）进入滤波器。

这里的“观测”并不意味着它不可预测。给定一个状态估计和传感器模型，我们同样可以计算传感器应该看到什么，例如写成 $\hat z_k=h(\hat x_k^-)$。实际传感器输出是 $z_k$，两者之差 $r_k=z_k-\hat z_k$ 才是用来修正状态的观测残差。

因此，“预测”和“观测”描述的是信息进入估计器时扮演的角色。“观测”同样可以有预测值。在本文的惯性状态估计语境中，IMU 和轮速计通常用于状态传播，视觉和 LiDAR 通常用于观测更新。传感器类别并不决定这种角色：轮速计也可以作为速度观测，视觉里程计也可以提供相对运动约束。关键在于它在模型中被放在什么位置。

问题可以表述为：

当已有估计和新观测都不完全可靠时，怎样在给定模型和噪声假设下持续估计状态？

Kalman Filter 处理的就是这个问题。ESKF 不改变估计目标，只改变状态误差的表示和处理方式。

2. 先从最简单的状态估计开始

2.1 预测和观测

先把机器人放在一边，只看一个随时间变化的未知量 $x$。在时刻 $k$，信息来自两处。

一类信息来自上一时刻的估计和运动模型，它给出预测：

$$x_k \sim \mathcal N(\hat{x}_k^-, P_k^-).$$

这里的上标 $-$ 表示“融合当前观测之前”，$\hat{x}_k^-$ 是预测值，$P_k^-$ 是预测的不确定性。

另一类信息来自传感器：

$$z_k = H_k x_k + v_k,$$

其中 $v_k$ 是测量噪声，满足

$$v_k \sim \mathcal N(0,R_k).$$

预测和观测都可能出错。预测越不确定，就越需要参考观测；传感器噪声越大，就越应保留预测。这是 Kalman Filter 的基本直觉。

2.2 观测更新是一次局部优化

把预测写成先验、把观测写成约束，代价函数就是：

$$\begin{aligned} E(x)= {}& (x-\hat{x}_k^-)^\top (P_k^-)^{-1}(x-\hat{x}_k^-) \\ &+(z_k-H_kx)^\top R_k^{-1}(z_k-H_kx). \end{aligned}$$

第一项惩罚新估计偏离预测，第二项惩罚新估计无法解释观测。两项的权重由协方差的逆决定：不确定性越小，约束越强。

这个目标与常见的最小二乘问题没有本质区别。Kalman Filter 把历史信息压缩成当前状态的均值和协方差，新的观测到来后再递推地完成融合。

在线性观测模型下，更新可以写成：

$$r_k=z_k-H_k\hat{x}_k^-,$$$$K_k=P_k^-H_k^\top (H_kP_k^-H_k^\top+R_k)^{-1},$$$$\delta\hat{x}_k=K_kr_k.$$

这里 $r_k$ 是观测减预测得到的残差，$K_k$ 把残差转换为状态修正量。需要抓住的是：

滤波器根据当前不确定性，用残差计算相应大小的状态修正；它不会把观测直接当作状态。

对于普通的欧氏向量，状态修正可以直接相加：

$$\hat{x}_k^+=\hat{x}_k^-+\delta\hat{x}_k.$$

问题也随之出现：状态包含旋转时，这个加法该如何定义？

3. 从 EKF 到 ESKF：为什么要估计误差

3.1 EKF 解决非线性问题

机器人运动和传感器观测通常是非线性的，可以写成：

$$x_{k+1}=f(x_k,u_k,w_k),$$$$z_k=h(x_k)+v_k,$$

其中 $u_k$ 是输入，$w_k$ 是过程噪声，$v_k$ 是观测噪声。

扩展卡尔曼滤波（Extended Kalman Filter，EKF）在当前估计附近对非线性模型做局部线性化：

$$\delta x_{k+1}\approx F_k\delta x_k+G_kw_k,$$$$r_k\approx H_k\delta x_k+v_k.$$

所以，EKF 仍然是在局部误差上做线性高斯估计。

3.2 普通 EKF 的加性误差

如果状态是普通向量，可以定义：

$$x=\hat{x}+\delta x.$$

此时，真实状态与当前估计之间的误差是一个可以直接相加的向量。

对于位置、速度和传感器零偏，这种表示通常没有问题。但姿态属于旋转空间，不能按普通三维向量处理。旋转矩阵必须满足正交性和行列式约束；四元数还必须保持单位范数。直接进行

$$R+\delta R$$

一般不会得到一个合法的旋转矩阵。

3.3 ESKF 的基本安排

ESKF 对不同状态采用不同的表示，并把它们分成两层：

1. 名义状态（nominal state）：保存当前对真实状态的完整估计，并按照非线性运动模型传播；
2. 误差状态（error state）：保存名义状态附近的小偏差，滤波器只对这一部分进行线性估计。

用抽象记号表示，真实状态 $\mathcal X$ 和名义状态 $\hat{\mathcal X}$ 的关系为：

$$\mathcal X=\hat{\mathcal X}\boxplus\delta x.$$

符号 $\boxplus$ 表示“把局部误差施加到状态上”。对于普通向量，它就是加法；对于旋转，它表示在旋转空间中进行局部扰动。

对应地，$\boxminus$ 表示从两个状态中计算局部误差：

$$\delta x=\mathcal X\boxminus\hat{\mathcal X}.$$

ESKF 使用适合每种状态的局部坐标，描述真实状态相对于名义状态的偏差。

3.4 一个直观类比

例如，我们估计某个物体位于 $100$ 米，真实位置可能是 $100.2$ 米。此时只估计 $0.2$ 米的局部误差，再修正原估计，就比重新描述整个位置更自然。

姿态也遵循这个思路。修正时要沿一个小旋转转动当前姿态，不能使用矩阵加法。

可以把 error-state 记成三句话：

• 名义状态负责描述“现在大概在哪里”；
• 误差状态负责描述“还差多少”；
• Kalman Filter 负责估计“还差多少”。

4. 一个典型的 ESKF 状态

考虑惯性导航中常见的一组状态：

$$\mathcal X=(R,p,v,b_g,b_a).$$

其中：

• $R$ 是姿态旋转；
• $p$ 是位置；
• $v$ 是速度；
• $b_g$ 是陀螺仪零偏；
• $b_a$ 是加速度计零偏。

相应的误差状态写成：

$$\delta x=(\delta\theta,\delta p,\delta v,\delta b_g,\delta b_a).$$

其中 $\delta\theta$ 是三维小旋转向量，其余部分是普通的三维加性误差。因此，误差状态共有 $15$ 个自由度：

$$3+3+3+3+3=15.$$

旋转矩阵有 $3\times3=9$ 个元素，但只有 $3$ 个自由度。ESKF 使用三维局部旋转误差，避免直接估计带约束的九维矩阵。

4.1 名义状态和误差状态的关系

先固定坐标约定。本文用 $R_{WB}$ 表示从机体坐标系 $B$ 到世界坐标系 $W$ 的旋转，因此

$$x_W=R_{WB}x_B, \qquad x_B=R_{WB}^{\top}x_W.$$

姿态属于旋转群 $SO(3)$，即满足正交性和单位行列式约束的三维旋转集合。对三维向量 $\phi$，定义

$$\operatorname{Exp}(\phi) =\exp([\phi]_{\times})\in SO(3),$$

其中 $\exp$ 是矩阵指数。它把一个小旋转向量映射为合法的旋转矩阵；在小角度附近，

$$\operatorname{Exp}(\phi) \approx I+[\phi]_{\times}.$$

本文选用一种常见的右侧旋转扰动约定：

$$R=\hat{R}\operatorname{Exp}([\delta\theta]_{\times}),$$

其中 $[\delta\theta]_{\times}$ 是由向量 $\delta\theta$ 构造的反对称矩阵，满足

$$[\delta\theta]_{\times}y =\delta\theta\times y.$$

在这个约定下，右乘的小旋转误差 $\delta\theta$ 在局部、也就是机体坐标系中表达。

位置、速度和零偏采用普通的加性误差：

$$\begin{aligned} p&=\hat{p}+\delta p,\\ v&=\hat{v}+\delta v,\\ b_g&=\hat{b}_g+\delta b_g,\\ b_a&=\hat{b}_a+\delta b_a. \end{aligned}$$

于是，$\boxplus$ 在这个状态上的具体含义就是：旋转使用小旋转复合，其他分量使用加法。

也可以采用左侧扰动，但误差所在坐标系会随之改变，误差动力学中的符号、旋转矩阵位置和雅可比形式也会变化。遇到两套不同推导时，应先核对误差定义和坐标约定。

5. ESKF 的完整闭环

每收到一批新数据，ESKF 都重复以下五步：

步骤	处理对象	核心问题
1. 名义状态传播	$\hat{\mathcal X}$	根据运动模型，状态现在应该走到哪里？
2. 误差协方差传播	$P$	经过这段运动，不确定性扩大了多少？
3. 观测残差计算	$r$	新观测和当前预测相差多少？
4. 误差状态更新	$\delta\hat{x}$	这个残差应该修正状态的哪些方向？
5. 误差注入与 reset	$\hat{\mathcal X},P$	如何把局部修正放回状态，并重新定义误差？

前四步与普通 EKF 的说明相近，第五步则是 ESKF 必须单独处理的环节。

5.1 名义状态传播

名义状态使用完整的非线性模型传播：

$$\hat{\mathcal X}_{k+1}^- =f(\hat{\mathcal X}_k^+,u_k,0).$$

过程噪声在名义状态传播中取均值 $0$。它的统计影响通过协方差传播体现；名义状态不直接加入这个随机量。

以惯性状态为例，陀螺仪读数推动姿态变化，加速度计读数经过姿态变换后推动速度和位置变化，零偏按自身模型传播。这里保留非线性运动方程，不把所有状态压成一个线性向量后统一相加。

名义状态传播和误差协方差传播回答的是不同问题：

• 名义状态传播描述估计值如何运动；
• 误差传播描述不确定性如何变化。

两者依赖同一套物理模型，但计算对象不同。

5.2 误差状态传播

在名义轨迹附近对真实运动模型做一阶展开，得到误差动力学：

$$\dot{\delta x}=F\delta x+Gw.$$

$F$ 描述已有误差如何随系统运动传播，$G$ 描述过程噪声如何进入误差状态。离散化以后，常写成：

$$\delta x_{k+1} \approx\Phi_k\delta x_k+w_{d,k},$$

其中 $\Phi_k$ 是离散状态转移矩阵，$w_{d,k}$ 是离散过程噪声。

协方差传播为：

$$P_{k+1}^- =\Phi_kP_k^+\Phi_k^\top+Q_{d,k}.$$

其中 $Q_{d,k}=\operatorname{Cov}(w_{d,k})$ 是离散过程噪声的协方差。这个量经过采样间隔和离散化得到，不能直接当作连续时间噪声强度 $Q_c$ 使用。

即使名义状态沿确定性的预测轨迹前进，不确定性仍会受到运动模型和传感器噪声的影响而扩大。

$F$ 和 $G$ 由模型推导得到，不能凭空指定为“滤波器矩阵”：

1. 写出真实状态的连续时间动力学；
2. 写出名义状态使用的动力学；
3. 用误差定义把真实状态和名义状态联系起来；
4. 对小误差和噪声做一阶近似；
5. 读出误差状态和噪声的系数。

推导 IMU ESKF 时，应沿这条路径逐项计算，不要直接背一个 $15\times15$ 的矩阵。

5.3 观测残差与误差模型

设传感器观测模型为：

$$z_k=h(\mathcal X_k)+v_k.$$

在预测名义状态处计算观测残差：

$$r_k=z_k-h(\hat{\mathcal X}_k^-).$$

由于真实状态可以写成 $\hat{\mathcal X}_k^-\boxplus\delta x_k$，在小误差条件下进行一阶展开：

$$h(\hat{\mathcal X}_k^-\boxplus\delta x_k) \approx h(\hat{\mathcal X}_k^-)+H_k\delta x_k.$$

因此，残差近似满足：

$$r_k\approx H_k\delta x_k+v_k.$$

因此，ESKF 可以使用线性 Kalman 更新。观测函数并没有被当作全局线性函数，而只在当前名义状态附近对局部误差做线性近似。

5.4 一个具体例子：位置观测

下面用一个位置观测说明 $H$ 的含义。假设传感器直接提供世界坐标系下的位置：

$$z_p=p+v_p,$$

其中 $v_p$ 是位置观测噪声。预测位置为 $\hat p^-$ 时，残差为

$$r_p=z_p-\hat p^-.$$

如果误差状态的排列顺序是

$$\delta x=(\delta\theta,\delta p,\delta v,\delta b_g,\delta b_a),$$

那么这个观测只直接约束位置误差，对应的观测矩阵具有如下结构：

$$H_p=\begin{bmatrix} 0 & I & 0 & 0 & 0 \end{bmatrix}.$$

这个观测不直接测量姿态、速度或零偏，但协方差中的相关性可以让它间接修正这些状态。

5.5 误差状态更新

由残差模型得到更新：

$$S_k=H_kP_k^-H_k^\top+R_k,$$$$K_k=P_k^-H_k^\top S_k^{-1},$$$$\delta\hat{x}_k=K_kr_k.$$

这里求出的是误差状态的估计，名义状态还要在后续注入步骤中更新。

协方差可以使用 Joseph 形式更新：

$$\widetilde P_k^+ =(I-K_kH_k)P_k^-(I-K_kH_k)^\top +K_kR_kK_k^\top.$$

Joseph 形式保留了两类来源：预测误差经过观测修正后的部分，以及观测噪声传入的部分。在有限精度计算中，它也更有利于保持协方差的对称性和半正定性。

5.6 误差注入

得到局部误差 $\delta\hat{x}_k$ 后，将它施加到名义状态：

$$\hat{\mathcal X}_k^+ =\hat{\mathcal X}_k^-\boxplus\delta\hat{x}_k.$$

对于本文采用的右侧旋转误差，姿态注入为：

$$\hat R_k^+ =\hat R_k^-\operatorname{Exp}([\delta\hat\theta_k]_{\times}).$$

位置、速度和零偏则分别进行加法修正：

$$\begin{aligned} \hat p_k^+&=\hat p_k^-+\delta\hat p_k,\\ \hat v_k^+&=\hat v_k^-+\delta\hat v_k,\\ \hat b_{g,k}^+&=\hat b_{g,k}^-+\delta\hat b_{g,k},\\ \hat b_{a,k}^+&=\hat b_{a,k}^-+\delta\hat b_{a,k}. \end{aligned}$$

5.7 为什么还需要 reset

注入后，名义状态已经移动，下一轮误差也必须改为相对于新名义状态定义。所谓“把误差归零”，准确地说是：

把误差状态后验分布的均值注入名义状态，再把新的局部误差坐标的均值定义为零。

这不会把真实误差强行设为零，也不会清除不确定性。在普通向量空间中，注入前的真实误差与更新量近似满足

$$\delta x_{\mathrm{post}} \approx\delta x_{\mathrm{old}}-\delta\hat{x}.$$

对于本文采用的右侧旋转扰动，新的旋转误差更准确地由群上的复合给出：

$$\delta\theta_{\mathrm{new}} =\operatorname{Log}\left( \operatorname{Exp}(-\delta\hat{\theta}) \operatorname{Exp}(\delta\theta_{\mathrm{old}}) \right),$$

其中 $\operatorname{Log}$ 是 $\operatorname{Exp}$ 在局部邻域内的逆映射。旋转误差的 reset 要在旋转群上进行。只有在小角度一阶近似下，才可以把它看成“误差减去更新量”。

设注入前、仍位于旧误差坐标中的后验协方差为 $\widetilde P^+$，新旧误差坐标的一阶变换由 $J_{\mathrm{reset}}$ 描述，则 reset 后的协方差为

$$P^+ =J_{\mathrm{reset}}\widetilde P^+ J_{\mathrm{reset}}^\top.$$

对于普通加性状态，坐标变化通常就是恒等变换；对于旋转误差，$J_{\mathrm{reset}}$ 与所选的左侧或右侧扰动、注入方式和一阶近似有关。因此，完整 ESKF 不能只写“更新误差，然后把误差设为零”，还必须说明 reset 后协方差位于哪个局部坐标，以及如何进行转换。

至此，一次 ESKF 循环才闭合：

$$\boxed{ \text{传播名义状态} \rightarrow \text{传播误差协方差} \rightarrow \text{计算残差} \rightarrow \text{更新误差} \rightarrow \text{注入并 reset} }$$

6. ESKF 与 EKF 的关系

ESKF 与 EKF 使用同一套局部线性高斯估计框架。ESKF 把 EKF 的局部线性估计应用在误差状态上。

对比项	直接状态 EKF	ESKF
滤波器估计的量	状态本身或其加性增量	名义状态附近的误差状态
状态传播	对完整状态使用非线性模型	名义状态使用非线性模型
误差传播	对状态误差做线性化	对误差状态做线性化
姿态处理	需要额外保证旋转合法性	用局部旋转误差和复合更新
更新结果	直接修改状态	先得到误差，再注入名义状态
更新后处理	纯欧氏加性状态通常没有独立 reset；四元数或流形状态仍可能需要 retraction	必须重新定义误差并处理协方差

ESKF 可以理解为误差状态形式的 EKF。它的关键区别在于状态误差如何定义、如何注入，以及如何在非欧氏状态上保持局部表示的一致性。

7. 为什么 ESKF 适合惯性状态估计

7.1 误差通常比完整状态更接近线性

姿态、位置和速度的绝对值可能很大，运动也可能很复杂；但只要名义轨迹足够接近真实轨迹，两者之间的偏差通常较小。对这个小误差做一阶近似，更符合滤波器的局部假设。

7.2 旋转更新保持在合法空间中

名义姿态通过旋转复合更新，避免矩阵加法破坏旋转结构。旋转的约束因此由状态表示本身保持。

7.3 非线性动力学和线性协方差传播被分开

名义状态保留非线性运动模型，协方差只描述附近小误差的不确定性。两者分开后，物理模型和线性高斯滤波都能保留下来。

7.4 误差状态的维度更自然

三维旋转只有三个局部自由度。使用三维旋转误差，可以避免直接估计旋转矩阵九个元素带来的参数冗余和约束处理问题。

8. ESKF 的公式依赖具体约定

一个常见误解是：所有论文和工程都应得到完全相同的 $F$、$G$、$H$ 和 reset 矩阵。实际形式取决于以下选择：

• 姿态旋转表示的是哪个坐标系到哪个坐标系；
• 旋转误差使用左侧扰动还是右侧扰动；
• 误差向量定义为“真实值减估计值”还是相反；
• 陀螺仪和加速度计噪声的连续时间模型如何定义；
• 观测残差采用“观测减预测”还是“预测减观测”；
• 名义状态在观测更新前还是更新后进行重置；
• 协方差使用哪一种离散化和更新形式。

阅读 ESKF 推导时，可以按下面的顺序核对：

1. 先确认坐标系和姿态方向；
2. 再确认名义状态和真实状态的关系；
3. 再确认误差的左右扰动定义；
4. 再确认残差符号；
5. 最后核对 $F$、$G$、$H$ 和 reset Jacobian。

只比较某个矩阵的正负号，通常不足以判断两套推导是否矛盾。它们可能只是采用了不同的误差坐标。

9. 本文建立的核心认识

9.1 ESKF 更新局部误差状态

真实状态是我们想估计但无法直接获得的量。名义状态是当前的完整估计，误差状态是围绕名义状态定义的局部坐标。

9.2 ESKF 的卡尔曼更新发生在误差空间

观测残差先被线性化为误差状态的函数：

$$r\approx H\delta x+v.$$

Kalman gain 计算出来的也是误差修正：

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

更新结果先表示为误差修正，随后通过注入步骤更新名义状态。

9.3 误差注入是 ESKF 的关键动作

误差被估计出来以后，需要通过 $\boxplus$ 施加到名义状态上。注入完成后，误差坐标被重新定义，协方差也要随坐标变化进行 reset。

9.4 ESKF 的核心闭环可以反复使用

无论观测来自 GPS、相机、激光雷达还是其他传感器，只要能够建立观测残差和误差状态之间的局部关系，就可以放入同一个 ESKF 框架中：

$$\text{运动模型提供预测} +\text{传感器提供约束} \rightarrow\text{估计误差} \rightarrow\text{修正名义状态}.$$

10. 讨论：本文暂时没有展开什么

本文只保留 ESKF 最内核的结构。下面这些内容需要在掌握本文闭环后分别展开：

1. IMU 测量模型：加速度计测量的是比力，重力和坐标变换会进入速度传播；
2. 噪声模型：白噪声、零偏、随机游走以及连续时间噪声到离散协方差的转换；
3. 误差动力学推导：从具体 IMU 方程逐项推导 $F$ 和 $G$；
4. 旋转误差的左右扰动：不同约定下的姿态误差、雅可比和 reset 公式；
5. 观测模型：位置、姿态、重投影和点到平面等不同观测如何形成 $H$；
6. Allan 方差：如何从真实 IMU 数据估计滤波器需要的噪声参数；
7. ESKF 与优化方法：它与预积分、因子图和滑动窗口优化的联系与区别。

这些内容都是把抽象闭环落实到具体系统时需要补齐的部分。先把“名义状态、误差状态、传播、更新、注入和 reset”这条主线弄清楚，再逐项补充细节。

11. 结论

理解误差状态卡尔曼滤波，需要看清它如何组织状态和误差，不需要先背很多 Kalman gain 公式：

• 名义状态按照完整的非线性模型传播；
• 误差状态描述真实状态相对名义状态的小偏差；
• 协方差描述这个小偏差的不确定性；
• 观测更新估计误差状态，再通过注入步骤修正名义状态；
• 误差通过适合状态空间的方式注入名义状态；
• 注入后重新定义误差，并同步处理协方差。

可以把它概括为：

ESKF 用非线性的名义状态承载运动，用局部线性的误差状态承载估计，用误差注入把两者重新连接起来。

这条主线也适用于后续的 IMU ESKF、VIO 和惯性导航推导。与其先背完整矩阵，不如从 IMU 测量模型出发，逐项推导误差状态的来源。

参考文献

1. Joan Solà, Quaternion kinematics for the error-state Kalman filter, arXiv:1711.02508, 2017. https://arxiv.org/abs/1711.02508
2. Joachim Hertzberg, René Wagner, Uwe Frese, and Lutz Schröder, Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds, Information Fusion, 2013. Preprint: https://arxiv.org/abs/1107.1119