1. 简明误差卡尔曼滤波器（ESKF）及其推导过程-Toy模板网

这篇具有很好参考价值的文章主要介绍了1. 简明误差卡尔曼滤波器（ESKF）及其推导过程。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1. 简明误差卡尔曼滤波器（ESKF）及其推导过程

简介

本文主要介绍一种特殊正交群 $\text{SO(3)}$ 上的ESKF(Error State Kalman Filter, 误差卡尔曼滤波器)（有时也叫做流形上的ESKF）推导过程。

ESKF基本过程及优点

在现代的大多数IMU系统中，人们往往使用误差状态卡尔曼滤波器（Error State Kalman Filter, ESKF），而非原始状态的卡尔曼滤波器。大部分基于滤波器的LIO或VIO实现，都使用ESKF作为状态估计方法。

相比于传统KF，ESKF的优点如下：

在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统的KF则需要使用四元数（ $4$ 维）或更高维的表达（旋转矩阵， $9$ 维）,要不就得采用带奇异性的表达方式（欧拉角）。
ESKF总是在原点附近，距离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题。
ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替。
误差状态的运动学也相比原状态变量要来的更小，因为我们可以把大量更新部分放到原状态变量中。

在ESKF中，我们通常把原状态变量称为名义状态变量（Norminal State），把ESKF中的状态变量称为误差状态变量（Error State）。

ESKF整体流程如下：

当IMU测量数据到达时，首先将其积分后，放入名义状态变量中。
由于这种做法没有考虑噪声，其结果自然会快速漂移。于是我们希望把误差部分作为误差变量，放入ESKF中。
ESKF内部会考虑各种噪声和零偏的影响，并且给出误差状态的一个 高斯分布 描述。同时ESKF本身作为一种卡尔曼滤波器，也具有预测过程和修正过程。其中 修正过程 需要依赖 IMU 以外的传感器观测。在修正后，ESKF可以给出 后验的误差高斯分布。
随后，我们将这部分误差放入名义状态变量中，并把ESKF置零，这样就完成了一次循环。

ESKF参数含义

1. 简明误差卡尔曼滤波器（ESKF）及其推导过程

连续时间上的 ESKF状态方程

我们设ESKF的真值状态为：
$x_t = [p_t, v_t,R_t,b_{at},b_{gt},g_t]^\text{T}$
这个状态随时间改变，可以记为 $x(t)_t$ 。

在连续时间上，我们记IMU读数为 $\tilde{\omega}, \tilde{a}$ ，那么可以写出状态变量导数相对于观测量之间的关系式：
$\dot{p_t} = v_t \tag{1 a}$

$\dot{v}_t = R_t(\tilde{a} - b_{at} -\eta_a) + g \tag {1 b}$

$\dot{R}_t = R_t(\tilde{\omega} - b_{gt} - \eta_g)^{\wedge} \tag{1 c}$

$\dot{b}_{gt} = \eta_{bg} \tag{1 d}$

$\dot{b}_{at} = \eta_{ba} \tag{1 e}$

$\dot{g} = 0 \tag{1 f}$

其中，带下标 $t$ 的表示真值。

这里把重力 $g$ 考虑进来的主要理由是方便确定IMU的初始姿态。

如果我们不在状态方程里写出重力变量，那么就必须事先确定初始时刻的IMU朝向 $R (0)$ ，才能执行后续的计算。此时IMU的姿态就是相对于初始的水平面来描述的。
而如果把重力写出来，就可以设IMU的初始姿态为单位矩阵 $R = I$ ，而把重力方向作为IMU当前姿态相对于水平面的一个度量。

两种方法都是可行的，不过将重力方向单独表达出来会使初始状态表达更为简单，同时还可以增加一些线性性。

如果把观测量 和 噪声量 整理成一个向量，我们也可以把上式整理成矩阵形式。不过这里的矩阵形式将含有很多的零项，相比于上式不会有明显的简化，所以这里采用散开的公式表示。

误差状态方程推导

下面我们推导误差状态方程。首先定义误差状态变量为：
$p_t = p + \delta p \tag{2 a}$

$v_t = v + \delta v \tag{2 b}$

$R_t = R\delta R \space \space 或 \space\space q_t = q \delta q \tag{2 c}$

$b_{gt} = b_g + \delta b_g \tag{2 d}$

$b_{at} = b_a + \delta b_a \tag{2 e}$

$g_t = g + \delta g \tag{2 f}$

其中，不带下标的就是名义状态变量。名义状态变量 的运动学方程与真值相同，只是 不需要考虑噪声（因为噪声在误差状态方程中考虑了） 。其中旋转部分的 $\delta R$ 可以用它的李代数 $\text{Exp}(\delta \theta)$ 来表示，此时旋转公式也需要改成用指数形式来表达。

关于误差变量的平移、零偏和重力公式，都很容易的而出对应的时间导数表达式，只需要在等式两侧分别对时间求导即可：
$\delta \dot{p} = \delta v \tag{3 a}$

$\delta \dot{b_g} = \eta_g \tag{3 b}$

$\delta \dot{b_a} = \eta_a \tag{3 c}$

$\delta g = 0 \tag{3 d}$

而速度、旋转两式由于和 $\delta R$ 有关系，所以需要单独推导。

误差状态的旋转项

对旋转式两侧求时间导数，可得：
$\begin{aligned} \dot{R}_t &= \dot{R}\text{Exp}(\delta \theta) + R \dot{\text{Exp}(\delta \theta)} \\ &=R_t(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } \end{aligned} \tag{4}$
该式右侧的 $\dot{\text{Exp}(\delta \theta)}$ 满足：
$\dot{\text{Exp}(\delta \theta)} = \text{Exp}(\delta \theta) \delta \dot{\theta}^{\wedge } \tag{5}$
因此，公式（4）中第一个式子可以写成：
$\dot{R}\text{Exp}(\delta \theta) + R \dot{\text{Exp}(\delta \theta)} = R(\tilde{\omega} - b_{g})^{\wedge} \text{Exp}(\delta \theta) + R\text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } \tag{6}$

而第二个式子可以写成：

$R_t(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } = R\text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } \tag{7}$

比较公式（6）、（7），将 $\delta \dot{\theta}$ 移到一侧，约掉两侧左边的 $R$ ，整理类似项，不难得到：

$\text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } = \text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge }- (\tilde{\omega} - b_{g})^{\wedge} \text{Exp}(\delta \theta) \tag{8}$
注意到 $\text{Exp}(\delta \theta)$ 本身是一个 $\text{SO(3)}$ 矩阵，我们利用 $\text{SO(3)}$ 上的伴随性质：
$\phi ^{\wedge} R = R (R^{\text{T}} \phi )^{\wedge } \tag{9}$

来交换公式（8）中的 $\text{Exp}(\delta \theta)$ ：
$\begin{aligned} \text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } &= \text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \text{Exp}(\delta \theta) \text{Exp}(-\delta \theta) (\tilde{\omega} - b_{g})^{\wedge} \\ &= \text{Exp}(\delta \theta) \left [ (\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \text{Exp}(-\delta \theta) (\tilde{\omega} - b_{g})^{\wedge} \right] \\ &\approx \text{Exp}(\delta \theta) \left [ (\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \left ((I-\delta \theta ^{\wedge}) (\tilde{\omega} - b_{g}) \right )^{\wedge} \right] \\ &= \text{Exp}(\delta \theta) \left [ b_{g} - b_{gt} - \eta_g + \delta \theta ^{\wedge}\tilde{\omega} - \delta \theta ^{\wedge}b_{g} \right] ^{\wedge } \\ &= \text{Exp}(\delta \theta) \left [ (-\tilde{\omega} + b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \right] ^{\wedge } \end{aligned} \tag{10}$

约掉公式（10）等式左侧的系数，可得：

$\delta\dot{\theta} \approx -(\tilde{\omega} - b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \tag{11}$

误差状态的速度项

接下来考虑速度方程的误差形式。同样地，对两侧求时间导数，就可以得到 $\delta \dot{v}$ 的表达式。等式左侧为：
$\begin{aligned} \dot{v}_t &= R_t(\tilde{a} - b_{at} -\eta_a) + g_t \\ &= R \text{Exp}(\delta \theta) (\tilde{a} - b_{a} - \delta b_a -\eta_a) + g + \delta g \\ &\approx R(I + \delta \theta ^{\wedge})(\tilde{a} - b_{a} - \delta b_a -\eta_a) + g + \delta g \\ &\approx R\tilde{a} - Rb_a - R\delta b_a - R\eta_a + R\delta \theta ^{\wedge} a - R\delta \theta ^{\wedge} b_a + g + \delta g \\ &= R\tilde{a} - Rb_a - R\delta b_a - R\eta_a - R\tilde{a}^{\wedge} \delta \theta + Rb_a^{\wedge} \delta \theta + g + \delta g \end{aligned} \tag{12}$
在公式（12）中，由第三行向第四行推导时，需要忽略 $\delta \theta ^{\wedge}$ 与 $\delta b_a$ ， $\eta_a$ 相乘的二阶小量。

从第四行推第五行则用到了叉乘符号交换顺序后，需要加负号的性质。

另一方面，等式右侧为：
$\dot{v} + \theta\dot{v} =R(\tilde{a} - b_a) + g + \delta \dot{v} \tag{13}$
因为上面两式相等，可以得到：
$\delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - R \eta_a + \delta g \tag{14}$
这样我们就得到了 $\delta v$ 的运动学模型。

需要补充一句，由于上式中 $\eta_a$ 是一个零均值白噪声，它乘上任意旋转矩阵之后，仍然是一个零均值白噪声，而且由于 $R^{\text{T}}R = I$ ，其协方差矩阵也不变。

所以，公式（14）可以简化为：
$\delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - \eta_a + \delta g \tag{15}$

完整误差变量的运动学方程

至此，我们可以把误差变量的运动学方程整理如下：
$\delta \dot{p} = \delta v \tag{16 a}$

$\delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - \eta_a + \delta g \tag{16 b}$

$\delta\dot{\theta} \approx -(\tilde{\omega} - b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \tag{16 c}$

$\delta \dot{b_g} = \eta_g \tag{16 d}$

$\delta \dot{b_a} = \eta_a \tag{16 e}$

$\delta g = 0 \tag{16 f}$

离散时间上的ESKF运动学方程

从连续时间状态方程，推出离散时间的状态方程并不困难，不妨直接来列写它们。

名义状态变量的离散时间运动学方程可以写为：
$\Delta t) = p(t) + v \Delta t + \frac{1}{2}(R(\tilde{a} - b_a)) \Delta t^2 + \frac{1}{2}g \Delta t^2 \tag{17 a}$

$\Delta t) = v(t) + R(\tilde{a} - b_a)) \Delta t + g \Delta t \tag{17 b}$

$\Delta t) = R(t)\text{Exp}((\tilde{\omega} - b_g) \Delta t) \tag{17 c}$

$b_g(t + \Delta t) = b_g(t) \tag{17 d}$

$b_a(t + \Delta t) = b_a(t) \tag{17 e}$

$\Delta t) = g(t) \tag{17 f}$

公式（17）只需要在上面基础上，添加零偏项和重力项即可。

而误差状态的离散形式，只需要处理连续形态中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为：
$\delta p(t + \Delta t) = \delta p + \delta v \Delta t \tag{18 a}$

$\delta v (t + \Delta t) = \delta v + (-R (\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a + \delta g) \Delta t + \eta_v \tag{18 b}$

$\delta \theta(t + \Delta t) = \text{Exp}(-(\tilde{\omega - b_a}) \Delta t) \delta \theta - \delta b_g \Delta t - \eta_\theta \tag{18 c}$

$\delta b_g (t + \Delta t) = \delta b_g + \eta_g \tag{18 d}$

$\delta b_a (t + \Delta t) = \delta b_a + \eta_a \tag{18 e}$

$\delta g (t + \Delta t) = \delta g \tag{18 f}$

公式（18）中，需要注意的是：

右侧部分省略了括号里的 $(t)$ 以简化公式；
关于旋转部分的积分，我们可以将连续形式看成关于 $\delta \theta$ 的微分方程，然后求解。求解过程类似于对角速度进行积分；
噪声项不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程中的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下：

$\sigma (\eta_v) = \Delta t \sigma_a , \space \space \sigma (\eta _\theta) = \Delta t \sigma _g, \space \space \sigma (\eta _g) = \sqrt{\Delta t} \eta _{bg}, \space \space \sigma (\eta _a) = \sqrt{\Delta t} \sigma _{ba} \tag{19}$

其中，前两式中的 $\Delta t$ 是由积分关系导致的。

至此，我们给出了如何在ESKF中进行IMU递推的过程，对应于卡尔曼滤波器中的状态方程。

为了让滤波器收敛，我们通常需要外部的观测来对卡尔曼滤波器进行修正，也就是所谓的组合导航。当然，组合导航的方法有很多，从传统的EKF，到本节介绍的ESKF，以及预积分和图优化技术，都可以应用于组合导航中。

ESKF的运动过程

根据上述讨论，我们可以写出ESKF的运动过程。

误差状态变量 $\delta x$ 的离散时间运动方程已经在上式给出，我们可以整体地记为：
$\delta x = f (\delta x) + w, \space w \sim \mathcal{N}(0, Q) \tag {20}$
其中，

$w$ 为噪声，

按照之前定义， $Q$ 应该为：
$\text{diag}\left ( 0_3, \text{Cov}(\eta _v), \text{Cov}(\eta_\theta), \text{Cov}(\eta_g), \text{Cov}(\eta_a), 0_3 \right) \tag{21}$
两侧的零是由于第一个和最后一个方程，本身没有噪声导致的。

为了保持与EKF的符号通一，我们计算运动方程的线性化形式：
$\delta x = \boldsymbol{F} \delta x + w \tag{22}$
其中， $\boldsymbol{F}$ 为线性化后的雅可比矩阵。

由于我们列写的运动方程以及是线性化的了，只需要把他么的线性系统拿出来即可：
$\boldsymbol{F} = \begin{bmatrix} I & I\Delta t & 0 & 0 & 0 & 0 \\ 0 & I & -R(\tilde{a} - b_a)^{\wedge} \Delta t & -R \Delta t & 0 & I \Delta t \\ 0 & 0 & \text{Exp}(-(\tilde{\omega} - b_g) \Delta t) & 0 & -I\Delta t & 0 \\ 0 & 0 & 0 & I & 0 & 0 \\ 0 & 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & 0 & I \end{bmatrix} \tag{23}$
在此基础上，我们执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU积分），以及对误差状态的预测：
$\delta x_{\text{pred}} = \boldsymbol{F} \delta x \tag{24 a}$

$\boldsymbol{P}_{\text{pred}} = \boldsymbol{FPF}^{\text{T}} + \boldsymbol{Q} \tag{24 b}$

不过，由于ESKF的误差状态，在每次更新以后都会被重置，因此运动方程的均值部分没有太大意义，而方差部分则可以指导整个误差估计的分布情况。

ESKF的更新过程

前面介绍的是ESKF的运动过程，现在我们来考虑更新过程。

假设一个抽象的传感器能够对状态变量产生观测，其观测方式为抽象的 $h$ ，那么可以写为：
$\space v \sim \mathcal{N}(0, \boldsymbol{V}) \tag {25}$
其中，

$z$ 是观测数据，

$v$ 是观测噪声，

$\boldsymbol{V}$ 为该噪声的协方差矩阵。由于变量里已经有 $R$ 了，这里我们换个符号。

在传统的EKF中，我们可以直接对观测方程线性化，求出预测方程相对于状态变量的雅可比矩阵 ，进而更新卡尔曼滤波器。

而在ESKF中，我们当前拥有名义状态 $x$ 以及误差状态 $\delta x$ 的估计，且希望更新的是误差状态，因此要计算观测方程，相比于误差状态的雅可比矩阵：
$\boldsymbol{H} = \frac{\partial h}{\partial \delta x} \tag{26}$
然后再计算卡尔曼增益，进而计算误差状态的更新过程：
$\boldsymbol{K} = \boldsymbol{P}_{\text{pred}} \boldsymbol{H}^{\text{T}} (\boldsymbol{H} \boldsymbol{P}_{\text{pred}} \boldsymbol{H}^{\text{T}} + \boldsymbol{V})^{-1} \tag{27 a}$

$\delta x = \boldsymbol{K}(z - h(x_t)) \tag{27 b}$

$\boldsymbol{P} = (\boldsymbol{I} - \boldsymbol{KH}) \boldsymbol{P}_{\text{pred}} \tag{27 c}$

其中，

$\boldsymbol{K}$ 为卡尔曼增益，

$\boldsymbol{P}_{\text{pred}}$ 为预测的协方差矩阵，

最后的 $\boldsymbol{P}$ 为修正后的 协方差矩阵。

这里 $\boldsymbol{H}$ 的计算可以通过链式法则来生成：
$\boldsymbol{H} = \frac{\partial h}{\partial x} \frac{\partial x}{\partial \delta x} \tag{28}$
其中，

第一项，只需要对观测方程进行线性化，

第二项，根据我们之前对状态变量的定义，可以得到：
$\frac{\partial x}{\partial \delta x} = \text{diag} \left ( \boldsymbol{I}_3, \boldsymbol{I}_3, \frac{\partial \text{Log}(R(\text{Exp}(\delta \theta ) )) }{\partial \delta \theta } , \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3 \right ) \tag {29}$
其他几种都是平凡的，只有旋转部分，因为 $\delta \theta$ 定义为 $R$ 得到右称，我们用右称的BCH即可：
$\frac{\partial \text{Log}(R(\text{Exp}(\delta \theta ) )) }{\partial \delta \theta } = \boldsymbol{J}_{r}^{-1}(\boldsymbol{R}) \tag{30}$
最后，我们可以给每个变量加下标 $k$ ，表示在 $k$ 时刻进行状态估计。

ESKF的误差状态后续处理

在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需要把误差状态 归入名义状态，然后重置ESKF即可。

归入部分，可以简单地写为：
$\boldsymbol{p}_{k+1} = \boldsymbol{p}_{k} + \delta \boldsymbol{p}_{k} \tag{30 a}$

$\boldsymbol{v}_{k+1} = \boldsymbol{v}_{k} + \delta \boldsymbol{v}_{k} \tag{30 b}$

$\boldsymbol{R}_{k+1} = \boldsymbol{R}_{k}\text{Exp}(\delta \boldsymbol{\theta}_{k}) \tag{30 c}$

$\boldsymbol{b}_{g,k+1} = \boldsymbol{b}_{g,k} + \delta \boldsymbol{b}_{g,k} \tag{30 d}$

$\boldsymbol{b}_{a,k+1} = \boldsymbol{b}_{a,k} + \delta \boldsymbol{b}_{a,k} \tag{30 e}$

$\boldsymbol{g}_{k+1} = \boldsymbol{g}_{k} + \delta \boldsymbol{g}_{k} \tag{30 f}$

有些文献里，也会定义为广义的状态变量加法：
$\boldsymbol{x}_{k+1} = \boldsymbol{x}_{k} \oplus \delta \boldsymbol{x}_{k} \tag{31}$
这种写法，可以简化整体的表达式。不过，如果公式里出现太多的广义加减法，可能让人不好马上辨别它们的具体含义，所以本文倾向于将各状态分别写开，或者直接用加法而非广义加法符号。

ESKF的重置分为均值部分和协方差部分。

均值部分可以简单地实现为：
$\delta \boldsymbol{x} = \boldsymbol{0} \tag {32}$
由于均值被重置了，之前我们描述的是关于 $\boldsymbol{x}_{k}$ 的切空间中的协方差，而现在描述的是 $\boldsymbol{x}_{k+1}$ 中的协方差。

这次重置会来带来一些微小的差异，主要影响旋转部分。事实上，在重置前，卡尔曼滤波器刻画了 $\boldsymbol{x}_{\text{pred}}$ 切空间处的一个高速分布 $\mathcal{N}(\delta \boldsymbol{x}, \boldsymbol{P})$ ，而重置之后，应该刻画 $\boldsymbol{x}_{\text{pred}} \boxplus \delta \boldsymbol{x}$ 处的一个 $\mathcal{N}(\delta \boldsymbol{x}, \boldsymbol{P}_{\text{reset}})$ 。

我们设重置前的名义旋转估计为 $\boldsymbol{R}_k$ ， 误差状态为 $\delta \boldsymbol{\theta}$ , 卡尔曼滤波器的增量计算结果为 $\delta \boldsymbol{\theta}_k$ ，注意此处的 $\delta \boldsymbol{\theta}_k$ 是已知的，而 $\delta \boldsymbol{\theta}$ 是一个随机变量。

重置之后的名义旋转部分为：
$\boldsymbol{R}^{+} \text{Exp}(\delta \boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta \boldsymbol{\theta}_k)\text{Exp}(\delta \boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta \boldsymbol{\theta}) \tag {33}$
不难得到：
$\text{Exp}(\delta \boldsymbol{\theta}^+) = \text{Exp}(-\delta \boldsymbol{\theta}_k) \text{Exp}(\delta \boldsymbol{\theta}) \tag {34}$
注意这里的 $\delta \boldsymbol{\theta}$ 为小量，利用线性化后的BCH公式，可以得到：
$\delta \boldsymbol{\theta}^{+} = -\delta \boldsymbol{\theta}_k + \delta \boldsymbol{\theta} - \frac{1}{2}\delta \boldsymbol{\theta}_k^{\wedge}\delta \boldsymbol{\theta} + o((\delta \boldsymbol{\theta})^2) \tag{35}$
于是有：
$\frac{\partial \delta \boldsymbol{\theta}^+}{\partial \delta \boldsymbol{\theta}} \approx \boldsymbol{I} - \frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge } \tag{36}$
公式（36）表明，重置前后的误差状态，相差一个旋转方面的小雅克比矩阵，我们记作：
$\boldsymbol{J_\theta} = \boldsymbol{I} - \frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge } \tag {37}$
把这个小雅可比矩阵 放到整个状态变量的维度下，并保持其他部分为单位矩阵，可以得到一个完整的雅可比矩阵：
$\boldsymbol{J_k} = \text{diag}( \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I_\theta}, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3 ) \tag{38}$
因此，把误差状态的均值归零的同时，它们的协方差矩阵也应该进行线性变换：
$\boldsymbol{P}_{\text{reset}} = \boldsymbol{J}_k\boldsymbol{P}\boldsymbol{J}_k^{\text{T}} \tag{39}$
不过，由于 $\delta \boldsymbol{\theta}_k$ 并不大，这里的 $\boldsymbol{J}_k$ 仍然十分接近于单位矩阵，所以大部分材料里并不处理这一项，而是直接把前面估计的 $\boldsymbol{P}$ 矩阵作为下一个时刻的起点。